from org.lsst.ccs.scripting import CCS

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


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("=== PDO%d ===" % (i+1))
            sdo = self.read_sdo(pdoId, 1)
            print("[+] COB-ID Transmit")
            print("readSDO(0x180%d, 1) = %s" % (i, hex(sdo)))
            print("Bit 30 PDO valid : %s" % hex((sdo >> 30) & 1))
            print("Bit 31 RTR : %s" % hex((sdo >> 31) & 1))
            print("Bit & Node ID : %s" % hex((sdo - 0x180 - i*0x100)))
            sdo = self.read_sdo(pdoId, 2)
            print("[+] Sync mode")
            print("readSDO(0x180%d, 2) = %s" % (i, 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, hex(sdo)))
            print("[+] Configuration")
            for j in range(4):
                sdo = self.read_sdo(pdoConfigId, j)
                print("readSDO(0x1A0%d, %d) = %s" % (i, j, hex(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"], default="fcs", 
        help="Name of the running subsystem: 'fcs', 'fcs-PROTO', 'autochanger1' or 'autochanger2'")
    parser.add_argument(
        "-n", "--nodeid",
        type=str, help="Node ID of the autochanger controller to print configuration for")
    args = parser.parse_args()

    node_id = int(args.nodeid, 16)
    
    ControllerConfigPrinter(args.subsystem, node_id).run()
