from pygame.math import Vector2
import math
from helperFunction import HelperFunctions
import random
import pygame

class SteeringBehaviours(object):

    def __init__(self, agent):
        self.agent = agent
        self.panic_distance = 100.0

        self.circle_radius = 10
        self.wander_angle = 30
        self.angle_change = 0.5

    def calculate(self):
        pass

    def seek(self, target_pos):
        desired_velocity = Vector2.normalize(target_pos - self.agent.pos) * self.agent.max_speed
        return desired_velocity - self.agent.velocity

    def arrive(self, target_pos):
        desired = target_pos - self.agent.pos
        d = Vector2.length(desired)
        desired = Vector2.normalize(desired)
        if d < 100:
            m = HelperFunctions.map(d, 0, 100, 0, self.agent.max_speed)
            desired *= m
        else:
            desired *= self.agent.max_speed

        return desired - self.agent.velocity

    def flee(self, target_pos):
        panic_distance_sq = self.panic_distance * self.panic_distance

        if Vector2.distance_squared_to(self.agent.pos, target_pos) > panic_distance_sq:
            return Vector2(0, 0)

        desired_velocity = Vector2.normalize(self.agent.pos - target_pos) * self.agent.max_speed
        return desired_velocity - self.agent.velocity

    def pursuit(self, evader):
        to_evader = evader.pos - self.agent.pos

        relative_heading = self.agent.heading.dot(evader.heading)

        if to_evader.dot(self.agent.heading) > 0 and relative_heading < -0.95:
            return self.seek(evader.pos)

        look_ahead_time = Vector2.length(to_evader) / (self.agent.max_speed + Vector2.length(evader.vel))

        return self.seek(evader.pos + evader.vel * look_ahead_time)

    def wander(self):
        circle_center = self.agent.velocity
        circle_center = Vector2.normalize(circle_center)
        circle_center = circle_center * self.circle_radius

        displacement = Vector2(0, -1)
        displacement *= self.circle_radius

        displacement = self.set_angle(displacement, self.wander_angle)
        self.wander_angle += random.random() * self.angle_change - self.angle_change * 0.5
        wander_force = circle_center + displacement
        pygame.draw.circle(self.agent.game.screen, (255, 0, 0), (int(circle_center.x + self.agent.pos.x) , int(circle_center.y + self.agent.pos.y)), int(self.circle_radius))
        pygame.draw.circle(self.agent.game.screen, (255, 255, 0), (int(wander_force.x + self.agent.pos.x), int(wander_force.y + self.agent.pos.y)), 5)
        return wander_force

    def set_angle(self, v, value):
        len = Vector2.length(v)
        v.x = math.cos(value) * len
        v.y = math.sin(value) * len
        return v