import logging; logger = logging.getLogger("morse." + __name__)
import morse.core.robot
from math import cos, sin,tan, pi

alpha_v     = 0.0142857
beta_v      = 0.7
gamma_psi   = 0.0666667
delta_theta = 0.01
dzeta_phi   = 1
eta_phi     = 0.1

# alpha_v     = 0.285714
# beta_v      = 14
# gamma_psi   = 1
# delta_theta = 1
# dzeta_phi   = 1
# eta_phi     = 0.1

class Scout(morse.core.robot.Robot):
    _name = 'Scout robot'

    # speed
    _v = 0.0

    #commands
    _u1 = 0.0 # propulseur arriere (v) entre -1 et 1
    _u2 = 0.0 # gouverne horizontale (theta) idem
    _u3 = 0.0# gouverne verticale (psi) idem
    

    def __init__(self, obj, parent=None):
        logger.info('%s initialization' % obj.name)
        morse.core.robot.Robot.__init__(self, obj, parent)

    def default_action(self):
        # delta t
        dt = 1/self.frequency
        
        # Here you should make sure you integrate the state equations of the scout
        psi = -self.position_3d.yaw
        theta = -self.position_3d.pitch
        phi = self.position_3d.roll
        v = self._v
        u1 = self._u1
        u2 = self._u2
        u3 = self._u3
        
        dX = v*cos(psi)*cos(theta)*dt
        dY = v*sin(psi)*cos(theta)*dt
        dZ = - v*sin(theta)*dt
        
        self._v = v + (beta_v*u1 - alpha_v*abs(v)*v)*dt
        dPhi    = - (dzeta_phi * ( eta_phi * sin(phi) + tan(theta)*v*(sin(phi)*u2 + cos(phi)*u3 )) )*dt
        dTheta  = delta_theta*(cos(phi)*v*u2 - sin(phi)*v*u3)*dt
        dPsi    = gamma_psi*((sin(phi)/cos(theta))*v*u2 + (cos(phi)/cos(theta))*v*u3)*dt;        

        self.bge_object.applyMovement([dX, -dY, -dZ])
        self.bge_object.applyRotation([dPhi, -dTheta, -dPsi])
        pass
