import sys
import os
sys.path.append(os.path.join(os.path.dirname(__file__), "../submodules/FEED/src/python"))
import leep
import json
from collections import OrderedDict


class ff_setup():

    def __init__(self, dev):
        self.dev = dev
        self.mem_size = 2048

    def _range_ok(self, x, WI, signed=True):
        WI = WI-1 if signed else WI
        if not (-2**WI <= x <= 2**WI-1):
            print("\n\nWARNING: register value (%d) out-of-range\n\n" % x)

    def _leep_wr_log(self, wr_arr):
        print("leep_write_log:", wr_arr)
        self.dev.reg_write(wr_arr)

    def write_coeffs(self, json_cfg):
        with open(json_cfg) as FH:
            cfg = json.load(FH)

        print("FF setup:")
        for k, v in cfg.items():
            print("\t%s : %f" % (k, v))
        dt, tau = cfg["dt"], cfg["tau"]
        cavity = dt/tau*2**22
        drv_couple = int(cfg["couple"] * cavity)
        cav_decay = int(-cavity)

        self._range_ok(drv_couple, WI=18)
        self._range_ok(cav_decay, WI=18)
        self._leep_wr_log([('ff_driver_coeff_0', drv_couple),
                           ('ff_driver_coeff_1', cav_decay)])

        # Compute and set clip limits
        amp_flat = cfg["d_amp"]  # Flat-top amplitude, in counts
        xmin = 0  # Prevents integrated drive from accumulating error
        xmax = int(amp_flat*1.5)

        self._range_ok(xmin, WI=18)
        self._range_ok(xmax, WI=18)
        self._leep_wr_log([('fdbk_core_mp_proc_lim_0', xmax),
                           ('fdbk_core_mp_proc_lim_2', xmin)])

    def write_mem(self, mem_file):
        with open(mem_file, 'r') as FH:
            ff_mem = [l.rstrip() for l in FH]

        if (len(ff_mem) < self.mem_size):
            print("Unexpected memory size, aborting write. Expected 2048.")
            return

        self.dev.reg_write([('ff_driver_mem', ff_mem)])

    def print_state(self):
        print("Feedback controller settings:")
        root_fb = "fdbk_core_mp_proc_"
        root_ff = "ff_driver_"
        fdbk_dict = OrderedDict({"FF EN": root_fb + "ff_en",
                                 "FF_COUPLE": root_ff + "coeff_0",
                                 "FF_DECAY": root_ff + "coeff_1",
                                 "xmax": root_fb + "lim_0",
                                 "xmin": root_fb + "lim_2",
                                 "ymax": root_fb + "lim_1",
                                 "ymin": root_fb + "lim_3",
                                 "xcoeff I": root_fb + "coeff_0",
                                 "xcoeff P": root_fb + "coeff_2",
                                 "ycoeff I": root_fb + "coeff_1",
                                 "ycoeff P": root_fb + "coeff_3",
                                 "xmag": root_fb + "setmp_0",
                                 "xpha": root_fb + "setmp_2",
                                 "ymag": root_fb + "setmp_1",
                                 "ypha": root_fb + "setmp_3"})

        rlist = [x for x in fdbk_dict.values()]
        rr = self.dev.reg_read(rlist)
        for k, v in zip(fdbk_dict.keys(), rr):
            print("\t%s : %d" % (k, v))

    def set_ctrl_state(self, state):
        print("Turning feedforward mode %s" % state)
        ff_reg = 1 if state == "on" else 0
        self._leep_wr_log([('fdbk_core_mp_proc_ff_en', ff_reg)])


if __name__ == "__main__":

    from argparse import ArgumentParser

    parser = ArgumentParser(description="RFS Feedforward Setup Helper")

    parser.add_argument('-a', '--address', dest="dev_addr", default=None, required=True,
                        help='Device URL (leep://<IP> or ca://<PREFIX>)')
    parser.add_argument('-z', '--zone', dest='zone', default=1, type=int, choices=[0, 1],
                        help='Cavity controller (zone)')
    parser.add_argument('-j', '--json', dest='json', default=None,
                        help='Feedforward seetings JSON')
    parser.add_argument('-m', '--mem', dest='mem', default=None,
                        help='Feedforward memory')
    parser.add_argument('-s', '--state', dest='state', default=None, type=str,
                        choices=['off', 'on'], help='Set Feedforward state for selected controller')

    args = parser.parse_args()

    dev = leep.open(args.dev_addr, instance=[args.zone])

    ffs = ff_setup(dev)
    if args.json:
        ffs.write_coeffs(args.json)
    if args.mem:
        ffs.write_mem(args.mem)
    if args.state:
        ffs.set_ctrl_state(args.state)

    ffs.print_state()
