###################################################################################################
#    OPERATION FUNCTIONS
###################################################################################################
#
# Contains functions to operate the FOFB application.
#
###################################################################################################

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


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


###################################################################################################
#    OPERATIONS ON CCN
###################################################################################################

def stop_ccn(nodename):
    """
    Stop the communication with cellnode on the specified fofbnode.

    PARAMETERS
    ----------
    nodename: str
        The target fofbnode, ie 'cellnode-c09' or 'centralnode'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(nodename)
    if prx is None:
        logger.error("Failed to stop CCN on {}".format(nodename))
        return


    logger.info("Stopping CCN on {}".format(nodename))
    nint=1
    if 'central' in nodename:
        nint = 4
    for n in range(nint):
        prx["ccnpack{}_control".format(n)] = False
        prx["ccnunpack{}_control".format(n)] = False

def reset_ccn(nodename):
    """
    Reset the communication with cellnode on the specified fofbnode.

    PARAMETERS
    ----------
    nodename: str
        The target fofbnode, ie 'cellnode-c09' or 'centralnode'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(nodename)
    if prx is None:
        logger.error("Failed to reset CCN on {}".format(nodename))
        return

    logger.info("Reset CCN on {}".format(nodename))
    nint=1
    if 'central' in nodename:
        nint = 4
    for n in range(nint):
        prx["ccneth{}_reset".format(n)] = 0x60000001 # impossible to write 0xE0000001
        prx["ccneth{}_gt_reset".format(n)] = 1

    time.sleep(2)

    for n in range(nint):
        prx["ccneth{}_gt_reset".format(n)] = 0
        prx["ccneth{}_reset".format(n)] = 0

    ack_ccn(nodename)

def start_ccn(nodename):
    """
    Start the communication with cellnode on the specified fofbnode.

    PARAMETERS
    ----------
    nodename: str
        The target fofbnode, ie 'cellnode-c09' or 'centralnode'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(nodename)
    if prx is None:
        logger.error("Failed to start CCN on {}".format(nodename))
        return

    logger.info("Starting CCN on {}".format(nodename))
    nint=1
    if 'central' in nodename:
        nint = 4
    for n in range(nint):
        prx["ccnpack{}_control".format(n)] = True
        prx["ccnunpack{}_control".format(n)] = True

def ack_ccn(nodename):
    """
    Start the communication with cellnode on the specified fofbnode.

    PARAMETERS
    ----------
    nodename: str
        The target fofbnode, ie 'cellnode-c09' or 'centralnode'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(nodename)
    if prx is None:
        logger.error("Failed to ack CCN on {}".format(nodename))
        return


    logger.info("Ack CCN error on {}".format(nodename))
    nint=1
    if 'central' in nodename:
        nint = 4
    for n in range(nint):
        prx["ccnpack{}_reset_error".format(n)] = True
        prx["ccnunpack{}_reset_error".format(n)] = True

    time.sleep(1)

    for n in range(nint):
        prx["ccnpack{}_reset_error".format(n)] = False
        prx["ccnunpack{}_reset_error".format(n)] = False

###################################################################################################
#    OPERATIONS ON CCN
###################################################################################################

def stop_combpm(cellnodename):
    """
    Stop the communication with bpm on the specified cellnode.

    PARAMETERS
    ----------
    cellnodename: str
        The target fofbnode, ie 'cellnode-c09'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(cellnodename)
    if prx is None:
        logger.error("Failed to stop ComBPM on {}".format(p))
        return

    logger.info("Stopping ComBPM on {}".format(cellnodename))
    prx["combpm_reset"] = 1
    prx["combpm_gt_control"] = 0x5

def start_combpm(cellnodename):
    """
    Start the communication with bpm on the specified cellnode.

    PARAMETERS
    ----------
    cellnodename: str
        The target fofbnode, ie 'cellnode-c09'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(cellnodename)
    if prx is None:
        logger.error("Failed to start ComBPM on {}".format(cellnodename))
        return

    logger.info("Starting ComBpm on {}".format(cellnodename))
    prx["combpm_reset"] = 0
    prx["combpm_gt_control"] = 0x1
    time.sleep(1)
    ack_combpm(cellnodename)


def ack_combpm(cellnodename):
    """
    Ack errors on the communication with bpm on the specified cellnode.

    PARAMETERS
    ----------
    cellnodename: str
        The target fofbnode, ie 'cellnode-c09'

    """
    prx=FofbTool.Utils.get_prx_from_nodename(cellnodename)
    if prx is None:
        logger.error("Failed to start ComBPM on {}".format(cellnodename))
        return

    logger.info("Ack ComBpm on {}".format(cellnodename))
    prx["combpm_reset_error"] = True
    time.sleep(1)
    prx["combpm_reset_error"] = False

###################################################################################################
#    OPERATIONS ON BPM ELECTRONICS
###################################################################################################

# Some local constants
ADDR_CCCFG=0        # Register address of Libera Electron
ADDR_FAICFG=0x2000  # Register address of Libera Electron

def electron_init_fa(bpmlist):
    """
    Configure FA register of Libera Electron

    PARAMETERS
    ----------
    bpmlist: list of (bpmid, bpmpath)
        ID to apply and path of BPM tango device

    RETURNS
    -------
    success: boolean
        True on success
    """
    # Init BPMs, put for each the ID number
    # Cannot do that with group
    logger.info("Initialize BPMs")
    for bpmid, bpmpath in bpmlist:
        logger.debug("Initialize BPM {}".format(bpmpath))
        pbpm = tango.DeviceProxy(bpmpath)
        try:
            pbpm.ping()
        except tango.ConnectionFailed:
            logger.error("Failed to connect to {}".format(bpmpath))
            return False


        # Configure : bpmid, tframe, mgtpower, mgtlb, bfrclr
        pbpm.command_inout("WriteFAData", [ADDR_CCCFG, 4, 5, bpmid, 6000, 0, 0, 6000])
        # Ack rise
        pbpm.command_inout("WriteFAData", [ADDR_FAICFG, 4, 1, 9])
        # Ack fall
        pbpm.command_inout("WriteFAData", [ADDR_FAICFG, 4, 1, 8])

    return True



def electron_start_next_trigger(bpmlist):
    """
    Prepare Libera Electron to start on next trigger

    PARAMETERS
    ----------
    bpmlist: list of str
        List of Tango paths to Libera devices

    RETURNS
    -------
    success: boolean
        True on success
    """
    logger.info("Prepare Libera Electron to start at next trigger")

    bpms = tango.Group('bpms')
    bpms.add(bpmlist)

    r=bpms.command_inout("settimeonnexttrigger")
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command SetTimeOnNextTrigger on bpm {}".format(_r.dev_name()))
            return False

    # Write start, tframelim=zero, no debug data
    r=bpms.command_inout("WriteFAData", [ADDR_FAICFG, 4, 1, 8])
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command WriteFAData on bpm {}".format(_r.dev_name()))
            return False

    # Write Enable ITech FAI
    r=bpms.command_inout("WriteFAData", [ADDR_FAICFG+4, 4, 1,1])
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command WriteFAData on bpm {}".format(_r.dev_name()))
            return False

    return True

def electron_stop_com(bpmlist):
    """
    Stop Libera Electron FA communication

    PARAMETERS
    ----------
    bpmlist: list of str
        List of Tango paths to Libera devices

    RETURNS
    -------
    success: boolean
        True on success
    """
    logger.info("Stop Libera Electron FA communication")

    bpms = tango.Group('bpms')
    bpms.add(bpmlist)

    # Write Disable ITech FAI
    r=bpms.command_inout("WriteFAData", [ADDR_FAICFG+4, 4, 1, 0])
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command WriteFAData on bpm {}".format(_r.dev_name()))
            return False

    # Soft stop
    r=bpms.command_inout("WriteFAData", [ADDR_FAICFG+8, 4, 1, 0])
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command WriteFAData on bpm {}".format(_r.dev_name()))
            return False

    # Reset User FAI
    r=bpms.command_inout("WriteFAData", [ADDR_FAICFG, 4, 1, 0])
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to apply command WriteFAData on bpm {}".format(_r.dev_name()))
            return False

    return True


def sync_all_bpm():
    """
    Synchronize all BPM electronics, Electron and Brillance Plus.
    This will use the timing system (central and local board).

    """
    EVN=66 # Event number

    # Get FofbManager proxy, only for its properties
    p = tango.DeviceProxy(FofbTool.Configuration.config["tangopath"]["fofb-manager"])

    bpmlist = [n.split(':')[2] for n in p.get_property("bpmlist")['bpmlist']]
    bpmidlist = [(int(n.split(':')[0]), n.split(':')[2]) for n in p.get_property("bpmlist")['bpmlist']]

    tlocal = tango.Group('tlocal')
    tlocal.add([n.split(':')[2] for n in p.get_property('TimingBoardList')['TimingBoardList'] if "LOCAL" in n])

    # Set a group of Libera Brillance Plus EVRX board, from FofbTool configuration
    lbpevrx = tango.Group('lbpevrx')
    lbpevrx.add(FofbTool.Configuration.config["tangopath"]["lbpevrx"])

    tcentral = tango.DeviceProxy(FofbTool.Configuration.config["tangopath"]["timing-central"])

    # ---------------------------------------------------------------------------------------------------------------
    # Init BPMs, stop first and put for each the ID number

    electron_stop_com(bpmlist)
    electron_init_fa(bpmidlist)

    # ---------------------------------------------------------------------------------------------------------------
    # Write event number

    logger.info("Set Event Number on local timing board, BpmTriggerEvent")
    r=tlocal.write_attribute("bpm.trigEvent", EVN)
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to set Event Number on local timing board {}, bpm.trigEvent".format(_r.dev_name()))
            return

    # ---------------------------------------------------------------------------------------------------------------
    # Prepare bpm for trigger reception

    electron_start_next_trigger(bpmlist)

    logger.info("Prepare Libera Brillance Plus to start on next trigger")
    r=lbpevrx.write_attribute("synchronize", 0)
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to write synchronize on LBP EVRX {}".format(_r.dev_name()))
            return

    # ---------------------------------------------------------------------------------------------------------------
    # Wait 2 seconds and Fire the soft event

    time.sleep(2)
    logger.info("Fire the trigger")

    tcentral.write_attribute("softEventAdress", EVN)
    tcentral.firesoftevent()

    # ---------------------------------------------------------------------------------------------------------------
    # Write event number back to 3

    logger.info("Set Event Number back to 3 on local timing board, BpmTriggerEvent")
    r=tlocal.write_attribute("bpm.trigEvent", 3)
    for _r in r:
        if _r.has_failed():
            logger.error("Failed to set Event Number on local timing board {}, bpm.trigEvent".format(_r.dev_name()))
            return