/**************************************************************************
*
* Copyright (c) 2024-2024 Diality Inc. - All Rights Reserved.
*
* THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
* WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
*
* @file    FPOperationModes.c
*
* @author  (last)      Sean
* @date    (last)      15-Nov-2024
*
* @author  (original)  Sean
* @date    (original)  15-Nov-2024
*
***************************************************************************/

#include "FPModeFault.h"
#include "FPModeInitPOST.h"
#include "FPModeStandby.h"
#include "FPOperationModes.h"
#include "Messaging.h"
#include "ModeGenPermeateDefeatured.h"
#include "ModePreGenPermeateDefeatured.h"
#include "ModeGenPermeate.h"
#include "ModePreGenPermeate.h"
#include "TaskGeneral.h"

/**
 *  @addtogroup FPOperationModes
 *  @{
 */

// ********** private definitions **********

#define BROADCAST_TD_OP_MODE_INTERVAL     ( 250 / TASK_GENERAL_INTERVAL )       ///< FP operation mode broadcast interval (in task interval/sec).
#define DATA_PUBLISH_COUNTER_START_COUNT  11                                    ///< Data publish counter start count.
#define SIGNAL_START                      1
#define SIGNAL_STOP                       0

// ********** private data **********

static volatile BOOL modeRequest[ NUM_OF_FP_MODES - 1 ];                        ///< Pending operation mode change requests.
static FP_OP_MODE_T lastMode;                                                   ///< Last operation mode prior to current mode.
static FP_OP_MODE_T currentMode;                                                ///< Current operation mode.
static U32 currentSubMode;                                                      ///< The currently active state of the active mode.
static U32 broadcastModeIntervalCtr;                                            ///< Interval counter used to determine when to broadcast operation mode.  Initialize to 11 to stagger broadcast.
static U32 currentSubState;                                                     ///< current sub state.
static U32 current4thLevelState;                                                ///< current 4th level state.
static BOOL isDeviceDefeatured;                                                 ///< bool to determine defeatured status
static BOOL isBoostInstalled;                                                   ///< bool to determine boost pump status

/// Interval (in task intervals) at which to publish operation mode data to CAN bus.
static OVERRIDE_U32_T opModePublishInterval = { BROADCAST_TD_OP_MODE_INTERVAL, BROADCAST_TD_OP_MODE_INTERVAL, BROADCAST_TD_OP_MODE_INTERVAL, 0 };

/// This matrix determines legal transitions from one mode to another
static const FP_OP_MODE_T MODE_TRANSITION_TABLE[ NUM_OF_FP_MODES - 1 ][ NUM_OF_FP_MODES - 1 ] = {
// From    to->    FAULT         SERVICE       INIT          STANBY        PGEN          GENW          DPGP          DEGP
/* FAUL     */{ FP_MODE_FAUL, FP_MODE_SERV, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG },
/* SERV     */{ FP_MODE_FAUL, FP_MODE_SERV, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG },
/* INIT     */{ FP_MODE_FAUL, FP_MODE_NLEG, FP_MODE_INIT, FP_MODE_STAN, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG },
/* STAN     */{ FP_MODE_FAUL, FP_MODE_SERV, FP_MODE_NLEG, FP_MODE_STAN, FP_MODE_PGEN, FP_MODE_NLEG, FP_MODE_DPGP, FP_MODE_NLEG },
/* PGEN     */{ FP_MODE_FAUL, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_STAN, FP_MODE_PGEN, FP_MODE_GENP, FP_MODE_NLEG, FP_MODE_NLEG },
/* GENW     */{ FP_MODE_FAUL, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_STAN, FP_MODE_NLEG, FP_MODE_GENP, FP_MODE_NLEG, FP_MODE_NLEG },
/* DPGP     */{ FP_MODE_FAUL, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_STAN, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_DPGP, FP_MODE_DEGP },
/* DEGP     */{ FP_MODE_FAUL, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_STAN, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_NLEG, FP_MODE_DEGP } };

// ********** private function prototypes **********

static FP_OP_MODE_T arbitrateModeRequest( void );
static void transitionToNewOperationMode( FP_OP_MODE_T newMode );
static void broadcastOperationMode( void );

/*********************************************************************//**
 * @brief
 * The initFPOperationModes function initializes the Operation Modes unit.
 * @details \b Inputs: none
 * @details \b Outputs: Operation Modes unit initialized.
 * @return none
 *************************************************************************/
void initFPOperationModes( void )
{
    U32 i;

    // Initialize mode requests to none pending
    for ( i = 0; i < ( NUM_OF_FP_MODES - 1 ); i++ )
    {
        modeRequest[ i ] = FALSE;
    }

    // Start in init mode
    lastMode                 = FP_MODE_INIT;
    currentMode              = FP_MODE_INIT;
    currentSubMode           = 0;
    currentSubState          = NO_FP_SUB_STATE;
    current4thLevelState     = NO_FP_SUB_STATE;
    broadcastModeIntervalCtr = DATA_PUBLISH_COUNTER_START_COUNT;
    isDeviceDefeatured       = FALSE;
    isBoostInstalled         = FALSE;

    transitionToNewOperationMode( FP_MODE_INIT );

    // Call initializers for the individual modes
    initFPFaultMode();
//    initFPServiceMode();
    initFPInitAndPOSTMode();
    initFPStandbyMode();
    initPreGenPMode();
    initGenPermeateMode();
}

/*********************************************************************//**
 * @brief
 * The execFPOperationModes function executes the Operation Modes state machine.
 * @details \b Alarm: ALARM_ID_FP_SOFTWARE_FAULT if current mode is invalid.
 * @details \b Inputs: none
 * @details \b Outputs: currentMode is set by state machine.
 * @return none
 *************************************************************************/
void execFPOperationModes( void )
{
    FP_OP_MODE_T newMode;
    U32 priorSubMode = currentSubMode;
    U32 priorSubState = currentSubState;
    U32 prior4thLevelState = current4thLevelState;

    // Any new mode requests?
    newMode = arbitrateModeRequest();   // Will return current mode if no pending requests
    newMode = MODE_TRANSITION_TABLE[ currentMode ][ newMode ];

    // Is requested new mode valid and legal at this time?
    if ( ( newMode >= FP_MODE_NLEG ) && ( isTestingActivated() != TRUE ) )
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, FP_FAULT_ID_OP_MODES_ILLEGAL_MODE_TRANSITION_REQUESTED, newMode )
        newMode = currentMode;
    }

    // Has mode changed?
    if ( currentMode != newMode )
    {
        lastMode = currentMode;
        // Handle transition to new mode
        transitionToNewOperationMode( newMode );
        currentMode = newMode;
//        sendOperationStatusEvent();
    }

    // Mode specific processing to be done continuously
    switch ( currentMode )
    {
        case FP_MODE_FAUL:
            currentSubMode = execFPFaultMode();
            break;

        case FP_MODE_SERV:
//          currentSubMode = execFPServiceMode();
            break;

        case FP_MODE_INIT:
            currentSubMode = execFPInitAndPOSTMode();
            break;

        case FP_MODE_STAN:
            currentSubMode = execFPStandbyMode();
            break;

        case FP_MODE_PGEN:
            currentSubMode = execPreGenPMode();
            break;

        case FP_MODE_GENP:
            currentSubMode = execGenPermeateMode();
            break;

        case FP_MODE_DPGP:
            currentSubMode = execPreGenPermeateDefeaturedMode();
            break;

        case FP_MODE_DEGP:
            currentSubMode = execPreGenPMode();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, FP_FAULT_ID_OP_MODES_INVALID_MODE_STATE, currentMode )
            currentMode = FP_MODE_FAUL;
            currentSubMode = 0;
            break;
    }   // End switch

#ifndef _VECTORCAST_
    // Send operation status event when appropriate
    if ( ( priorSubMode != currentSubMode ) || ( priorSubState != currentSubState ) || ( prior4thLevelState != current4thLevelState ) )
#endif
    {
//        sendOperationStatusEvent();
        SEND_EVENT_WITH_2_U32_DATA( FP_EVENT_SUB_MODE_CHANGE, priorSubMode, currentSubMode )
    }

    // Broadcast current operation mode on interval
    broadcastOperationMode();
}

/*********************************************************************//**
 * @brief
 * The requestNewFPOperationMode function requests transition to a new
 * operation mode.  The request will be arbitrated when the state machine
 * is next executed.
 * @details \b Alarm: ALARM_ID_FP_SOFTWARE_FAULT if given new mode is invalid.
 * @details \b Inputs: none
 * @details \b Outputs: modeRequest[]
 * @return none
 *************************************************************************/
void requestNewFPOperationMode( FP_OP_MODE_T newMode )
{
    // Validate requested mode
    if ( newMode < FP_MODE_NLEG )
    {
        // Make request
        modeRequest[ newMode ] = TRUE;
    }
    else
    {  // Invalid mode requested
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, FP_FAULT_ID_OP_MODES_INVALID_MODE_REQUESTED, newMode )
    }
}

/*********************************************************************//**
 * @brief
 * The getCurrentFPOperationMode function gets the current operation mode.
 * @details \b Inputs: currentMode
 * @details \b Outputs: none
 * @return the current operation mode
 *************************************************************************/
FP_OP_MODE_T getCurrentFPOperationMode( void )
{
    return currentMode;
}

/*********************************************************************//**
 * @brief
 * The getPreviousFPOperationMode function gets the previous operation mode.
 * @details \b Inputs: lastMode
 * @details \b Outputs: none
 * @return the previous operation mode
 *************************************************************************/
FP_OP_MODE_T getPreviousFPOperationMode( void )
{
    return lastMode;
}

/*********************************************************************//**
 * @brief
 * The getCurrentFPSubMode function gets the current operation sub-mode.
 * @details \b Inputs: currentSubMode
 * @details \b Outputs: none
 * @return the current operation sub-mode
 *************************************************************************/
U32  getCurrentFPSubMode( void )
{
    return currentSubMode;
}

/*********************************************************************//**
 * @brief
 * The arbitrateModeRequest function arbitrates any pending mode transition
 * requests.
 * @note Thread protection prevents new mode requests from being made while
 * arbitration is in progress.
 * @details \b Inputs: modeRequest[]
 * @details \b Outputs: modeRequest[] is reset
 * @return the next operation mode (current mode if no requests pending)
 *************************************************************************/
static FP_OP_MODE_T arbitrateModeRequest( void )
{
    FP_OP_MODE_T reqMode = currentMode;
    U32 i;

    // Block additional requests until after mode arbitration
    _disable_IRQ();

    // Select highest priority mode request -or- current mode if no requests pending
    for ( i = 0; i < FP_MODE_NLEG; i++ )
    {
        if ( modeRequest[ i ] != FALSE )
        {
            reqMode = (FP_OP_MODE_T)i;
            break;
        }
    }

    // Clear all requests now that an arbitration winner is selected
    for ( i = 0; i < FP_MODE_NLEG; i++ )
    {
        modeRequest[ i ] = FALSE;
    }

    // Un-block requests
    _enable_IRQ();

    return reqMode;
}

/*********************************************************************//**
 * @brief
 * The transitionToNewOperationMode function calls the transition to function
 * for a new operation mode that we are transitioning to.
 * @details \b Alarm: ALARM_ID_FP_SOFTWARE_FAULT if given new mode is invalid.
 * @details \b Inputs: none
 * @details \b Outputs: transition function called for new mode
 * @return none
 *************************************************************************/
static void transitionToNewOperationMode( FP_OP_MODE_T newMode )
{
    // Setup for new operating mode
    switch ( newMode )
    {
        case FP_MODE_FAUL:
            currentSubMode = transitionToFPFaultMode();
            break;

        case FP_MODE_SERV:
//            currentSubMode = transitionToFPServiceMode();
            break;

        case FP_MODE_INIT:
            currentSubMode = transitionToFPInitAndPOSTMode();
            break;

        case FP_MODE_STAN:
            currentSubMode = transitionToFPStandbyMode();
            break;

        case FP_MODE_PGEN:
            currentSubMode = transitionToPreGenPMode();
            break;

        case FP_MODE_GENP:
            currentSubMode = transitionToGenPermeateMode();
            break;

        case FP_MODE_DPGP:
            currentSubMode = transitionToPreGenPermeateDefeaturedMode();
            break;

        case FP_MODE_DEGP:
            currentSubMode = transitionToGenPermeateDefeaturedMode();
            break;


        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, FP_FAULT_ID_OP_MODES_ILLEGAL_MODE_TRANSITION_REQUESTED, newMode )
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The broadcastOperationMode function sends the current operation mode at
 * the prescribed interval.
 * @details \b Inputs: broadcastModeIntervalCtr
 * @details \b Outputs: FP operation mode broadcast message sent.
 * @return none
 *************************************************************************/
static void broadcastOperationMode( void )
{
    if ( ++broadcastModeIntervalCtr >= getU32OverrideValue( &opModePublishInterval ) )
    {
        OP_MODE_PAYLOAD_T data;

        broadcastModeIntervalCtr = 0;
        data.opMode = (U32)currentMode;
        data.subMode = currentSubMode;
        broadcastData( MSG_ID_FP_OP_MODE_DATA, COMM_BUFFER_OUT_CAN_FP_BROADCAST, (U08*)&data, sizeof( OP_MODE_PAYLOAD_T ) );
    }
}

/*********************************************************************//**
 * @brief
 * The setCurrentFPSubState function sets the current subState.
 * @details \b Inputs: none
 * @details \b Outputs: currentSubState
 * @param subState the enumerated sub state.
 * @return none
 *************************************************************************/
void setCurrentFPSubState( U32 subState )
{
    currentSubState = subState;
}

/*********************************************************************//**
 * @brief
 * The setCurrentFP4thLevelState function sets the current 4th level state.
 * @details \b Inputs: none
 * @details \b Outputs: current4thLevelState
 * @param subState the enumerated sub state.
 * @return none
 *************************************************************************/
void setCurrentFP4thLevelState( U32 state )
{
    current4thLevelState = state;
}

/*********************************************************************//**
 * @brief
 * The isFPDefeatured function returns if the Leahi device is defeatured.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @return TRUE if FP device is de-featured, FALSE if not.
 *************************************************************************/
BOOL isFPDefeatured( void )
{
    // TODO - pull status from NV mem.
    return isDeviceDefeatured;
}

/*********************************************************************//**
 * @brief
 * The isBoostPumpInstalled function returns if the Leahi device
 * has a boost pump installed.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @return TRUE if FP device is contains a boost pump, FALSE if not.
 *************************************************************************/
BOOL isBoostPumpInstalled( void )
{
    // TODO - pull status from NV mem.
    return isBoostInstalled;
}

/*********************************************************************//**
 * @brief
 * The signalStopGenPermeate function returns if the Leahi device
 * has a boost pump installed.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @return the rejection reason code for stopping generate permeate.
 *************************************************************************/
REQUEST_REJECT_REASON_CODE_T signalStopGenPermeate( void )
{
    REQUEST_REJECT_REASON_CODE_T reason = REQUEST_REJECT_REASON_NONE;
    BOOL result = FALSE;

    // Stop is agnostic to defeatured/featured.
    // As long as were not in fault or POST, request to go to standby to stop
    if ( TRUE == isFPDefeatured() )
    {
        result = requestPreGenDefStop();
    }
    else
    {
        result = requestPreGenStop();
    }

    if ( FALSE == result )
    {
        reason = REQUEST_REJECT_REASON_NOT_ALLOWED_IN_CURRENT_MODE;
    }

    return reason;
}

/*********************************************************************//**
 * @brief
 * The signalStartGenPermeate function returns if the Leahi device
 * has a boost pump installed.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @return the rejection reason code for starting generate permeate.
 *************************************************************************/
REQUEST_REJECT_REASON_CODE_T signalStartGenPermeate( void )
{
    REQUEST_REJECT_REASON_CODE_T reason = REQUEST_REJECT_REASON_NONE;
    BOOL result = FALSE;

    // TODO - Service and disinfection checks happen here.
    if ( TRUE == isFPDefeatured() )
    {
        result = requestPreGenDefStart();
    }
    else
    {
        result = requestPreGenStart();
    }

    if ( FALSE == result )
    {
        reason = REQUEST_REJECT_REASON_NOT_ALLOWED_IN_CURRENT_MODE;
    }

    return reason;
}


/*************************************************************************
 *                       TEST SUPPORT FUNCTIONS
 *************************************************************************/


/*********************************************************************//**
 * @brief
 * The testSetFPOperationMode function will transition to a given operation
 * mode if the transition is legal.
 * @details \b Inputs: none
 * @details \b Outputs: modeRequest[]
 * @param message message from Dialin which includes the op mode to
 * transition to.
 * @return TRUE if request successful, FALSE if not
 *************************************************************************/
BOOL testSetFPOperationMode( MESSAGE_T *message )
{
    BOOL result = FALSE;

    // Verify message payload length is valid
    if ( sizeof( U32 ) == message->hdr.payloadLen )
    {
        U32 newMode;

        memcpy( &newMode, message->payload, sizeof( U32 ) );
        if ( (FP_OP_MODE_T)newMode < NUM_OF_FP_MODES )
        {
            FP_OP_MODE_T reqMode = (FP_OP_MODE_T)newMode;

            requestNewFPOperationMode( reqMode );
            result = TRUE;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetFPOpModePublishIntervalOverride function sets the override of the
 * operation mode publication interval.
 * @details \b Inputs: none
 * @details \b Outputs: opModePublishInterval
 * @param message override message from Dialin which includes the interval
 * (in ms) to set the op mode publish to.
 * @return TRUE if override set successful, FALSE if not
 *************************************************************************/
BOOL testSetFPOpModePublishIntervalOverride( MESSAGE_T *message )
{
    BOOL result = u32BroadcastIntervalOverride( message, &opModePublishInterval, TASK_GENERAL_INTERVAL );

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetGeneratePermeateSignal function sets start stop signal of generate
 * permeate.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @param message message from Dialin which includes the signal to start or stop
 * generate permeate modes.
 * @return TRUE if set successful, FALSE if not
 *************************************************************************/
BOOL testSetGeneratePermeateSignal( MESSAGE_T *message )
{
    BOOL result = FALSE;

    // Verify message payload length is valid
    if ( sizeof( U32 ) == message->hdr.payloadLen )
    {
        U32 signalStartStop;

        memcpy( &signalStartStop, message->payload, sizeof( U32 ) );
        if ( SIGNAL_START == signalStartStop )
        {
            signalStartGenPermeate();
        }
        else if ( SIGNAL_STOP == signalStartStop )
        {
            signalStopGenPermeate();
        }
    }

    return result;
}

/**@}*/