import serial
import time
import struct
import numpy as np

def swap_bytes_int(v):
    return struct.unpack('>i', struct.pack('<i', v))

def swap_bytes_short(v):
    return struct.unpack('>h', struct.pack('<h', v))

class BinaryFormat(object):
    def __init__(self, format, varnames):
        self.struct = struct.Struct(format)
        self.varnames = varnames
        
    def __call__(self, s, **keys):
        ans = self.struct.unpack(s)
        return dict(zip(self.varnames, ans))

    def size(self):
        return self.struct.size

sample_response = b'\xA9\x1D\x5C\x00\x00\x5E\x67\x04\x00\x00\x00\x00\x00\x1D\x19\x00\x00\x00\x60\x00\x80\x00\x00\x00\x00\x5E\x96\x0E\x00\x50\x99\x00\x00\x00\x00\x2D\x67\x04\x00\x84\xFA'         
sample_config = b'\xe8\x03\x00\x00\x00\x00\x00\x00\x002\x98:\xdc\x05\xa0\x0f\xff\x00F\x00\xf0Up\x00\xe8\x03\x00\x00\x00\x00\x00\x00\x002\x98:\xdc\x05\xa0\x0f\xff\x00F\x00\xf0U\n'
sample_response = b'\x00\x01\x00\x944\x00\x00\xc8\x00\x00\x00\x80\x96\x98\x00\x11\x94\x02\x0fX\x00\x00\x81\xa3\x80\x02\x0fX\x00\x00\x81\xa3\x80Bi5\x00@\x945\x00P\xc3\x00\x00P\xc3\x00\x00\xa0\x0f\x00\x00\xa0\x0f\x00\x00\n'
binary_response = BinaryFormat(b'<ciiiiccccHHIBBiicc',['controller_address', 'X', 'Y', 'Dec', 'RA', 'keypad_status', 'Xbits', 'Ybits', 'ExtraBits', 'AnalogInput1', 'AnalogInput2', 'milliclock', 'Temp', 'Phase', 'LastDec', 'LastRa', 'check1', 'check2'])
binary_config = BinaryFormat(
    b'<2i7h2c 2i7h2c cc 3i h 4i 6i 6c 4i 4c',
    ['XAcceleration', 'XBacklash', 'XErrorLimit', 'XPropGain', 'XIntGain', 'XDerGain', 'XOutputLim', 'XCurLim', 'XIntLimit', 'XBits', 'pad1',
     'YAcceleration', 'YBacklash', 'YErrorLimit', 'YPropGain', 'YIntGain', 'YDerGain', 'YOutputLim', 'YCurLim', 'YIntLimit', 'YBits', 'pad1', 
     'Address', 'pad2',
     'EquatorialRate', 'EquatorialUpDownAdjust', 'TrackingPlatformGoal',
     'Latitude', #MSB FIRST
     'XScopeEncoderTicksPerRev', 'YScopeEncoderTicksPerRev', 'XMotorEncoderTicksPerRev', 'YMotorEncoderTicksPerRev', #MSB FIRST
     'XSlewRate', 'YSlewRate', 'XPanRate', 'YPanRate', 'XGuideRate', 'YGuideRate',
     'PECAutoSync', 'pad3', 'BaudRate', 'pad4', 'ArgoMode', 'pad5',
     'LocalSearchCentiDegrees', 'LocalSearchSpeed', 'BacklashSpeed', 'YPecTicks',
     'pad6', 'pad7', 'cs1', 'cs2'])
binary_xxr = struct.Struct(b'<4i5c')
binary_yxr = struct.Struct(b'<iiii5c')

    
def command_factory(self, command, axis={}):
    form, doc, variables, answer = _ascii_commands[command]
    def f(*args):
        if len(args) != len(variables):
            raise ValueError(f'Command expect {len(variables)} positionnal arguments: {variables}')
        keys=dict(zip(variables, args))
        for v in variables:
            s = variables[v] 
            if s is None:
                pass
            if isinstance(s, set):
                if not keys[v] in variables[v]:
                    raise ValueError(f'{v} sould be in {s}')
            else:
                if keys[v] > s[1] or keys[v] < s[0]:
                    raise ValueError(f'{v} should lie in range {s}')
        keys.update(axis)
        #print((form.format(**keys)+'\r').encode())
        self.serial.write((form.format(**keys)+'\r').encode())
        if answer is not None:
            try:
                size = answer.size()
                ans=self.serial.read(size)
                return answer(ans, axis=axis)
            except AttributeError:
                ans = self.serial.readline()
                #ans = b'S56245\r\n'
                return answer(ans, axis=axis)
    f.__doc__ = doc.format(**axis)
    return f

def generic_parser(prefix='', type=int):
    def f(answer, axis={}):
        if not answer.startswith(prefix.format(**axis).encode()):
            raise ValueError(f'Answer not understood: {answer} should start with {prefix}')
        if not answer.endswith(b'\r\n'):
            raise ValueError(f'Answer incomplete: {answer}')
        return type(answer[len(prefix):-2])
    return f


def parse_config(ans, **keys):
    return ans
    
_ascii_commands = {
    'set_baud_rate': ['SB{rate}', 'Set Baud Rate (1 = 9600, 2 = 19200) Changes all controllers on serial bus. (1.5 or later)', {'rate': set([1,2])}, None],
    'goto_servo_position_{axis}': ['{axis}{position}S{speed}', 'Move Servo.', {'position': [-2147483648, 2147483647], 'speed': [0, 2147483647]}, None],
    'get_servo_position_{axis}': ['{axis}', 'Returns the {axis} position of the servo', {}, generic_parser(prefix='{axis}')],
    'set_servo_position_{axis}': ['{axis}F{position}', 'Forces the {axis} position to be equal to the number (This stops the controller if moving)', {'position': [-2147483648, 2147483647]}, None],
    'set_servo_speed_{axis}': ['{axis}S{speed}', 'Velocity of {axis} Servo (0-2147483647)', {'speed': [0, 2147483647]}, None],
    'get_servo_speed_{axis}': ['{axis}S', 'Returns the {axis} velocity of the servo (what it would be if it was currently moving at fully accelerated speed, this may not be what it acually is at the moment).', {}, generic_parser(prefix='S')],
    'set_ramp_{axis}': ['{axis}R{ramp}', 'Ramping speed or Acceleration of {axis} servo', {'ramp': [0-3900]}, None],
    'get_ramp_{axis}': ['{axis}R', 'Ramping speed or Acceleration of {axis} servo', {}, generic_parser(prefix='R')],
    'get_P_coef_{axis}': ['{axis}P', 'Return the Proportional coefficient of the PID control', {}, generic_parser(prefix='P')],
    'set_P_coef_{axis}': ['{axis}P{pid}', 'Set the Proportional coefficient of the PID control', {'pid': [0, 32767]}, None],
    'get_D_coef_{axis}': ['{axis}D', 'Return the derivative coefficient of the PID control', {}, generic_parser(prefix='D')],
    'set_D_coef_{axis}': ['{axis}D{pid}', 'Set the derivative coefficient of the PID control', {'pid': [0, 32767]}, None],
    'get_I_coef_{axis}': ['{axis}I', 'Return the integral coefficient of the PID control', {}, generic_parser(prefix='I')],
    'set_I_coef_{axis}': ['{axis}I{pid}', 'Set the integral coefficient of the PID control', {'pid': [0, 32767]}, None],
    'get_integral_limit_{axis}': ['{axis}L', 'Return the integral limit for the PID control', {}, generic_parser(prefix='L')],
    'set_integral_limit_{axis}': ['{axis}L{pid}', 'Set the integral limit for the PID control', {'pid': [0, 24000]}, None],
    'get_position_error_{axis}': ['{axis}E', 'Return the current position error of the servo', {}, generic_parser(prefix='E')],
    'set_position_error_limit_{axis}': ['{axis}E{error}', 'Set the maximum position error limit before the servo turns off', {'error': [0, 32767]}, None],
    'get_position_error_limit_{axis}': ['{axis}EL', 'Return the maximum position error limit', {}, generic_parser(prefix='E')],
    'get_PWM_{axis}': ['{axis}O', 'Return the current PWM output of the servo', {}, generic_parser(prefix='O')],
    'set_PWM_limit_{axis}': ['{axis}O{pwm}', 'set the maximum PWM output', {'pwm': [0,255]}, None],
    'get_current_{axis}': ['{axis}C', 'Return the motor current * 100 (amps)', {}, generic_parser(prefix='A')],
    'set_current_limit_{axis}': ['{axis}C{current}', 'Set the motor current limit * 100 (amps)', {'current': [0,240]}, None],
    'manual_mode_{axis}': ['{axis}M{pwm}', 'Change axis to Manual mode and set the PWM output to the specified value', {'pwm': [0, 255]}, None],
    'auto_mode_{axis}': ['{axis}A', 'Change axis to Auto mode', {}, None],
    'normal_stop_{axis}': ['{axis}N', 'Normal stop -- ramps down, then stops. Automatically clears when new position is given.', {}, None],
    'normal_stop_and_track_{axis}': ['{axis}NT', 'Normal stop, transitioning to Drag and Track or Slew and Track mode once the speed is slow enough', {}, None],
    'emergency_stop_{axis}': ['{axis}G', 'Emergency stop (stops immediately)', {}, None],
    'get_{axis}bits': ['{axis}B', 'Return the current XBits (or YBits) value', {},generic_parser(prefix='B')],
    'set_{axis}bits': ['{axis}B{bits}', 'Set the current XBits (or YBits) value', {'bits': None}, None],
    'get_scope_encoder_{axis}': ['{axis}Z', 'Return the current scope encoder position', {}, generic_parser(prefix='Z')],
    'set_scope_encoder_{axis}': ['{axis}Z{enc}', 'Return the current scope encoder position', {'enc': [ -2147483648, 2147483648]}, None],
    'get_handpaddle_status': ['XK', 'Return the handpaddle button status in decimal', {}, generic_parser(prefix='K')],
    'get_cpu_temperature': ['XH', 'Return the temperature of the CPU, in degrees F', {}, generic_parser(prefix='H')],
    'get_firmware_version': ['XV', 'Return the firmware version * 10 e.g. 31 = version 3.1', {}, generic_parser(prefix='V')],
    'get_supply_voltage': ['XJ', 'Return the motor power supply voltage * 10 e.g. 121 = 12.1 volts', {}, generic_parser(prefix='J')],
    'get_cpu_clock': ['XY', 'Return the current CPU clock, in milliseconds Range: -2,147,483,648 to 2,147,483,647', {}, generic_parser(prefix='Y')],
    'set_cpu_clock': ['XY{ms}', 'Set the current CPU clock, in milliseconds', {'ms': [0, 4294967295]}, None],
    'reset_servo_system': ['XQ', 'Reset/reboot the servo system (both axes)', {}, None],
    'reset_factory_default': ['XU', 'Program factory defaults into flash ROM', {}, None],
    'save_config': ['XW', 'Save the configuration currently in working RAM to the flash ROM (both axes)', {}, None],
    'load_config': ['XT', 'Read the configuration from the flash ROM into working RAM', {}, None],
    'send_config': ['FC', 'Flash Configure. 128 bytes should follow this command, plus 2 bytes of checksum (simple addition of the 128 bytes). This configuration will be written to Flash, and read into working RAM.', {}, None],
    'get_config': ['SC', 'Send Configuration. The controller sends 128 bytes, followed by two bytes of checksum (simple addition of the 128 bytes). This data represents the contents of flash ROM (but not necessarily the contents of working RAM). See “Send Configuration Layout” table for more info.', {}, binary_config],
    'get_serial_number': ['YV', 'Get Serial Number', {}, generic_parser(prefix='S')],
    'get_binary_status': ['XXS', 'Controller responds with 41 bytes (as of firmware 3.6C) of status info', {}, binary_response],
    'get_latitude': ['XXL', 'Get the latitude in centidegrees', {}, generic_parser(prefix='L')],
    'set_latitude': ['XXL{latitude}', 'Get the latitude in centidegrees', {'latitude':None}, None],
    'get_scope_encoder_ticks_per_rev_X': ['XXT','Get the X (Altitude) scope encoder ticks per revolution', {}, generic_parser(prefix='T')],
    'set_scope_encoder_ticks_per_rev_X': ['XXT{tpr}','Set the X (Altitude) scope encoder ticks per revolution', {'tpr':None}, None],
    'get_scope_encoder_ticks_per_rev_Y': ['XXZ','Get the Y (Az) scope encoder ticks per revolution', {}, generic_parser(prefix='Z')],
    'set_scope_encoder_ticks_per_rev_Y': ['XXZ{tpr}','Set the Y (Az) scope encoder ticks per revolution', {'tpr':None}, None],
    'get_motor_encoder_ticks_per_rev_X': ['XXU','Get the X (Altitude) motor encoder ticks per revolution', {}, generic_parser(prefix='U')],
    'set_motor_encoder_ticks_per_rev_X': ['XXU{tpr}','Set the X (Altitude) motor encoder ticks per revolution', {'tpr':None}, None],
    'get_motor_encoder_ticks_per_rev_Y': ['XXV','Get the Y (Az) motor encoder ticks per revolution', {}, generic_parser(prefix='V')],
    'set_motor_encoder_ticks_per_rev_Y': ['XXV{tpr}','Set the Y (Az) motor encoder ticks per revolution', {'tpr':None}, None],
    'get_slew_rate_X': ['XXA','Get the X (Altitude) slew rate', {}, generic_parser(prefix='A')],
    'set_slew_rate_X': ['XXA{rate}','Set the X (Altitude) slew rate', {'rate':None}, None],
    'get_slew_rate_Y': ['XXB','Get the Y (Az) slew rate', {}, generic_parser(prefix='B')],
    'set_slew_rate_Y': ['XXB{rate}','Set the Y (Az) slew rate', {'rate':None}, None],
    'get_pan_rate_X': ['XXC','Get the X (Altitude) pan rate', {}, generic_parser(prefix='C')],
    'set_pan_rate_X': ['XXC{rate}','Set the X (Altitude) pan rate', {'rate':None}, None],
    'get_pan_rate_Y': ['XXD','Get the Y (Az) pan rate', {}, generic_parser(prefix='D')],
    'set_pan_rate_Y': ['XXD{rate}','Set the Y (Az) pan rate', {'rate':None}, None],
    'get_platform_tracking_rate': ['XXE','Get the platform tracking rate', {}, generic_parser(prefix='E')],
    'set_platform_tracking_rate': ['XXE{rate}','Set the platform tracking rate', {'rate':None}, None],
    'get_platform_updown_adjuster': ['XXF','Get the platform up/down adjuster', {}, generic_parser(prefix='F')],
    'set_platform_updown_adjuster': ['XXF{rate}','Set the platform up/down adjuster', {'rate':None}, None],
    'get_platform_goal': ['XXG','Get the platform up/down adjuster', {}, generic_parser(prefix='F')],
    'set_platform_goal': ['XXG{rate}','Set the platform up/down adjuster', {'rate':None}, None],
}
for prop, letter, doc in [('scope_encoder_ticks_per_rev_X', 'T', 'X (altitude) scope encoder ticks per revolution'),
                          ('scope_encoder_ticks_per_rev_Y', 'Z', 'Y (Azimuth) scope encoder ticks per revolution'),
                          ('motor_encoder_ticks_per_rev_X', 'U', 'X (altitude) motor encoder ticks per revolution'),
                          ('motor_encoder_ticks_per_rev_Y', 'V', 'Y (altitude) motor encoder ticks per revolution'),
                          ('slew_rate_X', 'A', 'X slew rate'),
                          ('slew_rate_Y', 'B', 'Y slew rate'),
                          ('pan_rate_X', 'C', 'X pan rate'),
                          ('pan_rate_Y', 'D', 'Y pan rate'),
                          ('plateform_tracking_rate', 'E', 'platform tracking rate'),
                          ('updown_adjuster', 'F', 'platform up/down adjuster'),
                          ('platform_goal', 'G', 'platform goal'),
                          ('guide_rate_X', 'H', 'X (altitude) guide rate'),
                          ('guide_rate_Y', 'I', 'Y (azimuth) guide rate'),
                          ('pic_servo_timeout', 'J', 'PicServo timeout value, in seconds. More recently, sets whether RA PEC Auto Sync is enabled (value = 1) or disabled (value = 0)'),
                          ('receiver_digital_output', 'Q', 'digital outputs on the radio handpad receiver'),
                          ('argo_navis_mode', 'N', 'Argo Navis mode'),
                          ('local_search_distance_X', 'K', 'X (Altitude) local search distance'),
                          ('local_search_distance_Y', 'M', 'Y (Azimuth) local search distance'),
                          ('backlash_X', 'O', 'X (Altitude) backlash'),
                          ('backlash_Y', 'P', 'Y (Azimuth) backlash'),]:
    
    _ascii_commands[f'set_{prop}'] = [f'XX{letter}', f'Get the {doc}', {}, generic_parser(prefix=letter)]
    _ascii_commands[f'get_{prop}'] = [f'XX{letter}', f'Set the {doc}', {'prop':None}, generic_parser(prefix=letter)]
    
class ServoII(object):
    def __init__(self, port='/dev/ttyUSB0'):
        self.serial = serial.Serial(port=port, baudrate=19200, bytesize=8, parity='N', stopbits=1, timeout=1, xonxoff=False, rtscts=False, write_timeout=None, dsrdtr=False, inter_byte_timeout=None, exclusive=None)
        self._register_commands()
        self.ticksPerRev = np.array([5771010, -2136768256])
        
    def DegsPerSec2MotorSpeed(self, dps):
        return (self.ticksPerRev * dps * 0.09321272116971)

    def MotorSpeed2DegsPerSec(self, speed):
        return speed / self.ticksPerRev * 10.7281494140625

    def _register_commands(self):
        for command in _ascii_commands:
            if '{axis}' in command:
                for axis in 'XY':
                    setattr(self, command.format(axis=axis), command_factory(self, command, axis={'axis': axis}))
            else:
                setattr(self, command, command_factory(self, command))
    def status(self):
        return {}
    
if __name__=='__main__':
    s = ServoII()