#!/usr/bin/env python
import math
import pygame
from pygame.locals import HWSURFACE, DOUBLEBUF
import random
from trigo import angle_to_vector, segments_intersection, distance

FLAGS=  HWSURFACE | DOUBLEBUF #| FULLSCREEN

GX = 800
GY = 800
CELL_COLOR = (80,80,80)
CAR_SIZE=50
pygame.init()
IMG = pygame.image.load("car50.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 = (GX / 2, GY / 2)
        self.speed = 5
        self.heading = 0
        self.heading_change = 0
        self.vision_length = 200 # line liength
        self.vision_span = 22 # 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.update_sensors()
        self.sensors = [self.left_sensor]#, self.center_sensor, self.right_sensor]

        self.reset_probes()


    def update_sensors(self):
        center = self.rect.center
        vc = angle_to_vector(self.heading)
        self.center_sensor = [center, [self.vision_length * vc[0] + center[0], -self.vision_length * vc[1] + center[1]]]
        
        vl = angle_to_vector(self.heading+self.vision_span)
        self.left_sensor = [center, [self.vision_length * vl[0] + center[0], -self.vision_length * vl[1] + center[1]]]
        
        vr = angle_to_vector(self.heading-self.vision_span)
        self.right_sensor = [center, [self.vision_length * vr[0] + center[0], -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 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()

        self.heading += self.heading_change
        self.heading = self.heading % 360
        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)


    def reset_probes(self):
        self.probes = [self.vision_length*2]#*3
    
    
    def probe_lines_proximity(self, lines):
        self.reset_probes()
        # print([x for x in zip(self.sensors, self.probes)])
        for l in lines :
            for i,s in enumerate(self.sensors) :
                    ip = segments_intersection(s,l)
                    print(s, l)
                    if ip is not None :
                        # print("ip", ip)
                        d = distance(ip, self.left_sensor[1])
                        # print('d',d)
                        # print('p was', p, 'is' , min(p,d))
                        self.probes[i] = min(self.probes[i],d)
                        # print()
                    else :
                        print('nothing found')
                        pass
            # print(f"D to line {l} :", self.probes)
        print(f"probes :", self.probes)
        # print()



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

all_cars = pygame.sprite.Group()
# car = Car()
# car.heading = 0
# all_cars.add(car)
car2 = Car()
car2.heading = 270
car2.heading_change = 3
car2.speed = 5
all_cars.add(car2)

ip = segments_intersection(car2.center_sensor, car2.left_sensor)
print(ip)
print(math.hypot(ip[0] - car2.rect.center[0], ip[1] - car2.rect.center[1]))

# stress test
# for x in range(100):
#     car = Car()
#     car.heading=x
#     car.heading_change = int(x)/30
#     car.speed = int(random.random()*6)
#     all_cars.add(car)

lines = [
    [
        (
            int(random.random()*GX),
            int(random.random()*GY)
        ),(
            int(random.random()*GX),
            int(random.random()*GY)
        )
    ]
for x in range(1)
]

print(lines)


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(lines)

    for line in lines :
        pygame.draw.line(screen, (255,255,255), line[0], line[1])
    pygame.display.flip()
    clock.tick(2)