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

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

    # Speed
    _v = 0
    
    # u1 (acceleration)
    _u1 = 0
    # u2 (rudder)
    _u2 = 0

    _first_run = True

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

    def default_action(self):
        # Integrate the state - equations of the boat here

        if abs(self._u1)<0.2:
            self._u1 = 0.0
        if abs(self._u2)<0.3:
            self._u2 = 0.0

        dt = 1/self.frequency
        psi = self.position_3d.yaw
        dX = self._v*cos(psi)*dt
        dY = self._v*sin(psi)*dt
        dZ = 0
        
        #self._v = self._v + (self._u1 - 0.3*abs(self._v)*self._v)*dt
        self._v = self._u1
        dPsi = 0.01*self._v*self._u2*dt
        
        self.bge_object.applyMovement([dX, dY, dZ])
        self.bge_object.applyRotation([0, 0, dPsi])

        # Attach the camera to the movment of the boat
        if self._first_run == True:
            blenderapi.scene().objects['CameraFP'].setParent(self.bge_object, 0, 0)
            self._first_run = False

        pass
