import logging; logger = logging.getLogger("morse." + __name__)
import pymoos.MOOSCommClient
from morse.middleware.moos import AbstractMOOS

class ScoutActuatorReader(AbstractMOOS):
    def initialize(self):
        AbstractMOOS.initialize(self)

        # register for control variables from the database
        self.m.Register("DESIRED_THRUST")
        self.m.Register("DESIRED_HRUDDER")
        self.m.Register("DESIRED_VRUDDER")

    def default(self, ci='unused'):
        current_time = pymoos.MOOSCommClient.MOOSTime()
        # get latest mail from the MOOS comm client
        messages = self.getRecentMail()#self.m.FetchRecentMail()

        # look for command messages: u1 and u2
        for message in messages:
            #logger.info("received %s " % message.GetKey())
            if (message.GetKey() == "DESIRED_THRUST") and (message.IsDouble()):
                #logger.info("VX = %s RECEIVED" % message.GetDouble())
                self.data['u1'] = message.GetDouble() # command linear acceleration
            elif  (message.GetKey()== "DESIRED_HRUDDER") and (message.IsDouble()):
                self.data['u2'] = message.GetDouble() # command angular rudder vertical
            elif  (message.GetKey()== "DESIRED_VRUDDER") and (message.IsDouble()):
                self.data['u3'] = message.GetDouble() # command angular rudder horizontal
