import turtle
import random
import math

# --- Parameter (für Vibe-Coding ideal zum Drehen) ---
N = 25                 # Agentenanzahl
SPEED = 4.0            # Schrittweite pro Tick
JITTER_DEG = 8.0       # zufällige Drehung pro Tick (Grad)
NEIGHBOR_R = 70.0      # Nachbarschaftsradius
SEPARATION_R = 18.0    # zu nah -> abstoßen
ALIGN_W = 0.06         # Gewicht: Ausrichtung
COHESION_W = 0.002     # Gewicht: leichte Annäherung an Nachbarn
SEPARATION_W = 0.25    # Gewicht: Abstoßung
WALL_W = 0.015         # Gewicht: Rand-Abstoßung

WIDTH, HEIGHT = 900, 600
HALF_W, HALF_H = WIDTH / 2, HEIGHT / 2


def clamp_angle_rad(a):
    # auf [-pi, pi]
    while a > math.pi:
        a -= 2 * math.pi
    while a < -math.pi:
        a += 2 * math.pi
    return a


class Agent:
    def __init__(self):
        self.x = random.uniform(-HALF_W * 0.9, HALF_W * 0.9)
        self.y = random.uniform(-HALF_H * 0.9, HALF_H * 0.9)
        self.heading = random.uniform(-math.pi, math.pi)  # rad
        self.pen = turtle.Turtle(visible=False)
        self.pen.penup()
        self.pen.speed(0)
        self.pen.goto(self.x, self.y)
        self.pen.pendown()

    def step(self, agents):
        # Nachbarn sammeln
        nx = ny = 0.0
        n_heading_x = n_heading_y = 0.0
        sep_x = sep_y = 0.0
        count = 0

        for other in agents:
            if other is self:
                continue
            dx = other.x - self.x
            dy = other.y - self.y
            d2 = dx*dx + dy*dy
            if d2 < NEIGHBOR_R * NEIGHBOR_R:
                d = math.sqrt(d2) + 1e-9
                count += 1
                nx += other.x
                ny += other.y
                n_heading_x += math.cos(other.heading)
                n_heading_y += math.sin(other.heading)

                # Separation
                if d < SEPARATION_R:
                    # wegdrücken: stärker je näher
                    strength = (SEPARATION_R - d) / SEPARATION_R
                    sep_x -= (dx / d) * strength
                    sep_y -= (dy / d) * strength

        # aktuelle Bewegungsrichtung als Vektor
        vx = math.cos(self.heading)
        vy = math.sin(self.heading)

        # Regelkräfte
        ax = ay = 0.0

        if count > 0:
            # Cohesion: leicht zum Nachbarschaftszentrum
            cx = (nx / count) - self.x
            cy = (ny / count) - self.y
            ax += cx * COHESION_W
            ay += cy * COHESION_W

            # Alignment: Richtung an Nachbarn angleichen
            ahx = n_heading_x / count
            ahy = n_heading_y / count
            ax += (ahx - vx) * ALIGN_W
            ay += (ahy - vy) * ALIGN_W

            # Separation
            ax += sep_x * SEPARATION_W
            ay += sep_y * SEPARATION_W

        # Wall avoidance: sanft von Rändern weg
        ax += (-self.x) * WALL_W * max(0.0, (abs(self.x) - HALF_W * 0.85) / (HALF_W * 0.15))
        ay += (-self.y) * WALL_W * max(0.0, (abs(self.y) - HALF_H * 0.85) / (HALF_H * 0.15))

        # Jitter: kleine zufällige Drehung
        jitter = math.radians(random.uniform(-JITTER_DEG, JITTER_DEG))

        # neue Richtung aus (v + a) ableiten
        nvx = vx + ax
        nvy = vy + ay
        new_heading = math.atan2(nvy, nvx)
        self.heading = clamp_angle_rad(new_heading + jitter)

        # bewegen
        self.x += math.cos(self.heading) * SPEED
        self.y += math.sin(self.heading) * SPEED

        # zeichnen
        self.pen.goto(self.x, self.y)


def main():
    screen = turtle.Screen()
    screen.setup(WIDTH, HEIGHT)
    screen.tracer(0, 0)  # keine Einzel-Animation, wir updaten pro Tick

    agents = [Agent() for _ in range(N)]

    while True:
        for a in agents:
            a.step(agents)
        screen.update()


if __name__ == "__main__":
    main()