import utils
import bge
import random
from mathutils import Vector
import mathutils
#import sound_man
#import math

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

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 single_marker(pos, col):
    pm = bge.logic.getCurrentScene().addObject('path_marker', None, 0)
    pm.worldPosition = pos
    pm.color = col

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 = 45
        dist = self.obj.getDistanceTo(x.obj)
        if dist < min_dist:
            potentials.remove(x)
    if len(potentials) > 0:
        new_parking = random.choice(potentials)
        path2 = self.manager.navmesh2.queue_path(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition, self)
        #print('astar request')
        #if path2 is not None:
            
        return new_parking, path2
    else:
        get_parking_type(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:
        pass

def update_point(self):
    if self.path_index >= (len(self.path) ):
        get_parking_type(self)
        #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, mult):
    v = self.obj.getVectTo(self.lane_point)[1]
    v.z = 0
    self.obj.alignAxisToVect(v, 0, .05 * mult)

def align_to_target(self, mult):
    v = self.obj.getVectTo(self.FSM.FSM.curState.park_loc)[1]
    v.z = 0
    self.obj.alignAxisToVect(v, 0, .05 * mult)


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 * 500
    if local < 0:
        f *= -1
    self.obj.applyForce([0, f, 0], True)
            
              
def apply_gas(self, mult):
    if self.obj.linearVelocity.x < self.speed_targ * mult:
        self.obj.applyForce([self.speed_inc * mult, 0, 0], True)

def starting_mod(self):
    path2 = self.path
    #path2.append(self.target.obj.worldPosition) #add endpoint
    #path2.append(self.target.obj.worldPosition) 

def check_front(self):    #
    obj_vec = self.obj.getAxisVect([1,0,0])
    targ_vec = self.obj.getVectTo(self.path[self.path_index] +1)[1].normalized()
    dot_ = obj_vec.dot(targ_vec)

    if dot_ > 0:
        print('in front', self)
    else:
        print('behind', self)

    if dot_ > 0:
        return True
    else:
        return False    

def get_parking_type(self):
    if self.target.type == 'perp':
        self.FSM.FSM.ToTransition('toEnterPerpPark')
    else:
        self.FSM.FSM.ToTransition('toEnterParallelPark')

def get_parking_point(self):
    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

    local = Vector([7,0,0])
    return Vector(self.target.obj.worldPosition + (self.target.obj.worldOrientation * local))


    #return Vector(self.target.obj.worldPosition) - 7 * tv

def move_to_point(self):
    #print(self.v_to_target[0])
    if self.v_to_target[0] < .95:
        #print('reached')
        return True
    else:
        vectTo = Vector(self.v_to_target[1])
        vectTo.magnitude = 4.01
        self.FSM.owner.obj.linearVelocity = vectTo
        return False

def forward_ray(self):

    Axis = 0
    Distance = 30
    End = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
    Start = self.obj.worldPosition.copy()
    return self.obj.rayCast(End, Start, 10,'traffic', 1, 0, 0)
    
#dist = v1[0]
#print(dist)    

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

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.path_index = 1
        
        try:
            self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
            get_lane_point(self.FSM.owner)
        except:
            print('no path')    
        self.FSM.owner.target.status = 'targetted'
        self.FSM.owner.start_empty.status = 'available'
        starting_mod(self.FSM.owner)
        super(ExitParallelPark, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        set_height(self.FSM.owner)
        if self.FSM.owner.path != []:
            update_point(self.FSM.owner)
            #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0])
            #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.lane_point)            
            #print(v[0])
            #self.FSM.owner.obj.alignAxisToVect(v[1], 0, .02)
            #self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1)
            #if self.FSM.stateLife > 160 or self.FSM.owner.obj.getDistanceTo(self.FSM.owner.lane_point) < 2:
            #if self.FSM.owner.obj.getDistanceTo(self.FSM.owner.path[0]) < 3.5:# or (self.FSM.stateLife > 30 and check_front(self.FSM.owner)):  
            if self.FSM.owner.path_index > 1:    
                #print('exiting park')  
                #self.FSM.owner.path_index = 2
                self.FSM.ToTransition('toNavigateToTarget')
            

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

            #if self.FSM.stateLife < 30:
                #self.FSM.owner.obj.applyRotation([0,0,.0075], True)  

    def Exit(self):
        pass        

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

class ExitPerpPark(State):
    def __init__(self,FSM):
        super(ExitPerpPark, 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.path_index = 1
        
        try:
            self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
            get_lane_point(self.FSM.owner)
        except:
            print('no path')    
        self.FSM.owner.target.status = 'targetted'
        self.FSM.owner.start_empty.status = 'available'
        starting_mod(self.FSM.owner)
        super(ExitPerpPark, self).Enter()        
        
    def Execute(self):
        #print('exiting perp park', self.FSM.owner.start_empty.type)
        self.FSM.stateLife += 1
        set_height(self.FSM.owner)
        if self.FSM.owner.path != []:
            update_point(self.FSM.owner)
            if self.FSM.owner.path_index > 1 or self.FSM.stateLife > 300:    
                self.FSM.ToTransition('toNavigateToTarget')
            

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

            if self.FSM.stateLife < 120:
                self.FSM.owner.obj.applyForce([-50,0,0], True)  

    def Exit(self):
        pass        

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






class EnterParallelPark(State):
    def __init__(self,FSM):        
        super(EnterParallelPark, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        
        clear_markers(self)
        super(EnterParallelPark, self).Enter() 

    def cleanup(self):
        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.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
        self.FSM.owner.last_lane_point = self.FSM.owner.target.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

        self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
        self.FSM.owner.obj.suspendDynamics()       
        
    def Execute(self):
        self.FSM.stateLife += 1
        if self.FSM.stateLife == 240:
            self.cleanup()    
            #self.FSM.owner.obj.suspendPhysics()
    def Exit(self):
        pass

class EnterPerpPark(State):
    def __init__(self,FSM):        
        super(EnterPerpPark, self).__init__(FSM)    

    def Enter(self):
        self.FSM.stateLife = 1
        self.park_loc = get_parking_point(self.FSM.owner)
        single_marker(self.park_loc, [1,0,0,.3])

        clear_markers(self)
        super(EnterPerpPark, self).Enter()     

    def cleanup(self):
        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([7, 0, 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.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
        self.FSM.owner.last_lane_point = self.FSM.owner.target.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

        self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
        self.FSM.owner.obj.suspendDynamics()       
        
    def Execute(self):
        self.FSM.stateLife += 1

        self.v_to_target = self.FSM.owner.obj.getVectTo(self.park_loc)

        goal_met = move_to_point(self)
        set_height(self.FSM.owner)
        if self.v_to_target[0] > 2:

            align_to_target(self.FSM.owner, .65)

        else:
            self.FSM.owner.obj.alignAxisToVect(-self.FSM.owner.target.obj.worldOrientation[0], 0, .05)

        if self.FSM.stateLife == 440 or goal_met:
            self.cleanup()    
            #self.FSM.owner.obj.suspendPhysics()

    def Exit(self):
        pass        

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

class NavigateToTarget(State):
    def __init__(self,FSM):
        super(NavigateToTarget, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        self.FSM.owner.obj.actuators['cs'].volume = .5
        #self.FSM.owner.obj.actuators['cs'].startSound()
        super(NavigateToTarget, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        self.forward_ray = forward_ray(self.FSM.owner)
        update_point(self.FSM.owner)

        align_to_point(self.FSM.owner, 1)
        align_to_road(self.FSM.owner)
        set_height(self.FSM.owner)
        delta_to_vect(self.FSM.owner)
        gas = True
        #print(self.FSM.owner.manager.parent.name)
        #self.FSM.owner.manager.parent['sndmgr'].queue_sound(['engine_idle', self.FSM.owner.obj, self.FSM.owner.manager.parent, 1, 1]) 
        #self.FSM.owner.manager.parent['sndmgr'].queue_sound(['engine_idle', self.FSM.owner.manager.parent, self.FSM.owner.obj,  .5, 1]) 
        #print(self.FSM.owner.obj.actuators['cs'])
        
        if self.forward_ray[0]:
            
            dist = self.FSM.owner.obj.getDistanceTo(self.forward_ray[1])
            if dist < 10:
                gas = False
                self.FSM.owner.obj.linearVelocity.x *= .98
            #print(dist, 'forward ray', self.forward_ray[0])
        
        if gas:
            apply_gas(self.FSM.owner, 1)

        pitch = self.FSM.owner.obj.linearVelocity.x
        pitch = pitch / self.FSM.owner.manager.default_speed
        pitch2 = abs(round(pitch * 1.5, 2))
        #if pitch2 > .75:
            #self.FSM.owner.obj.actuators['cs'].pitch  = 1.25
        #else:
            #self.FSM.owner.obj.actuators['cs'].pitch  = .75            
        self.FSM.owner.obj.actuators['cs'].pitch  = pitch2

        #print(pitch2, 'pitch') 

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

    def Exit(self):
        self.FSM.owner.obj.actuators['cs'].volume = .0
        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

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

    def Exit(self):
        pass

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

class RequestPath(State):
    def __init__(self,FSM):
        super(RequestPath, self).__init__(FSM)    
        
    def Enter(self):
        self.FSM.stateLife = 1
        self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner)
        self.FSM.owner.path_index = 0
        super(RequestPath, self).Enter()        
        
    def Execute(self):
        self.FSM.stateLife += 1
        
    def Exit(self):
        pass