import utils
import bge
import random
from mathutils import Vector

#====================================     

def mark_path(path, y):
    iter_ = 0
    for x in path:
        pm = bge.logic.getCurrentScene().addObject('path_marker', y.obj, 0)
        pm.worldPosition = path[iter_]    
        iter_ += 1
        if iter_ == 1:
            pm.color = [0,1,0,.4]
        if iter_ == (len(path) ):
            pm.color = [1,0,0,.4]
        if iter_ == (len(path) +1):
            pm.color = [1,0,1,.4]    
        y.path_display.append(pm)

def clear_markers(self):
    for x in self.FSM.owner.path_display:
        try:
            x.endObject()
        except:
            pass
def get_ground_ray(self):
    Axis = 2
    Distance = -10
    end = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
    start = self.obj.worldPosition.copy()
    ground_ray = self.obj.rayCast(end, start, 6,'', 1, 0)
    return ground_ray

def set_height(self):
    ground_ray = get_ground_ray(self)
    target_height = 0.9
    hitpoint = ground_ray[1]
    try:
        dist = self.obj.getDistanceTo(hitpoint)
        if dist < target_height:
            self.obj.worldPosition.z += target_height - dist
            self.obj.linearVelocity.z = 0
        self.obj.linearVelocity.y *= .1
    except:
        pass

def align_to_road(self):
    ground_ray = get_ground_ray(self)
    try:
        self.obj.alignAxisToVect(ground_ray[2], 2, .15)
    except:
        pass
    
def find_new_parking(self):
    potentials = []
    for x in self.manager.parking_spots:
        if x.status == 'available':
            potentials.append(x)
    for x in potentials:
        min_dist = 100
        dist = self.obj.getDistanceTo(x.obj)
        if dist < min_dist:
            #potentials.remove(x)
            print('----removing potential')
    if len(potentials) > 0:

        new_parking = random.choice(potentials)
        path = self.manager.navmesh.findPath(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition)
        print('parking added at', new_parking.obj.worldPosition)

        mark_path(path, self)
        return new_parking, path
    else:
        print('cant find parking for', self)
        self.FSM.FSM.ToTransition('toEnterParallelPark')

def get_lane_point(self):
    self.point = self.path[self.path_index]
    if self.point != self.last_lane_point:
        v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0])
        tv = v.normalized()
        nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
        self.last_lane_point = self.lane_point
        self.lane_point = self.point + self.manager.lane_position * nv
    else:
        print('skipping lane point')

def update_point(self):
    print(self.path_index, 'path index', len(self.path))
    if self.path_index >= (len(self.path) ):
        self.FSM.FSM.ToTransition('toEnterParallelPark')
    else:
        dist = self.obj.getDistanceTo(self.lane_point)
        self.point = self.path[self.path_index]
        if dist < 2.5:
            get_lane_point(self)   
            if self.path_index > (len(self.path)):
                pass
            else:
                self.path_index += 1
         
def align_to_point(self):
    v = self.obj.getVectTo(self.lane_point)[1]
    v.z = 0
    self.obj.alignAxisToVect(v, 0, .1)

def delta_to_vect(self):
    v = self.obj.getVectTo(self.lane_point)[1]
    delta = self.last_lane_point - self.lane_point
    delta = delta.cross(v)
    delta_mult = -.1
    mult = 1.0
    deltamove = delta[2] * delta_mult

    #check if in front
    local = self.obj.worldOrientation.inverted() * (self.lane_point - self.obj.worldPosition) 
    f = deltamove * 50
    if local < 0:
        f *= -1
    self.obj.applyForce([0, f, 0], True)
            
              
def apply_gas(self):
    if self.obj.linearVelocity.x < self.speed_targ:
        self.obj.applyForce([self.speed_inc, 0, 0], True)

#====================================     

State = type("State", (object,), {})
#====================================     
class State(object):
    def __init__(self, FSM):
        self.FSM = FSM
        self.timer = 0
        self.startTime = 0
    def Enter(self):
        self.timer = 0
        self.startTime = 0
    def Execute(self):
        print('Executing')
    def Exit(self):
        print('Exiting')
#==================================== 
            
class Example(State):
    def __init__(self,FSM):
        super(Example, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        self.FSM.owner.resumePhysics()
        self.FSM.owner.resumeDynamics()
        print('physics resumed')
        super(Example, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        print('doing example')
        
    def Exit(self):
        pass

class ExitParallelPark(State):
    def __init__(self,FSM):
        super(ExitParallelPark, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        self.FSM.owner.obj.restorePhysics()
        self.FSM.owner.obj.restoreDynamics()
        self.FSM.owner.obj.linearVelocity = [0,0,0]
        self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner)
        self.FSM.owner.path_index = 0
        self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
        self.FSM.owner.target.status = 'targetted'
        self.FSM.owner.start_empty.status = 'available'
        
        print('physics resumed')
        super(ExitParallelPark, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0])
        self.FSM.owner.obj.alignAxisToVect(v[1], 0, .01)
        self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1)
        if self.FSM.stateLife > 220:
            self.FSM.ToTransition('toNavigateToTarget')

    def Exit(self):
        pass        

#====================================  
            
class EnterParallelPark(State):
    def __init__(self,FSM):        
        super(EnterParallelPark, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        print('entering parallel park')
        self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
        self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
        self.FSM.owner.obj.applyMovement([0, -6, 0], True)
        self.FSM.owner.target.status = 'in_use'
        self.FSM.owner.obj.worldPosition.z += .9
        self.FSM.owner.active = False
        self.FSM.owner.start_empty = self.FSM.owner.target
        self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
        self.FSM.owner.last_lane_point = self.FSM.owner.obj.worldPosition.copy()
        self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
        self.FSM.owner.path_index = 0
        self.FSM.owner.path = None
        print('clearning last path')
        
        clear_markers(self)
        
        self.FSM.owner.obj.suspendDynamics()
        self.FSM.owner.obj.suspendPhysics()
        super(EnterParallelPark, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        if self.FSM.stateLife == 2:
            self.FSM.owner.manager.cars_active.remove(self.FSM.owner)

    def Exit(self):
        pass

#==================================== 

class NavigateToTarget(State):
    def __init__(self,FSM):
        super(NavigateToTarget, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        
        super(NavigateToTarget, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        update_point(self.FSM.owner)

        align_to_point(self.FSM.owner)
        align_to_road(self.FSM.owner)
        set_height(self.FSM.owner)
        #delta_to_vect(self.FSM.owner)
        apply_gas(self.FSM.owner)

        #emergency exit
        if self.FSM.stateLife > 30 * 100:
            self.FSM.ToTransition('toEnterParallelPark')

    def Exit(self):
        pass        

#====================================  

class Activate(State):
    def __init__(self,FSM):
        super(Activate, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        super(Activate, self).Enter()        
        
    def find_target(self):
        pass

    def drive_to_point(self):
        pass
        # if self.FSM.path:
        #     ground_ray = get_ground_ray(self)
        #     if ground_ray[0]:
        #         set_height(self, ground_ray)
        #         align_to_road(self, ground_ray) 

        #     v = self.FSM.owner.getVectTo(self.lane_point)               
        #     speed_force = 800.0
        #     max_speed = 5.0
        #     behind = False
        #     local = self.FSM.owner.worldOrientation.inverted() * (self.lane_point - self.FSM.owner.worldPosition) 
        #     v2 = v[1].copy()
        #     v2.z = 0
        #     delta = self.last_lane_point - self.lane_point
        #     delta = delta.cross(v[1])
        #     delta_mult = -.1
        #     mult = 1.0
        #     backup_time = 20

        #     #change max speed
        #     if self.FSM.stateLife % 180 == 0:
        #         print('change speed force')
        #         self.speed_force = random.choice([max_speed * 1.3, max_speed * 1.6, max_speed * .8, max_speed * .6, max_speed])

        #     if local.x > 0 and self.backup == 0:

        #         if self.FSM.owner.linearVelocity.x < self.max_speed:
        #             self.FSM.owner.applyForce([speed_force, 0, 0], True)

        #         deltamove = delta[2] * delta_mult
        #         #self.FSM.owner.applyMovement([0, deltamove, 0], True)

        #         f = deltamove * 5000
        #         self.FSM.owner.applyForce([0, f, 0], True)
        #         v = self.FSM.owner.getVectTo(self.lane_point)
        #         v2 = v[1].copy()
        #         v2.z = 0
        #         self.FSM.owner.alignAxisToVect(v2, 0, .05)


        #     else:
        #         if local.x < 0:
        #             self.backup = backup_time   

        #         if self.backup > 0:
        #             print('backing up')
        #             v = self.FSM.owner.getVectTo(self.FSM.path[0])
        #             v2 = v[1].copy()
        #             v2.z = 0
        #             self.FSM.owner.alignAxisToVect(v2, 0, .02)
        #             if self.FSM.owner.linearVelocity.x > -max_speed / 2:
        #                 self.FSM.owner.applyForce([-speed_force * .8, 0, 0], True)
        #             self.backup -= 1

        #     dist = self.FSM.owner.getDistanceTo(self.lane_point)

        #     if dist < 2.5 and (len(self.FSM.path) > 0):    
        #         #print(self.FSM.path,'this is the path')
        #         #print('navmesh point removed')
        #         self.last_point = self.point
        #         self.last_lane_point = self.lane_point
        #         self.FSM.path.remove(self.FSM.path[0])

        #         if len(self.FSM.path) > 0:
        #             self.point = self.FSM.path[0]
        #             v = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0])
        #             tv = v.normalized()
        #             nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
        #             self.lane_point = self.point + self.lane_position * nv

        #         else:
        #             self.point = None

        #     if self.FSM.path == []:
        #         self.FSM.curTarget = None          

        #     #progress
        #     self.pos_his.append(self.FSM.owner.worldPosition.copy())
        #     pos_his_len = len(self.pos_his)
        #     if pos_his_len > 200:
                
        #         sum_ = abs(self.FSM.owner.worldPosition.x - self.pos_his[0][0]) + abs(self.FSM.owner.worldPosition.y - self.pos_his[0][1])
        #         #print(sum_, 'sum')
        #         if sum_ < .05:
        #             self.backup = backup_time
        #             print('progress stopped')
        #         del self.pos_his[0]
            

    def Execute(self):
        self.FSM.stateLife += 1

    def Exit(self):
        pass

#====================================