#!/usr/bin/python3 -B
import numpy as np
import asyncio
import time, math
import moteus
import rospy
from std_msgs.msg import String
rospy.init_node('Pcan2moteus_for_ros', anonymous=True)

class CHAMPCONTROLLER: # a Class who let interact champ quadruped package with Ylo2, based on 12 moteus controllers
    def __init__(self):
        self.pcanBoard_initialized = False # bool variable will act as a condition for can ports initialization, or not ( if already initialized !)
        self.position=0.1  # for now, only position is usefull on Champ package
        self.velocity=0.01
        self.maximum_torque=1.5
        self.stop_position=math.nan
        self.watchdog_timeout=math.nan
        self.feedforward_torque=0.0
        self.query=True
        self.send_command_var_old = [math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan,math.nan]
        self.send_command_var_new = [0.0, 0.0, 0.0,   # FRONT LEFT
                                      0.0, 0.0, 0.0, # FRONT RIGHT
                                      0.0, 0.0, 0.0, # BACK LEFT
                                      0.0, 0.0, 0.0] # BACK RIGHT

    async def InitializePcanPorts(self): # initialize the 4 ports of the Pcan M2 Board, only once, until shutdown.
        while not self.pcanBoard_initialized:
          # We create one 'moteus.Controller' instance for each servo
          self.pcanBus1 = moteus.PythonCan(interface='pcan', channel='PCAN_PCIBUS1')
          self.C1 = moteus.Controller(id=1, transport=self.pcanBus1)
          self.C2 = moteus.Controller(id=2, transport=self.pcanBus1)
          self.C3 = moteus.Controller(id=3, transport=self.pcanBus1)
          print("  --- The 4 ports have been correctly initialized")
          self.pcanBoard_initialized = True # pass thru the initialization process, if active

    async def stop_commands(self):
        await self.pcanBus1.cycle([
            self.C3.make_stop(),
            self.C1.make_stop(),
            self.C2.make_stop(),
            ])
        await asyncio.sleep(0.02)
        print("        --- Motors are stopped, now\n")

    async def send_commands(self, jointTrajectory):
        await self.InitializePcanPorts()
        FL = await self.pcanBus1.cycle([
          self.C1.make_position(position=jointTrajectory[0], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
          self.C2.make_position(position=jointTrajectory[1], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
          self.C3.make_position(position=jointTrajectory[2], velocity=self.velocity, maximum_torque=self.maximum_torque, query=True),
        ])
        print("    --- Position orders are sent to motors")
        for result in FL:
          motor_id = {result.id}.pop() # extract the int id
          try :
            self.send_command_var_new[motor_id-1] = (f"{result.values[moteus.Register.POSITION]}")
          except :
            self.send_command_var_new[motor_id-1] = self.send_command_var_old[motor_id-1]
        self.send_command_var_old = self.send_command_var_new # actualize list, to help an empty id request, pick the previous good value
        print("    --- Position commands are queried :")
        print(self.send_command_var_new)

app = CHAMPCONTROLLER()

async def main(commands):
    print("Running Main() :")
    await asyncio.sleep(0.02)
    await app.send_commands(commands)
    await app.InitializePcanPorts()
    await app.stop_commands()

def callback(data):
    commands = (data.data)
    c = commands.split(',')
    x = np.array(c)
    y = x.astype(np.float)
    asyncio.run(main(y), debug=True)

def pcan2moteus_node():
  rospy.Subscriber("pcan2moteus", String, callback)
  rospy.spin()

if __name__ == '__main__':
    pcan2moteus_node()