# -*- coding: utf-8 -*-

# #####################################################################
# #####################################################################
# #####################################################################
# Class Mountutils_eqmod
# #####################################################################
# #####################################################################
# This class add utilities for EQMOD protocol
# #####################################################################

class Mountutils_eqmod():
    
    # ==self.= Constant for error codes
    NO_ERROR = 0

    # === Current status for the command :f
    
    # --- current motions
    MOTION_JOG_FAST_POSITIVE = 0
    MOTION_JOG_FAST_NEGATIVE = 1
    MOTION_JOG_SLOW_POSITIVE = 2
    MOTION_JOG_SLOW_NEGATIVE = 3
    MOTION_STOPPED_POSITIVE  = 4
    MOTION_STOPPED_NEGATIVE  = 5
    
    # --- current moving
    MOTION_STOPPED = 0
    MOTION_MOVING  = 1

    # --- current motor
    MOTOR_OFF = 0
    MOTOR_ON  = 1

    # === Current parameters for the command :G

    # --- motion types
    G_OFFSET_FAST     = 0
    G_CONTINUOUS_SLOW = 1
    G_OFFSET_SLOW     = 2
    G_CONTINUOUS_FAST = 3
    
    # --- last motion senses
    # NB: increasing means diurnal or toward north pole for north hemisphere.
    G_SENSE_POSITIVE = 0 # increasing
    G_SENSE_NEGATIVE = 1 # decreasing


    def hexa2int(self, hexa_str):
        a1 = hexa_str[0:2]
        a2 = hexa_str[2:4]
        a3 = hexa_str[4:6]
        hexa = a3+a2+a1
        dec = int(hexa,16)
        return dec

    def int2hexa(self, dec):
        dec = int(dec)
        hexa_str = "{:06X}".format(dec)
        a1 = hexa_str[0:2]
        a2 = hexa_str[2:4]
        a3 = hexa_str[4:6]
        hexa = a3+a2+a1
        return hexa

    def decode_G(self, lastG:str):
        """
        If we get ":G100", then input lastG="00"
        This function will return the last motion_type and motion_sense
        """
        digit =  int(lastG[0])
        motion_type = self.G_OFFSET_FAST
        if digit==0:
            motion_type = self.G_OFFSET_FAST
        if digit==1:
            motion_type = self.G_CONTINUOUS_SLOW
        if digit==2:
            motion_type = self.G_OFFSET_SLOW
        if digit==3:
            motion_type = self.G_CONTINUOUS_FAST
        digit =  int(lastG[1])
        if digit==0:
            motion_sense = self.G_SENSE_POSITIVE
        else:
            motion_sense = self.G_SENSE_NEGATIVE
        return motion_type, motion_sense            
    
    def encode_G(self, motion_type, motion_sense):
        """
        From the last motion type and the last motion sense
        this function will return the digits xy to add to the ":G?xy" command.
        e.g. The output lastG="00"
        """
        lastG = ""
        digit = 0
        if motion_type == self.G_OFFSET_FAST:
            digit = 0
        elif motion_type == self.G_CONTINUOUS_SLOW:
            digit = 1
        elif motion_type == self.G_OFFSET_SLOW:
            digit = 2
        elif motion_type == self.G_CONTINUOUS_FAST:
            digit = 3
        lastG += str(digit)        
        if motion_sense == self.G_SENSE_POSITIVE:
            digit = 0
        else:
            digit = 1
        lastG += str(digit)        
        return lastG
        
    def decode_f(self, f:str):
        """
        If we get "=101", then input f="101"
        This function will return the the current motion, moving and motor
        """
        f_x =  int(f[0])
        if f_x==0:
            current_motion   = self.MOTION_JOG_SLOW_POSITIVE
        elif f_x==1:
            current_motion   = self.MOTION_STOPPED_POSITIVE # or MOTION_JOG_SLOW_POSITIVE
        elif f_x==2:
            current_motion   = self.MOTION_JOG_SLOW_NEGATIVE
        elif f_x==3:
            current_motion   = self.MOTION_STOPPED_NEGATIVE # or MOTION_JOG_SLOW_POSITIVE
        elif f_x==4:
            current_motion   = self.MOTION_JOG_FAST_POSITIVE
        elif f_x==5:
            current_motion   = self.MOTION_JOG_FAST_POSITIVE
        elif f_x==6:
            current_motion   = self.MOTION_JOG_FAST_NEGATIVE
        elif f_x==7:
            current_motion   = self.MOTION_JOG_FAST_NEGATIVE
        f_y =  int(f[1])
        if f_y==0:
            current_moving  = self.MOTION_STOPPED
        else:
            current_moving  = self.MOTION_JOGGING
        f_z =  int(f[2])
        if f_z==0:
            current_motor  = self.MOTOR_OFF
        else:
            current_motor  = self.MOTOR_ON
        return current_motion, current_moving, current_motor
        
    def encode_f(self, motion_type:int, current_motion:int, current_moving:int, current_motor:int)->str:
        """
        From the last motion_type, current motion, moving, motor
        this function will return the answer of a ":f?" command.
        e.g. The output = "101"
        """
        #print(f"(0) motion_type={motion_type} current_motion={current_motion}")        
        f_x = 1
        if motion_type==self.G_OFFSET_FAST or motion_type==self.G_OFFSET_SLOW:
            if current_motion==self.MOTION_JOG_SLOW_POSITIVE:
                f_x = 0
            elif current_motion==self.MOTION_STOPPED_POSITIVE:
                f_x = 1
            elif current_motion==self.MOTION_JOG_SLOW_NEGATIVE:
                f_x = 2
            elif current_motion==self.MOTION_STOPPED_NEGATIVE:
                f_x = 3
            elif current_motion==self.MOTION_JOG_FAST_POSITIVE:
                f_x = 4
            elif current_motion==self.MOTION_JOG_FAST_NEGATIVE:
                f_x = 6
            #print(f"(1) f_x={f_x}")
        elif motion_type==self.G_CONTINUOUS_SLOW:
            if current_motion==self.MOTION_JOG_SLOW_POSITIVE or current_motion==self.MOTION_STOPPED_POSITIVE:
                f_x = 1
            elif current_motion==self.MOTION_JOG_SLOW_NEGATIVE or current_motion==self.MOTION_STOPPED_NEGATIVE:
                f_x = 3
        elif motion_type==self.G_CONTINUOUS_FAST:
            if current_motion==self.MOTION_STOPPED_POSITIVE:
                f_x = 1
            elif current_motion==self.MOTION_STOPPED_NEGATIVE:
                f_x = 3
            elif current_motion==self.MOTION_JOG_FAST_POSITIVE:
                f_x = 5
            elif current_motion==self.MOTION_JOG_FAST_NEGATIVE:
                f_x = 7
        # ---
        #print(f"(2) current_motion={current_motion}")        
        if current_moving==self.MOTION_STOPPED:
            f_y = 0
        else:
            f_y = 1 # MOTION_MOVING
        if current_motor==self.MOTOR_OFF:
            f_z = 0
        else:
            f_z = 1 # MOTOR_ON
        f = str(f_x) + str(f_y) + str(f_z)
        #print(f"(3) f_x={f_x} f_y={f_y} f={f}")
        return f