###################################################################################################
#    CONFIGURATION FUNCTIONS
###################################################################################################
#
# Contains functions to configure blocks of the FOFB application.
#
###################################################################################################

import tango
import logging
import numpy as np
import FofbTool.Utils


# Get the module logger
logger = logging.getLogger("FofbTool")


###################################################################################################
#    CONFIGURE COM BPM
###################################################################################################

# Filter list depends on the cellnode. These are the default values.
bpmfilterlist = {
        "cellnode-c01":np.array(list(range(1,23))+list(range(115,123))),
        "cellnode-c06":np.array(range(23,53)),
        "cellnode-c09":np.array(range(53,83)),
        "cellnode-c14":np.array(range(83,115)),
        }

def cellnode_configure_combpm(cellnodename, bpmallowed=None):
    """
    Configure the combpm block of a CellNode.
    The BPM data allowed through are either taken in the argument, or default to nominal values.


    PARAMETERS
    ----------
    cellnodename: str
        The target cellnode, ie 'cellnode-c09'
    bpmallowed: [int], None
        List of BPMID allowed through the block. If None, default to nominal values.

    RETURN
    ------
    success: boolean
        True if configuration is a success
    """

    # Get device proxy
    try:
        p = FofbTool.Utils.tangopath_cellnodes[cellnodename.lower()]
    except KeyError:
        logger.error("Wrong cellnodename. Possibilities are {}".format(FofbTool.Utils.tangopath_cellnodes.keys()))
        return False

    try:
        prx= tango.DeviceProxy(p)
    except tango.DevFailed as e:
        logger.error("Failed to get the Device proxy to '{}'".format(p))
        logger.debug(str(e))
        return False

    # Select BPM id allowed to pass through
    if bpmallowed is None:
        logger.debug("Default to nominal values for BPM filter")
        bpmallowed = bpmfilterlist[cellnodename.lower()]


    logger.debug("Activate BPMs {} in the filter for {}".format(bpmallowed, p))
    f = prx["combpm_filter_table"].value
    f[:] = 0
    f[np.array(bpmallowed)] = 0x80
    prx["combpm_filter_table"] = f

    logger.info("Configuration of ComBpm done on {}.".format(p))
    return True


###################################################################################################
#    CONFIGURE CCN: COM CELLNODES
###################################################################################################


def cellnode_configure_ccn(cellnodename, nbpm=None, npsc=100):
    """
    Configure the ComCellNode block on a cellnode.
    Automatically set the number of bpm/psc packets and MAC length.

    PARAMETERS
    ----------
    cellnodename: str
        The target cellnode, ie 'cellnode-c09'
    nbpm:
        Number of BPM allowed by the filter, hence the number of expected BPM packets.
        If None, auto detect the number from the combpm_filter_table attribute.
    npsc:
        Number of total PSC, hence the number of expected PSC packets.
        Default to 100.

    RETURN
    ------
    success: boolean
        True if configuration is a success
    """

    # Get device proxy
    try:
        p = FofbTool.Utils.tangopath_cellnodes[cellnodename.lower()]
    except KeyError:
        logger.error("Wrong cellnodename. Possibilities are {}".format(FofbTool.Utils.tangopath_cellnodes.keys()))
        return False

    try:
        prx= tango.DeviceProxy(p)
    except tango.DevFailed as e:
        logger.error("Failed to get the Device proxy to '{}'".format(p))
        logger.debug(str(e))
        return False

    if nbpm is None:
        # Get number of BPM activated in the filter
        nbpm = int((prx["combpm_filter_table"].value == 0x80).sum())
        logger.debug("{} bpm allowed in the filter on {}".format(nbpm, p))

    maclen = 10*nbpm+10
    logger.debug("Configure packeter framesize (mac length) to {}".format(maclen))
    prx["ccnpack0_framesize"] = maclen

    logger.debug("Configure packeter npackets to {}".format(nbpm-1))
    prx["ccnpack0_npackets"] = nbpm-1

    maclen = npsc*6+10
    logger.debug("Configure unpacketer framesize (mac length) to {}".format(maclen))
    prx["ccnunpack0_framesize"] = maclen

    logger.info("Configuration of CCN done on {}.".format(p))
    return True

def centralnode_configure_ccn(nbpm=[30,30,30,32], npsc=100):
    """
    Configure the ComCellNode block on the centralnode.
    Automatically set the number of bpm/psc packets and MAC length.

    PARAMETERS
    ----------
    nbpm: list(int)
        Number of BPM packet received on each interface.
    npsc:
        Number of total PSC, hence the number of expected PSC packets.

    RETURN
    ------
    success: boolean
        True if configuration is a success
    """
    p = FofbTool.Utils.tangopath_nodes["centralnode"]
    try:
        prx= tango.DeviceProxy(p)
    except tango.DevFailed as e:
        logger.error("Failed to get the Device proxy to '{}'".format(p))
        logger.debug(str(e))
        return False

    for n in range(4):
        maclen = npsc*6+10
        logger.debug("Configure packeter {} framesize (mac length) to {}".format(n, maclen))
        prx["ccnpack{}_framesize".format(n)] = maclen

        logger.debug("Configure packeter {}  npackets to {}".format(n, npsc-1))
        prx["ccnpack{}_npackets".format(n)] = npsc-1

        maclen = 10*nbpm[n]+10
        logger.debug("Configure unpacketer {} framesize (mac length) to {}".format(n, maclen))
        prx["ccnunpack{}_framesize".format(n)] = maclen

    logger.info("Configuration of CCN done on {}.".format(p))
    return True

###################################################################################################
#    CONFIGURE COM CORR
###################################################################################################

pscidlist = {
    'cellnode-c01':[50,100,49,99,255,255,255,255,2,52,1,51,255,255,255,255,56,6,3,53,4,54,5,55,10,60,7,57,8,58,9,59],
    'cellnode-c06':[12,62,11,61,255,255,255,255,14,64,13,63,255,255,255,255,68,18,15,65,16,66,17,67,22,72,19,69,20,70,21,71],
    'cellnode-c09':[24,74,23,73,255,255,255,255,26,76,25,75,255,255,255,255,80,30,27,77,28,78,29,79,34,84,31,81,32,82,33,83],
    'cellnode-c14':[36,86,35,85,37,87,38,88,40,90,39,89,255,255,255,255,94,44,41,91,42,92,43,93,48,98,45,95,46,96,47,97]
    }

def cellnode_configure_comcorr(cellnodename, pscid=None, enable=True):
    """
    Configure the comcorr block of a CellNode.
    The PSC ID either taken in the argument, or default to nominal values.


    PARAMETERS
    ----------
    cellnodename: str
        The target cellnode, ie 'cellnode-c09'
    pscid: [int], None
        List of 32 PSCID to program on the output board. If None, default to nominal values.

    enable: boolean
        Enable or not the hardware outputs.

    RETURN
    ------
    success: boolean
        True if configuration is a success
    """

    # Get device proxy
    try:
        p = FofbTool.Utils.tangopath_cellnodes[cellnodename.lower()]
    except KeyError:
        logger.error("Wrong cellnodename. Possibilities are {}".format(FofbTool.Utils.tangopath_cellnodes.keys()))
        return False

    try:
        prx= tango.DeviceProxy(p)
    except tango.DevFailed as e:
        logger.error("Failed to get the Device proxy to '{}'".format(p))
        logger.debug(str(e))
        return False

    if pscid is None:
        logger.debug("Default to nominal values for PSCID")
        pscid = pscidlist[cellnodename.lower()]
    else:
        if len(pscid) != 32:
            logger.error("pscid shall have length 32")
            return False


    logger.debug("Set PSCIDs {} in the filter for {}".format(pscid, p))
    f= prx["comcorr_line_id"].value
    f[:] = pscid
    f[:] = f+ 0x10000*bool(enable)
    logger.debug("Set comcorr_line_id {} for {}".format(f, p))
    prx["comcorr_line_id"] = f

    logger.info("Configuration of ComCorr done on {}.".format(p))
    return True

###################################################################################################
#    CONFIGURE MATRIX CORRECTOR
###################################################################################################

def centralnode_configure_corr():
    """
    Configure the correction algorithm on the centralnode.
    """
    p = FofbTool.Utils.tangopath_nodes["centralnode"]
    try:
        prx= tango.DeviceProxy(p)
    except tango.DevFailed as e:
        logger.error("Failed to get the Device proxy to '{}'. Configuration of corrector algorithm failed.".format(p))
        logger.debug(str(e))
        return False

    # Legacy
    prx["corr_k1a_x"]=256
    prx["corr_k1b_x"]=0
    prx["corr_k1ic_x"]=64
    prx["corr_k1d_x"]=16300

    prx["corr_k1a_y"]=256
    prx["corr_k1b_y"]=0
    prx["corr_k1ic_y"]=64
    prx["corr_k1d_y"]=16300

    # Unitary
    prx["corr_k2a_x"]=128
    prx["corr_k2b_x"]=0
    prx["corr_k2ic_x"]=8192
    prx["corr_k2d_x"]=0

    prx["corr_k2a_y"]=128
    prx["corr_k2b_y"]=0
    prx["corr_k2ic_y"]=8192
    prx["corr_k2d_y"]=0

    f= prx["corr_pscid"].value
    f[0:100] = range(1,101)
    prx["corr_pscid"] = f

    logger.info("Configuration of Corr done on {}.".format(p))
    return True