/**************************************************************************
*
* Copyright (c) 2025-2025 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    Ejector.c
*
* @author  (last)      Sean
* @date    (last)      13-May-2025
*
* @author  (original)  Sean
* @date    (original)  13-May-2025
*
***************************************************************************/

#include "AlarmMgmtTD.h"
#include "Ejector.h"
#include "FpgaTD.h"
#include "Messaging.h"
#include "OperationModes.h"
#include "PersistentAlarm.h"
#include "TaskGeneral.h"
#include "Timers.h"

/**
 *  @addtogroup Ejector
 *  @{
 */

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

#define EJECTOR_RETRACT_OP_TIME             ( ( MS_PER_SECOND * 5 ) / TASK_GENERAL_INTERVAL )   ///< Ejector retract operation interval.
#define EJECTOR_EXTEND_OP_TIME              ( ( MS_PER_SECOND * 10 ) / TASK_GENERAL_INTERVAL )  ///< Ejector extend operation interval.
#define EJECTOR_BACKOFF_OP_TIME             ( 50 / TASK_GENERAL_INTERVAL )                      ///< Ejector back-off operation interval.
#define EJECTOR_DATA_PUB_INTERVAL           ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )           ///< Ejector data publish interval.
#define DATA_PUBLISH_COUNTER_START_COUNT    13                                                  ///< Ejector data publish start counter.

#define EJECTOR_RETRACT_MOTOR_SPEED_RPM     -350.0F                                             ///< Ejector motor retract speed (in RPM).
#define EJECTOR_EXTEND_MOTOR_SPEED_RPM      162.5F                                              ///< Ejector motor extend speed (in RPM).
#define EJECTOR_OFF_MOTOR_SPEED_RPM         0.0F                                                ///< Ejector motor extend speed (in RPM).

#define EJECTOR_OPT_SENSOR_ACTIVE           0                                                   ///< Ejector optical sensor active.
#define EJECTOR_OPT_SENSOR_INACTIVE         1                                                   ///< Ejector optical sensor inactive.

/// Enumeration of ejector states.
typedef enum EjectorOperations
{
    EJECTOR_OPERATION_HOME = 0,                                                                 ///< Ejector home operation
    EJECTOR_OPERATION_RETRACT,                                                                  ///< Ejector retract operation
    EJECTOR_OPERATION_EXTEND,                                                                   ///< Ejector extend operation
    NUM_OF_EJECTOR_OPERATIONS,                                                                  ///< Number of ejector operations
} EJECTOR_OPERATION_T;

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

static EJECTOR_STATE_T currentEjectorState;                                                     ///< Current ejector control state.
static F32 currentEjectorSetSpeed;                                                              ///< Current ejector set speed (in RPM).
static U32 ejectorOperationTimerCounter;                                                        ///< Ejector operation timer counter (in task intervals).
static U32 ejectorDataPublicationTimerCounter;                                                  ///< Ejector data broadcast timer counter (in task intervals).
static OVERRIDE_U32_T ejectorDataPublishInterval;                                               ///< Ejector data broadcast interval (in ms).

static BOOL ejectorHomeRequested;                                                               ///< Flag indicating that ejector home operation is requested.
static BOOL ejectorRetractRequested;                                                            ///< Flag indicating that ejector retract operation is requested.
static BOOL ejectorExtendRequested;                                                             ///< Flag indicating that ejector extend operation is requested.
static BOOL ejectorAbortRequested;                                                              ///< Flag indicating that ejector abort operation in requested.
static OVERRIDE_U32_T ejectorRetractOptSensor;                                                  ///< Ejector retract optical sensor.
static OVERRIDE_U32_T ejectorEngageOptSensor;                                                   ///< Ejector engage optical sensor.

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

static EJECTOR_STATE_T handleEjectorStartState( void );
static EJECTOR_STATE_T handleEjectorHomingState( void );
static EJECTOR_STATE_T handleEjectorRetractedState( void );
static EJECTOR_STATE_T handleEjectorExtendedState( void );
static EJECTOR_STATE_T handleEjectorRetractingState( void );
static EJECTOR_STATE_T handleEjectorExtendingState( void );
static EJECTOR_STATE_T handleEjectorRetractBackoffState( void );
static EJECTOR_STATE_T handleEjectorDirChangeStopState( void );
static EJECTOR_STATE_T handleEjectorAbortState( void );
static void setEjectorSpeed( F32 speed );
static void resetEjectorFlags( void );
static void publishEjectorData( void );

/*********************************************************************//**
 * @brief
 * The initEjector function initializes the ejector driver.
 * @details \b Inputs: none
 * @details \b Outputs: Ejector driver unit initialized
 * @return none
 *************************************************************************/
void initEjector(void)
{
    // Initialize driver
    initEjectorMotor();

    // Initialize controller variables
    currentEjectorState = EJECTOR_STATE_INIT;
    currentEjectorSetSpeed = EJECTOR_OFF_MOTOR_SPEED_RPM;
    resetEjectorFlags();
    ejectorOperationTimerCounter = 0;
    ejectorDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT;
    ejectorDataPublishInterval.data = EJECTOR_DATA_PUB_INTERVAL;
    ejectorDataPublishInterval.ovData = EJECTOR_DATA_PUB_INTERVAL;
    ejectorDataPublishInterval.ovInitData = EJECTOR_DATA_PUB_INTERVAL;
    ejectorDataPublishInterval.override = OVERRIDE_RESET;

    ejectorRetractOptSensor.data       = (U32)getFPGAEjectorRetractOpticalSensor();
    ejectorRetractOptSensor.ovData     = 0;
    ejectorRetractOptSensor.ovInitData = 0;
    ejectorRetractOptSensor.override   = OVERRIDE_RESET;

    ejectorEngageOptSensor.data        = (U32)getFPGAEjectorEngageOpticalSensor();
    ejectorEngageOptSensor.ovData      = 0;
    ejectorEngageOptSensor.ovInitData  = 0;
    ejectorEngageOptSensor.override    = OVERRIDE_RESET;
}

/*********************************************************************//**
 * @brief
 * The resetEjectorFlags function resets the ejector flags to FALSE.
 * @details \b Inputs: none
 * @details \b Outputs: ejectorHomeRequested, ejectorRetractRequested,
 * ejectorExtendRequested, ejectorAbortRequested
 * @return none
 *************************************************************************/
static void resetEjectorFlags( void )
{
    ejectorHomeRequested = FALSE;
    ejectorRetractRequested = FALSE;
    ejectorExtendRequested = FALSE;
    ejectorAbortRequested = FALSE;
}

/*********************************************************************//**
 * @brief
 * The setEjectorSpeed function records the requested ejector speed and
 * sets the ejector motor speed to that speed.
 * @details \b Inputs: none
 * @details \b Outputs: currentEjectorSetSpeed
 * @param speed Target speed for ejector motor.
 * @return none
 *************************************************************************/
static void setEjectorSpeed( F32 speed )
{
    currentEjectorSetSpeed = speed;
    setEjectorMotorSpeed( currentEjectorSetSpeed );
}

/*********************************************************************//**
 * @brief
 * The homeEjector function requests an ejector home operation.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: ejectorHomeRequested
 * @return TRUE if home command accepted, FALSE if not
 *************************************************************************/
BOOL homeEjector( void )
{
    BOOL result = FALSE;

    if ( EJECTOR_STATE_INIT == currentEjectorState )
    {
        ejectorHomeRequested = TRUE;
        result = TRUE;
    }
    else
    {
        result = retractEjector();
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The retractEjector function requests an ejector retract operation.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: ejectorRetractRequested
 * @return TRUE if retract command accepted, FALSE if not
 *************************************************************************/
BOOL retractEjector( void )
{
    BOOL result = FALSE;

    if ( EJECTOR_STATE_EXTENDED == currentEjectorState )
    {
        ejectorRetractRequested = TRUE;
        result = TRUE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The extendEjector function requests an ejector extend operation.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: ejectorExtendRequested
 * @return TRUE if extend command accepted, FALSE if not
 *************************************************************************/
BOOL extendEjector( void )
{
    BOOL result = FALSE;

    if ( EJECTOR_STATE_RETRACTED == currentEjectorState )
    {
        ejectorExtendRequested = TRUE;
        result = TRUE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The abortEjectorOperation function requests an ejector abort operation.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: ejectorAbortRequested
 * @return none
 *************************************************************************/
void abortEjectorOperation( void )
{
    switch ( currentEjectorState )
    {
        case EJECTOR_STATE_HOMING:
        case EJECTOR_STATE_RETRACTING:
        case EJECTOR_STATE_EXTENDING:
            setEjectorMotorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM );
            // Set the flag to TRUE regardless of where the ejector is at
            ejectorAbortRequested = TRUE;
            break;

        default:
            // Do nothing on the rest of the states. Only initiate the abort sequence if an operation is in progress.
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The getEjectorState function returns the current ejector control state machine.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: none
 * @return current state of the ejector control state machine.
 *************************************************************************/
EJECTOR_STATE_T getEjectorState( void )
{
    return currentEjectorState;
}

/*********************************************************************//**
 * @brief
 * The execEjectorController function executes the ejector control state machine.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if the current state is invalid.
 * @details \b Inputs: currentEjectorState
 * @details \b Outputs: currentEjectorState
 * @return none
 *************************************************************************/
void execEjectorController( void )
{
    if ( TRUE == ejectorAbortRequested )
    {
        // If abort has been called, set it to abort state no matter where we are at.
        currentEjectorState = EJECTOR_STATE_ABORT;
    }

    // Update the optical sensor values
    ejectorRetractOptSensor.data = (U32)getFPGAEjectorRetractOpticalSensor();
    ejectorEngageOptSensor.data  = (U32)getFPGAEjectorEngageOpticalSensor();

    switch( currentEjectorState )
    {
        case EJECTOR_STATE_INIT:
            currentEjectorState = handleEjectorStartState();
            break;

        case EJECTOR_STATE_HOMING:
            currentEjectorState = handleEjectorHomingState();
            break;

        case EJECTOR_STATE_RETRACTED:
            currentEjectorState = handleEjectorRetractedState();
            break;

        case EJECTOR_STATE_EXTENDED:
            currentEjectorState = handleEjectorExtendedState();
            break;

        case EJECTOR_STATE_RETRACTING:
            currentEjectorState = handleEjectorRetractingState();
            break;

        case EJECTOR_STATE_EXTENDING:
            currentEjectorState = handleEjectorExtendingState();
            break;

        case EJECTOR_STATE_RETRACT_BACKOFF:
            currentEjectorState = handleEjectorRetractBackoffState();
            break;

        case EJECTOR_STATE_DIR_CHANGE_STOP:
            currentEjectorState = handleEjectorDirChangeStopState();
            break;

        case EJECTOR_STATE_ABORT:
            currentEjectorState = handleEjectorAbortState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_EJECTOR_INVALID_STATE, (U32)currentEjectorState )
            break;
    }

    // reset request flags after processing ejector control state machine (they expire)
    resetEjectorFlags();

    // publish ejector data if it's time to publish
    publishEjectorData();
}

/*********************************************************************//**
 * @brief
 * The handleEjectorStartState function starts the ejector control state machine.
 * @details \b Inputs: ejectorHomeRequested
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorStartState( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_INIT;

    if ( TRUE == ejectorHomeRequested )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_HOMING;
    }
    else
    {
        disableEjectorMotor();
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorHomingState function handles the homing state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorHomingState( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_HOMING;

    if ( ++ejectorOperationTimerCounter >= EJECTOR_RETRACT_OP_TIME )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_DIR_CHANGE_STOP;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorRetractedState function handles the retracted state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorExtendRequested
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorRetractedState( void )
{
   EJECTOR_STATE_T state = EJECTOR_STATE_RETRACTED;

   if ( TRUE == ejectorExtendRequested )
   {
       ejectorOperationTimerCounter = 0;
       setEjectorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM );
       state = EJECTOR_STATE_EXTENDING;
   }
   else
   {
       disableEjectorMotor();
   }

   return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorExtendedState function handles the extended state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorRetractRequested
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorExtendedState ( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_EXTENDED;

    if ( TRUE == ejectorRetractRequested )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_RETRACTING;
    }
    else
    {
        disableEjectorMotor();
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorRetractingState function handles the retracting state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorRetractingState ( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_RETRACTING;

    if ( ( ++ejectorOperationTimerCounter >= EJECTOR_RETRACT_OP_TIME ) || ( EJECTOR_OPT_SENSOR_ACTIVE == getU32OverrideValue( &ejectorRetractOptSensor ) ) )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_DIR_CHANGE_STOP;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorDirChangeStopState function handles the direction change
 * stop state of the ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorDirChangeStopState( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_DIR_CHANGE_STOP;

    if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_EXTEND_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_RETRACT_BACKOFF;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorRetractBackoffState function handles the retract back-off
 * state of the ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorRetractBackoffState( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_RETRACT_BACKOFF;

    if ( ++ejectorOperationTimerCounter >= EJECTOR_BACKOFF_OP_TIME )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_RETRACTED;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorExtendingState function handles the extending state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorExtendingState ( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_EXTENDING;

    if ( ( ++ejectorOperationTimerCounter >= EJECTOR_EXTEND_OP_TIME ) || ( EJECTOR_OPT_SENSOR_ACTIVE == getU32OverrideValue( &ejectorEngageOptSensor ) ) )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorSpeed( EJECTOR_OFF_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_EXTENDED;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleEjectorAbortState function handles the abort state of the
 * ejector control state machine.
 * @details \b Inputs: ejectorOperationTimerCounter, ejectorHomeRequested
 * @details \b Outputs: ejectorOperationTimerCounter
 * @return next state of the ejector control state machine
 *************************************************************************/
static EJECTOR_STATE_T handleEjectorAbortState( void )
{
    EJECTOR_STATE_T state = EJECTOR_STATE_ABORT;

    if ( TRUE == ejectorHomeRequested )
    {
        ejectorOperationTimerCounter = 0;
        setEjectorMotorSpeed( EJECTOR_RETRACT_MOTOR_SPEED_RPM );
        state = EJECTOR_STATE_HOMING;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The publishEjectorData function constructs and sends the air pump data
 * broadcast message.
 * @details \b Message \b Sent: MSG_ID_TD_EJECTOR_DATA
 * @details \b Inputs: ejectorDataPublicationTimerCounter, currentEjectorState,
 * currentEjectorSetSpeed
 * @details \b Outputs: ejectorDataPublicationTimerCounter
 * @return none
 *************************************************************************/
static void publishEjectorData( void )
{
    if ( ++ejectorDataPublicationTimerCounter >= getU32OverrideValue( &ejectorDataPublishInterval ) )
    {
        EJECTOR_PAYLOAD_T data;

        data.h5State              = getEjectorState();
        data.h5SetSpeed           = currentEjectorSetSpeed;
        data.retractOpticalSensor = getU32OverrideValue( &ejectorRetractOptSensor );
        data.engageOpticalSensor  = getU32OverrideValue( &ejectorEngageOptSensor );

        broadcastData( MSG_ID_TD_EJECTOR_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( EJECTOR_PAYLOAD_T ) );
        ejectorDataPublicationTimerCounter = 0;
    }
}


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


/*********************************************************************//**
 * @brief
 * The testEjectorDataPublishIntervalOverride function overrides the interval
 * at which the ejector data is published.
 * @details \b Inputs: none
 * @details \b Outputs: ejectorDataPublishInterval
 * @param message Override message from Dialin which includes the interval
 * (in ms) to override the ejector data broadcast interval to.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testEjectorDataPublishIntervalOverride( MESSAGE_T *message )
{
    BOOL result = u32BroadcastIntervalOverride( message, &ejectorDataPublishInterval, TASK_GENERAL_INTERVAL );

    return result;
}

/*********************************************************************//**
 * @brief
 * The testEjectorCommand function handles a test request to execute an
 * ejector operation.
 * @details \b Inputs: none
 * @details \b Outputs: requested operation initiated
 * @param message Ejector operation request message from Dialin which includes
 * the requested ejector operation ID
 * @return TRUE if request is successful, FALSE if not
 *************************************************************************/
BOOL testEjectorCommand( MESSAGE_T *message )
{
    BOOL result = FALSE;

    // Verify tester has logged in with TD and override type is valid
    if ( TRUE == isTestingActivated() )
    {
        // Verify payload length is valid
        if ( sizeof( U32 ) == message->hdr.payloadLen )
        {
            U32 cmd;
            EJECTOR_OPERATION_T op;

            memcpy( &cmd, message->payload, sizeof(U32) );
            op = (EJECTOR_OPERATION_T)cmd;

            switch ( op )
            {
                case EJECTOR_OPERATION_HOME:
                    result = homeEjector();
                    break;

                case EJECTOR_OPERATION_RETRACT:
                    result = retractEjector();
                    break;

                case EJECTOR_OPERATION_EXTEND:
                    result = extendEjector();
                    break;

                default:
                    // no alarm since this is a test function
                    break;
            }
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testEjectorRetractOpticalSensorOverride function overrides the retract
 * optical sensor.
 * @details \b Inputs: none
 * @details \b Outputs: ejectorRetractOptSensor
 * @param message Override message from Dialin which includes the retract
 * optical sensor value.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testEjectorRetractOpticalSensorOverride( MESSAGE_T *message )
{
    BOOL result = u32Override( message, &ejectorRetractOptSensor, EJECTOR_OPT_SENSOR_ACTIVE, EJECTOR_OPT_SENSOR_INACTIVE );

    return result;
}

/*********************************************************************//**
 * @brief
 * The testEjectorEngageOpticalSensorOverride function overrides the engage
 * optical sensor.
 * @details \b Inputs: none
 * @details \b Outputs: ejectorEngageOptSensor
 * @param message Override message from Dialin which includes the engage
 * optical sensor value.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testEjectorEngageOpticalSensorOverride( MESSAGE_T *message )
{
    BOOL result = u32Override( message, &ejectorEngageOptSensor, EJECTOR_OPT_SENSOR_ACTIVE, EJECTOR_OPT_SENSOR_INACTIVE );

    return result;
}

/**@}*/