#!/usr/bin/env python
import math
import pygame
import random
import numpy as np

from brain import Neural_Network
from maps import map1
from trigo import angle_to_vector, get_line_feats, segments_intersection, distance
from params import FLAGS, GX, GY, CELL_COLOR, CAR_SIZE, VISION_LENGTH, VISION_SPAN, THROTTLE_POWER



pygame.init()
IMG = pygame.image.load("car25.png")#.convert()





class Car(pygame.sprite.Sprite):
    def __init__(self):
        pygame.sprite.Sprite.__init__(self)
        self.top_surface = pygame.Surface((CAR_SIZE, CAR_SIZE))
        self.original_image = IMG
        # self.image = pygame.Surface((CAR_SIZE, CAR_SIZE))
        # self.image.fill((0,255,0))
        # self.original_image = self.image

        self.image = self.original_image

        self.rect = self.image.get_rect()
        self.rect.center = (75, GY -50)
        self.speed = 0.5
        self.heading = 0
        self.heading_change = 0
        self.vision_length = VISION_LENGTH # line liength
        self.vision_span = VISION_SPAN # degrees
        self.draw_sensors = True

        # lets add 3 sensors as a start 
        # 1 straight ahead
        # 2 left 15°
        # 3 right 15 °
        # we will give each of them a max lenght to 
        # and we will eventually detect any line crossing the sensor and 
        # retain the min value as a distance to collision input
        self.center_sensor = None
        self.left_sensor = None
        self.right_sensor = None
        self.sensors = [self.left_sensor, self.center_sensor, self.right_sensor]
        self.probes = [self.vision_length] *3 
        
        self.brain = Neural_Network()

        self.update_sensors()
        self.probe_brain()

    def update_sensors(self):
        center = self.rect.center
        vc = angle_to_vector(self.heading)
        self.center_sensor = [center, (int(self.vision_length * vc[0] + center[0]), int(-self.vision_length * vc[1] + center[1]))]
        
        vl = angle_to_vector(self.heading+self.vision_span)
        self.left_sensor = [center, (int(self.vision_length * vl[0] + center[0]), int(-self.vision_length * vl[1] + center[1]))]
        
        vr = angle_to_vector(self.heading-self.vision_span)
        self.right_sensor = [center, (int(self.vision_length * vr[0] + center[0]), int(-self.vision_length * vr[1] + center[1]))]


    def update_position(self):
        vec = angle_to_vector(self.heading)
        old_center = self.rect.center
        self.rect.center = (self.speed * vec[0] + old_center[0], -self.speed * vec[1] + old_center[1])
        self.update_sensors()


    
    def probe_lines_proximity(self, lines):
        # print(self.center_sensor, lines[0])
        self.probes = [self.vision_length*2] *3 
        for idx,sensor in enumerate([self.left_sensor, self.center_sensor, self.right_sensor]) :
            for line in lines :
                ip = segments_intersection(sensor, line)
                # print(ip)
                if ip :
                    pygame.draw.circle(screen, (125,125,255), ip, 4, 2)
                    dist = int(distance(ip,self.rect.center))
                    self.probes[idx] = min(dist, self.probes[idx])
                # else :
                #     self.probes[idx] = self.vision_length * 2 
        # print(self.probes)


    def probe_brain(self):
        res = self.brain.predict(np.array(self.probes))
        self.heading_change = res[0] * 10
        self.throttle = res[1] * 10 


    def update(self):
        # rotate
        old_center = self.rect.center
        self.image = pygame.transform.rotate(self.original_image, self.heading)
        self.rect = self.image.get_rect()
        self.rect.center = old_center
        self.update_position()
        print(
            'Speed', self.speed,
            'heading', self.heading,
            'throttle', self.throttle,
            'heading change', self.heading_change,
        )

        if self.speed : 
            self.heading += self.heading_change * 10 / self.speed
            self.heading = self.heading % 360

            self.speed += self.throttle #THROTTLE_POWER
            # if self.throttle :
                # self.speed += self.throttle #THROTTLE_POWER
            # else :
                # self.speed -= self.throttle #THROTTLE_POWER

            self.speed = max(0, self.speed)

        super().update()


    def show_features(self):
        if self.draw_sensors:
            pygame.draw.line(screen, (255,0,0), self.center_sensor[0], self.center_sensor[1])
            pygame.draw.line(screen, (0,255,0), self.left_sensor[0], self.left_sensor[1])
            pygame.draw.line(screen, (0,0,255), self.right_sensor[0], self.right_sensor[1])
            pygame.draw.circle(screen, (125,255,125), self.rect.center, 4, 2)







screen = pygame.display.set_mode((GX, GY), FLAGS)
screen.set_alpha(None)

map_lines = map1
# print(lines)



all_cars = pygame.sprite.Group()

for x in range(1):
    car = Car()
    car.heading = x * 30 + 35
    all_cars.add(car)

clock = pygame.time.Clock()
while True :
    screen.fill(CELL_COLOR)
    all_cars.update()
    all_cars.draw(screen)
    for c in all_cars :
        c.show_features()
        c.probe_lines_proximity(map_lines)
        c.probe_brain()

    for line in map_lines :
        pygame.draw.line(screen, (255,255,255), line[0], line[1])

    # point = (int(GX/2), int(GY/2+25))
    # print(distance(point, car2.rect.center))
    # pygame.draw.circle(screen, (125,255,125), point, 4, 2)

    pygame.display.flip()
    clock.tick(1)