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

class GyroscopeNotifier(AbstractMOOS):
    def default(self,  ci='unused'):
        cur_time=pymoos.MOOSCommClient.MOOSTime()
        # post the simulation time so that it can be synced to MOOSTime
        self.m.Notify('actual_time', blenderapi.persistantstorage().current_time, cur_time)
        
        precision = 0.1
        Syaw = "binf="+str(self.data['yaw']-precision)+",bsup="+str(self.data['yaw']+precision)
        Spitch = "binf="+str(self.data['pitch']-precision)+",bsup="+str(self.data['pitch']+precision)
        Sroll = "binf="+str(self.data['roll']-precision)+",bsup="+str(self.data['roll']+precision)
        #logger.info("Yaw : %s (rad)" % Syaw)
        #logger.info("Pitch : %s (rad)" % Spitch)
        #logger.info("Roll : %s (rad)" % Sroll)
        self.m.Notify("IMU_HEADING", Syaw, cur_time)
        self.m.Notify("IMU_PITCH", Spitch, cur_time)
        self.m.Notify("IMU_ROLL", Sroll, cur_time)