from org.lsst.ccs.scripting import CCS

CONTROLLERS = {
    0x2a: "acTruckXminusController",
    0x2c: "acTruckXplusController",
    0x30: "onlineClampXplusController",
    0x31: "onlineClampXminusController",
    0x32: "onlineClampYminusController",
    0xe: "latchXplusController",
    0xf: "latchXminusController",
    0xc: "carouselController",
    0x1a: "clampXminusController",
    0x1b: "clampXplusController",
}

MASK = dict(
    STATUS_WORD = 0x60410010,
    AVG_CURRENT = 0x20270010,
    POSITION = 0x60640020,
    CURRENT = 0x60780010,
    FOLLOWING_ERROR = 0x20F40010,
    AVG_VELOCITY = 0x20280020,
    ACTIVATE = 0x40000000,
    DEACTIVATE = 0xC0000000,
)
INV_MASK = dict((v, k) for k, v in MASK.items())

def long_hex(value):
    return hex(value)[:-1]

class ControllerConfigPrinter(object):
    def __init__(self, subsystem_name, node_id):
        self.node_id = node_id
        controller_name = CONTROLLERS.get(node_id, None)
        if not controller_name:
            import sys
            sys.exit(1)

        print("Printing PDO configuration for %s" % controller_name)
        self.ctl = CCS.attachSubsystem(subsystem_name + "/canbus0/" + controller_name)
        
    def read_sdo(self, index, subindex):
        return self.ctl.sendSynchCommand("readSDO", index, subindex)

    def run(self):
        for i in range(4):
            pdoId = 0x1800 + i
            pdoConfigId = 0x1A00 + i
            print("\n=== PDO%d ===" % (i+1))
            sdo = self.read_sdo(pdoId, 1)
            print("[+] COB-ID Transmit")
            print("readSDO(0x180%d, 1) = %s" % (i, long_hex(sdo)))
            print("RTR : %d" % int((sdo >> 30) & 1))
            print("PDO valid (0:yes 1:no) : %d" % int((sdo >> 31) & 1))
            print("Bit & Node ID : %s" % long_hex((sdo - 0x180 - i*0x100)))
            sdo = self.read_sdo(pdoId, 2)
            print("[+] Sync mode")
            print("readSDO(0x180%d, 2) = %s" % (i, long_hex(sdo)))
            print(" 1 = synchronous ; 253 = asynchronous on RTR only ; 255 = asynchronous")
            sdo = self.read_sdo(pdoId, 3)
            print("[+] Inhibit time (x 100 us)")
            print("readSDO(0x180%d, 3) = %s" % (i, long_hex(sdo)))
            print("[+] Configuration")
            for j in range(4):
                sdo = self.read_sdo(pdoConfigId, j)
                print("readSDO(0x1A0%d, %d) = %s | %s" % (i, j, long_hex(sdo), INV_MASK.get(sdo)))


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser("PDO configuration printer")
    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(
        "-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 args.list or node_id not in CONTROLLERS.keys():
        print("\n".join(["%s: %s" % (hex(k), v) for (k, v) in CONTROLLERS.items()]))
        import sys
        sys.exit(0)

    ControllerConfigPrinter(args.subsystem, node_id).run()
