"""
Configure PDO for autochanger controllers
=========================================

Trucks Xminus => index : 0x2A
Trucks Xplus  => index : 0x2C
Clamp Xplus   => index : 0x30
Clamp Xminus  => index : 0x31
Clamp Yminus  => index : 0x32

= PDO1 =
status word (16 bits)
average current (16 bits)
position (32 bits)
= PDO2 =
status word (16 bits)
following error (16 bits)
average velocity (32 bits)
= PDO3 =
DEACTIVATED
= PDO4 =
DEACTIVATED

---

Latch Xplus   => index : 0xE
Latch Xminus  => index : 0xF

= PDO1 =
DEACTIVATED
= PDO2 =
DEACTIVATED
= PDO3 =
DEACTIVATED
= PDO4 =
DEACTIVATED

"""
from org.lsst.ccs.scripting import CCS
from java.lang import Long


CONTROLLERS = {
    0x2a: "acTruckXminusController",
    0x2c: "acTruckXplusController",
    0x30: "onlineClampXplusController",
    0x31: "onlineClampXminusController",
    0x32: "onlineClampYminusController",
    0xe: "latchXplusController",
    0xf: "latchXminusController",
}

# Masks
STATUS_WORD = 0x60410010
CURRENT = 0x60780010
AVG_CURRENT = 0x20270010
POSITION = 0x60640020
FOLLOWING_ERROR = 0x20F40010
AVG_VELOCITY = 0x20280020
ACTIVATE = 0x40000000
DEACTIVATE = 0xC0000000


class ControllerPDOSetup(object):
    def __init__(self, main_subsystem):
        self.canbus_path = "%s/canbus0" % main_subsystem
        self.canbus = CCS.attachSubsystem(self.canbus_path)

    def get_controller(self, controller_name):
        return CCS.attachSubsystem("%s/%s" % (self.canbus_path, controller_name))
    
    def set_operational(self, node_id):
        self.canbus.sendSynchCommand("setNMTStateOperational", node_id)
    
    def set_preoperational(self, node_id):
        self.canbus.sendSynchCommand("setNMTStatePreOperational", node_id)

    def save_parameters(self, node_id):
        self.get_controller(CONTROLLERS[node_id]).sendSynchCommand("saveParameters")

    def write_sdo(self, node_id, index, subindex, size, value):
        self.canbus.sendSynchCommand("writeSDO", node_id, index, subindex, size, value)
    
    def activate_pdo(self, node_id, pdo_id):
        cob_id = 0x180 + pdo_id * 0x100
        self.write_sdo(node_id, 0x1800 + pdo_id, 1, 4, Long(ACTIVATE + node_id + cob_id))
        self.write_sdo(node_id, 0x1800 + pdo_id, 2, 1, 1)

    def deactivate_pdo(self, node_id, pdo_id):
        cob_id = 0x180 + pdo_id * 0x100
        self.write_sdo(node_id, 0x1800 + pdo_id, 1, 4, Long(DEACTIVATE + node_id + cob_id))

    def run_setup_for_controllers_with_encoder(self, node_id):
        print("Working on node %s => %s" % (node_id, CONTROLLERS[node_id]))

        self.set_preoperational(node_id)

        # Activate PDO 1 and 2
        self.activate_pdo(node_id, 0)
        self.activate_pdo(node_id, 1)
        # Deactivate PDO 3 and 4
        self.deactivate_pdo(node_id, 2)
        self.deactivate_pdo(node_id, 3)

        print("Configure PDO1")
        self.write_sdo(node_id, 0x1A00, 0, 1, 0)
        self.write_sdo(node_id, 0x1A00, 1, 4, STATUS_WORD)
        self.write_sdo(node_id, 0x1A00, 2, 4, AVG_CURRENT)
        self.write_sdo(node_id, 0x1A00, 3, 4, POSITION)
        self.write_sdo(node_id, 0x1A00, 0, 1, 3)
        
        print("Configure PDO2")
        self.write_sdo(node_id, 0x1A01, 0, 1, 0)
        self.write_sdo(node_id, 0x1A01, 1, 4, CURRENT)
        self.write_sdo(node_id, 0x1A01, 2, 4, FOLLOWING_ERROR)
        self.write_sdo(node_id, 0x1A01, 3, 4, AVG_VELOCITY)
        self.write_sdo(node_id, 0x1A01, 0, 1, 3)

        self.save_parameters(node_id)

        self.set_operational(node_id)

    def run_setup_for_latches(self, node_id):
        print("Working on node %s => %s" % (node_id, CONTROLLERS[node_id]))

        self.set_preoperational(node_id)

        # Deactivate all 4 PDOs
        print("Deactivate PDOs")
        self.deactivate_pdo(node_id, 0)
        self.deactivate_pdo(node_id, 1)
        self.deactivate_pdo(node_id, 2)
        self.deactivate_pdo(node_id, 3)

        self.save_parameters(node_id)

        self.set_operational(node_id)

    def run(self):
        """Execute the sequence"""
        print(__doc__)
        for node_id, name in CONTROLLERS.items():
            if "latch" in name:
                self.run_setup_for_latches(node_id)
            else:
                self.run_setup_for_controllers_with_encoder(node_id)
        print("\nSetup done.")


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser("Autochanger PDO configuration")
    parser.add_argument(
        "-s", "--subsystem", 
        type=str, choices=["fcs", "fcs-PROTO"], default="fcs", 
        help="Name of the running subsystem: 'fcs' or 'fcs-PROTO'")
    args = parser.parse_args()

    ControllerPDOSetup(main_subsystem=args.subsystem).run()
