# Car Game 0.1 - 21/2/19 - hrs: 8 at 10:30pm

import pygame
import random
import math
import os
pygame.init()
from pygame.locals import *
e = 2.718281828459045

screen_width = 800
screen_height = 600
screen = pygame.display.set_mode([screen_width, screen_height])
background = pygame.Surface(screen.get_size())

pygame.display.set_caption("Car Game 0.1")

#-------------------------------------------------------------------------------

# Define colours
BLACK = (  0,   0,   0)
GREY  = (150, 150, 150)
WHITE = (255, 255, 255)
RED   = (255,   0,   0)
GREEN = (  0, 255,   0)
BLUE  = (  0,   0, 255)

scale = 0.2
max_accel = 20
max_v = 500
max_angle = 8
drifting_on = False
#-------------------------------------------------------------------------------

def draw_line(x, y, w, l):
    pygame.draw.rect(screen, BLACK, [x, y, w, l])

def sigmoid1(x):
    return round(x/(1+abs(x)),2)
#print(sigmoid1(300))

def swap_boolean_state(var):
    if var == True:
        return False
    return True

def make_angle_in_range(angle):
    if angle > 3.1415:
        angle -= 2*3.1415
        make_angle_in_range(angle)
    if angle < -3.1415:
        angle += 2*3.1415
        make_angle_in_range(angle)
    return round(angle, 2)

def draw_fps_label():
    milliseconds = clock.tick(20)
    myfont = pygame.font.SysFont("monospace", 15)
    car_vn_label  = myfont.render("vn:  %.2f"  % car.vn, 5, WHITE)
    car_dd_label  = myfont.render("dd:  %.2f"  % car.angle, 5, WHITE)
    car_d_label  = myfont.render("d:   %.2f"  % car.dir, 5, WHITE)
    car_a_label  = myfont.render("a:   %.2f"  % car.a, 5, WHITE)
    car_v_label  = myfont.render("v:   %.2f"  % car.v, 5, WHITE)
    fps_label    = myfont.render("FPS: %d"  % int(clock.get_fps()), 5, WHITE)
    #pygame.draw.rect(screen, BLACK, [6, (screen_height - 22), 90, 13])
    screen.blit(car_vn_label, (6, (screen_height - 82)))
    screen.blit(car_dd_label, (6, (screen_height - 70)))
    screen.blit(car_d_label, (6, (screen_height - 58)))
    screen.blit(car_a_label, (6, (screen_height - 46)))
    screen.blit(car_v_label, (6, (screen_height - 34)))
    screen.blit(fps_label, (6, (screen_height - 22)))

def find_angle(run, rise, degrees=False):
    if run > 0 and rise >= 0:
        value = math.atan(rise/run)
    if run < 0 and rise >= 0:
        value = math.atan(rise/run) + math.pi
    if run < 0 and rise < 0:
        value = math.atan(rise/run) - math.pi
    if run > 0 and rise < 0:
        value = math.atan(rise/run)
    if run == 0 and rise > 0:
        value = math.pi / 2
    if run == 0 and rise < 0:
        value = - math.pi / 2
    if rise == 0 and run == 0:
        value = 0
    if degrees == False:
        return round(value, 2)
    return int(math.degrees(round(value, 2)))
#-------------------------------------------------------------------------------

class Mouse_Class(pygame.sprite.Sprite): 
    def __init__(self, colour, width, height):
        pygame.sprite.Sprite.__init__(self)
        all_sprites_list.add(self)
        self.image = pygame.Surface([width, height])
        self.rect = self.image.get_rect()
        self.tile_over = None
        self.name = "mouse square"
        
    def update(self):
        pos = pygame.mouse.get_pos()
        self.x = pos[0]
        self.y = pos[1]
        #self.mouse_over = pygame.sprite.spritecollide(self, tile_list, False)
        #for tile in self.mouse_over:
        #    self.tile_over = tile

class Car_Class(pygame.sprite.Sprite):
    def __init__(self, colour, co):
        pygame.sprite.Sprite.__init__(self)
        all_sprites_list.add(self)
        car_list.add(self)
        img_file = "data/car_game_data/car_%s.png" %(colour)
        print(img_file)
        self.imageMaster = pygame.image.load(img_file)
        w, h = self.imageMaster.get_size()
        self.imageMaster = pygame.transform.scale(self.imageMaster, (int(w*scale), int(h*scale)))
        self.width, self.height = self.imageMaster.get_size()
        self.image = self.imageMaster
        self.rect = self.image.get_rect()
        
        self.colour = colour
        self.x, self.y = co
        self.rect.x = self.x
        self.rect.y = self.y
        self.a = 0
        self.v = 400
        self.vn = 0
        self.angle = 0
        self.dir = 0
        
        self.s5 = Sensor_Class(self, RED, self.rect.center, 400, 0, 0, 0)
        #self.s4 = Sensor_Class(self, RED, self.rect.center, 400, -0.15, 0, 0)
        #self.s6 = Sensor_Class(self, RED, self.rect.center, 400, 0.15, 0, 0)
        self.s4b = Sensor_Class(self, RED, self.rect.center, 400, 0, -0.4, 22)
        self.s6b = Sensor_Class(self, RED, self.rect.center, 400, 0, 0.4, 22)
        #Sensor_Class(self, RED, self.rect.center, 400, -0.6)
        #Sensor_Class(self, RED, self.rect.center, 400, 0.6)
        #Sensor_Class(self, RED, self.rect.center, 400, -1.57)
        #Sensor_Class(self, RED, self.rect.center, 400, 1.57)
    
    def update(self):
        self.dir += self.angle / 60
        self.dir = make_angle_in_range(self.dir)
        
        self.rect = self.image.get_rect()
        oldCenter = self.rect.center
        self.image = pygame.transform.rotate(self.imageMaster, math.degrees(-self.dir))
        self.rect = self.image.get_rect()
        center_diff = oldCenter[0] - self.rect.center[0], oldCenter[1] - self.rect.center[1]
        self.x += center_diff[0]
        self.y += center_diff[1]
        
        if self.v < max_v and self.a > 0:
            self.v += self.a / 60
        if self.v > -100 and self.a < 0:
            self.v += self.a / 60
        self.vx = self.v * math.cos(self.dir)
        self.vy = self.v * math.sin(self.dir)
        self.x += self.vx / 60
        self.y += self.vy / 60
        
        if drifting_on == True:
            n = (self.v * abs(self.angle) - 333) / 1000
            self.v_old = self.v
            # #self.v = (1 - n) * self.v
            self.vn = n * self.v_old
            self.v = self.v_old
            """self.vx = n * self.v * math.cos(self.dir)
            self.vy = n * self.v * math.sin(self.dir)
            self.x -= self.vx / 60
            self.y -= self.vy / 60"""
            
            if self.vn < 0 and self.angle >=0:
                self.vn = 0
            # #if self.vn > 0 and self.angle > 0:
            # #    self.vn = 0
            if self.angle > 0:
                theta = self.dir - math.pi/2
            else:
                theta = self.dir + math.pi/2
            self.x += self.vn * math.cos(theta) / 60
            self.y += self.vn * math.sin(theta) / 60
        
        self.rect.x = self.x
        self.rect.y = self.y

def get_gradient(co1, co2):
    if (co2[0] - co1[0]) == 0:
        return round((co2[1] - co1[1]) / (0.01), 4)
    return round((co2[1] - co1[1]) / (co2[0] - co1[0]), 4)

def get_intersection(l1co1, l1co2, l2co1, l2co2):
    if l1co1[0] > l1co2[0]:
        temp = l1co1
        l1co1 = l1co2
        l1co2 = temp
    if l2co1[0] > l2co2[0]:
        temp = l2co1
        l2co1 = l2co2
        l2co2 = temp
    m1 = get_gradient(l1co1, l1co2)
    m2 = get_gradient(l2co1, l2co2)
    c1 = l1co1[1] - m1*l1co1[0]
    c2 = l2co1[1] - m2*l2co1[0]
    if (m1 - m2) == 0:
        x = (c2 - c1) / 0.01
    else:
        x = (c2 - c1) / (m1 - m2)
    if l1co1[0] <= x <= l1co2[0] and l2co1[0] <= x <= l2co2[0]:
        return (round(x,2), round(m1*x+c1,2))
    else:
        return False

def get_distance(co1, co2):
    return(round(math.sqrt((co2[0] - co1[0])**2 + (co2[1] - co1[1])**2),2))

class Sensor_Class(pygame.sprite.Sprite):
    def __init__(self, master, colour, co, length, angle_offset, dir, offset):
        pygame.sprite.Sprite.__init__(self)
        all_sprites_list.add(self)
        sensor_list.add(self)
        self.image = pygame.Surface([screen_width, screen_height], pygame.SRCALPHA)
        self.co2 = co[0]+length*math.cos(master.dir+angle_offset), co[1]+length*math.sin(master.dir+angle_offset)
        pygame.draw.line(self.image, colour, [co[0], co[1]], [self.co2[0], self.co2[1]], 1)
        self.rect = self.image.get_rect()
        
        self.master = master
        self.colour = colour
        self.length = length
        self.angle_offset = angle_offset
        self.rect.x = 0
        self.rect.y = 0
        self.dist = 400
        self.dir = dir
        self.offset = offset
        self.co1 = self.master.rect.center
    
    def update(self):
        self_co1x = self.master.rect.centerx + self.offset * math.cos(car.dir + self.dir)
        self_co1y = self.master.rect.centery + self.offset * math.sin(car.dir + self.dir)
        #print(self.master.rect.centerx,self.offset, math.cos(car.dir + self.dir), self_co1x, self_co1y)
        self.co1 = self_co1x, self_co1y
        self.image = pygame.Surface([screen_width, screen_height], pygame.SRCALPHA)
        self.dist = self.length
        best_p = False
        for line in boundary_list:
            p = get_intersection(self.co1, self.co2, line.co1, line.co2)
            if not p == False:
                dist = get_distance(self.co1, p)
                if dist < self.dist:
                    self.dist = dist
                    best_p = p
        self.co2 = self.co1[0]+self.length*math.cos(self.master.dir+self.angle_offset), self.co1[1]+self.length*math.sin(self.master.dir+self.angle_offset)
        if not best_p == False:
            #pygame.draw.line(self.image, self.colour, [self.co1[0], self.co1[1]], [self.co2[0], self.co2[1]], 1)
            pygame.draw.line(self.image, self.colour, [self.co1[0], self.co1[1]], [int(best_p[0]), int(best_p[1])], 1)
            pygame.draw.circle(self.image, BLUE, [int(best_p[0]), int(best_p[1])], 4)

class Boundary_Class(pygame.sprite.Sprite):
    def __init__(self, colour, co1, co2):
        pygame.sprite.Sprite.__init__(self)
        #all_sprites_list.add(self)
        boundary_list.add(self)
        self.image = pygame.Surface([screen_width, screen_height], pygame.SRCALPHA)
        pygame.draw.line(self.image, colour, [co1[0], co1[1]], [co2[0], co2[1]], 5)
        self.rect = self.image.get_rect()
        self.colour = colour
        self.co1 = co1
        self.co2 = co2
    
    def update(self, co2):
        self.co2 = co2
        self.image = pygame.Surface([screen_width, screen_height], pygame.SRCALPHA)
        pygame.draw.line(self.image, self.colour, [self.co1[0], self.co1[1]], [self.co2[0], self.co2[1]], 5)

def car_ai():
    if car.s4.dist > car.s6.dist:
        print("left")
        if car.angle > -max_angle:
            car.dir -= 0.08
    if car.s4.dist < car.s6.dist:
        print("right")
        if car.angle < max_angle:
            car.dir += 0.08

def car_ai2():
    if car.s4b.dist > car.s6b.dist:
        print("left")
        if car.angle > -max_angle:
            car.dir -= 0.08
    if car.s4b.dist < car.s6b.dist:
        print("right")
        if car.angle < max_angle:
            car.dir += 0.08

def save_map():
    with open("data/car_game_data/map.txt", "w") as file:
        for l in boundary_list:
            file.write("%s,%s,%s,%s\n" %(l.co1[0], l.co1[1], l.co2[0], l.co2[1]))
        file.close()

def load_map():
    with open("data/car_game_data/map.txt") as file:
        content = file.readlines()
        content = [lines.strip('\n') for lines in content]
        for line in content:
            co10, co11, co20, co21 = line.split(",")
            #print(co10, co11, co20, co21)
            Boundary_Class(BLACK, (int(co10),int(co11)), (int(co20),int(co21)))

#-------------------------------------------------------------------------------

all_sprites_list = pygame.sprite.Group()
car_list = pygame.sprite.Group()
sensor_list = pygame.sprite.Group()
boundary_list = pygame.sprite.Group()
#-------------------------------------------------------------------------------

mouse = Mouse_Class(BLACK, 1, 1)
car = Car_Class("red", (200, 50))
#b = Boundary_Class(BLACK, (300,300), (400,100))
#-------------------------------------------------------------------------------

screen.fill(GREY)
car_list.draw(screen)
boundary_list.draw(screen)
BUTTONDOWNPOS = (0,0)
ai_on = False
show_boundaries = True
done = False
playtime = 0
clock = pygame.time.Clock()
while not done:
    if ai_on == True:
        car_ai2()
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            done = True
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_ESCAPE:
                done = True
            if event.key == pygame.K_o:
                ai_on = swap_boolean_state(ai_on)
            if event.key == pygame.K_b:
                show_boundaries = swap_boolean_state(show_boundaries)
            if event.key == pygame.K_k:
                save_map()
            if event.key == pygame.K_l:
                load_map()
            if event.key == pygame.K_w:
                if car.a < max_accel:
                    car.a += 4
            if event.key == pygame.K_s:
                if car.a > -max_accel:
                    car.a -= 4
            if event.key == pygame.K_a:
                if car.angle > -max_angle:
                    car.angle -= 0.5
            if event.key == pygame.K_d:
                if car.angle < max_angle:
                    car.angle += 0.5
        if event.type == pygame.MOUSEBUTTONDOWN:
            BUTTONDOWNPOS = pygame.mouse.get_pos()
        if event.type == pygame.MOUSEBUTTONUP:
            if pygame.mouse.get_pos() == BUTTONDOWNPOS:
                if event.button == 1:
                    b = Boundary_Class(BLACK, (mouse.x,mouse.y), (mouse.x,mouse.y))
                if event.button == 3:
                    b.update((mouse.x, mouse.y))
    
    mouse.update()
    car_list.update()
    sensor_list.update()
    
    screen.fill(GREY)
    if show_boundaries == True:
        boundary_list.draw(screen)
    sensor_list.draw(screen)
    car_list.draw(screen)
    
    draw_fps_label()
    pygame.display.flip()
    
pygame.quit()
