"""
Configure PDOs for Maxon controllers
====================================

Carousel             => nodeID: 0xC
Trucks Xminus        => nodeID: 0x2A
Trucks Xplus         => nodeID: 0x2C
OnlineClamp Xplus    => nodeID: 0x30
OnlineClamp Xminus   => nodeID: 0x31
OnlineClamp Yminus   => nodeID: 0x32

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

---

CarouselClamp Xminus => nodeID: 0x1A
CarouselClamp Xplus  => nodeID: 0x1B

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

---

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

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

"""
from time import sleep
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",
    0xc: "carouselController",
    0x1a: "clampXminusController",
    0x1b: "clampXplusController",
}

AUTOCHANGER_CONTROLLERS = (0x2a, 0x2c, 0x30, 0x31, 0x32, 0xe, 0xf)
CAROUSEL_CONTROLLERS = (0x1a, 0x1b, 0xc)

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


class ControllerPDOSetup(object):
    def __init__(self, main_subsystem):
        CCS.attachSubsystem(main_subsystem, 3)
        self.canbus_path = "%s/canbus0" % main_subsystem
        self.canbus = CCS.attachSubsystem(self.canbus_path)
        self.controllers = AUTOCHANGER_CONTROLLERS
        if "fcs" in main_subsystem: 
            self.controllers += CAROUSEL_CONTROLLERS
        if main_subsystem == "carousel":
            self.controllers = CAROUSEL_CONTROLLERS

    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):
        print("=> Activating PDO%d" % 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):
        print("=> Deactivating PDO%d" % 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("Configuring PDOs for 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("=> Setting mapping for 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("=> Setting mapping for 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_carousel_clamps(self, node_id):
        print("Configuring PDOs for node %s => %s" % (node_id, CONTROLLERS[node_id]))

        self.set_preoperational(node_id)

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

        print("=> Setting mapping for 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, AVG_VELOCITY)
        self.write_sdo(node_id, 0x1A00, 0, 1, 3)
        
        self.save_parameters(node_id)

        self.set_operational(node_id)

    def run_disable_all_pdos(self, node_id):
        print("Deactivating PDOs for node %s => %s" % (node_id, CONTROLLERS[node_id]))

        self.set_preoperational(node_id)

        # Deactivate all 4 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, do_disable=False):
        """Execute the sequence"""
        print(__doc__)
        for node_id in self.controllers:
            self.run_single(node_id, do_disable)

    def run_single(self, node_id, do_disable=False):
        name = CONTROLLERS[node_id]
        if do_disable or "latch" in name:
            self.run_disable_all_pdos(node_id)
        elif "clampX" in name:
            self.run_setup_for_carousel_clamps(node_id)
        else:
            self.run_setup_for_controllers_with_encoder(node_id)


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser("Autochanger PDO configuration")
    parser.add_argument(
        "-s", "--subsystem", 
        type=str, choices=["fcs", "fcs-PROTO", "autochanger1", "autochanger2", "carousel"], default="fcs", 
        help="Name of the running subsystem: 'fcs', 'fcs-PROTO', 'carousel', 'autochanger1' or 'autochanger2'")
    parser.add_argument(
        "-d", "--disable", action="store_true",
        help="If set, all PDOs of the autochanger are disabled")
    parser.add_argument(
        "-n", "--nodeid",
        type=str, default="0", help="Node ID of the autochanger controller to print configuration for")
    parser.add_argument(
        "-l", "--list", action="store_true", help="Print the nodeIDs of the Maxon controllers")
    args = parser.parse_args()

    node_id = int(args.nodeid, 16)

    if node_id == 0:
        ControllerPDOSetup(main_subsystem=args.subsystem).run(do_disable=args.disable)
        print("\nFull controller setup done.")
    elif args.list or node_id not in CONTROLLERS.keys():
            print("List of controllers handled by this script:\n")
            print("\n".join(["  %s: %s" % (hex(k), v) for (k, v) in CONTROLLERS.items()]))
            import sys
            sys.exit(0)
    else:
        ControllerPDOSetup(main_subsystem=args.subsystem).run_single(node_id=node_id, do_disable=args.disable)
        print("\nSetup done for " + CONTROLLERS[node_id])
