/**************************************************************************
*
* 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    RotaryValve.c
*
* @author  (last)      Sean
* @date    (last)      03-Oct-2024
*
* @author  (original)  Sean
* @date    (original)  03-Oct-2024
*
***************************************************************************/

#include "AlarmMgmtTD.h"
#include "FpgaTD.h"
#include "Messaging.h"
#include "RotaryValve.h"

/**
 *  @addtogroup RotaryValve
 *  @{
 */

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

#define VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION     6.25F                   ///< 6.25 microsteps per encoder count.

#define VALVE_MOTOR_DRIVER_FAULT_BIT_MASK           0x1                     ///< Bit mask for valve motor driver fault.
#define VALVE_DIRECTION_FAULT_BIT_MASK              0x2                     ///< Bit mask for valve direction fault.
#define VALVE_INCORRECT_ENCODER_STATUS_BIT_MASK     0x4                     ///< Bit mask for valve incorrect encoder sensor fault.

/// Bit mask for all (OR'd) valve faults.
#define VALVE_GEN_FAULT_BIT_MASK                    ( VALVE_MOTOR_DRIVER_FAULT_BIT_MASK | VALVE_DIRECTION_FAULT_BIT_MASK | VALVE_INCORRECT_ENCODER_STATUS_BIT_MASK )

#pragma pack(push, 1)
/// Payload record structure for rotary valve set position request
typedef struct
{
    U32 valve;                                                              ///< which rotary valve to set position for (0=H1, 1=H19)
    S16 stepChange;                                                         ///< number of steps to change position (pos indicates CW, neg indicates CCW)
} VALVE_ROTARY_SET_CMD_PAYLOAD_T;
#pragma pack(pop)

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

static U08 valveControl[ NUM_OF_VALVES ];                                   ///< Current control bits for each valve.
static OVERRIDE_U32_T valveStatus[ NUM_OF_VALVES ];                         ///< Current status bits for each valve.
static S16 commandValvePosChange[ NUM_OF_VALVES ];                          ///< Current commanded valve position changes.  Negative indicates CCW direction.
static OVERRIDE_S32_T currentValveEncPosition[ NUM_OF_VALVES ];             ///< Current encoder valve positions (overrideable).  Negative indicates CCW direction.

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

static U08 getValveStatus( VALVE_T valve );

/*********************************************************************//**
 * @brief
 * The initRotaryValvesDriver function initializes the rotary valves driver
 * unit.
 * @details \b Inputs: none
 * @details \b Outputs: Rotary valves driver unit is initialized
 * @return none
 *************************************************************************/
void initRotaryValvesDriver(void)
{
    U32 i;

    for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ )
    {
        valveControl[ i ] = FPGA_PINCH_VALVES_1_32_STEP | FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP;  // enable valves, configure for 1/32 step control
        commandValvePosChange[ i ] = 0;
        currentValveEncPosition[ i ].data = 0;
        currentValveEncPosition[ i ].ovData = 0;
        currentValveEncPosition[ i ].ovInitData = 0;
        currentValveEncPosition[ i ].override = OVERRIDE_RESET;
        valveStatus[ i ].data = 0;
        valveStatus[ i ].ovData = 0;
        valveStatus[ i ].ovInitData = 0;
        valveStatus[ i ].override = OVERRIDE_RESET;
    }

    // set valve control bits once at startup
    setH1Control( valveControl[ H1_VALV ] );
    setH19Control( valveControl[ H19_VALV ] );
}

/*********************************************************************//**
 * @brief
 * The readValves function gets the current position and status of all rotary
 * pinch valves from the FPGA.
 * @note This function should be called periodically to maintain fresh
 * status and positions for all valves.
 * @details \b Alarm: ALARM_ID_TD_PINCH_VALVE_FAULT if valve reports a driver,
 * direction, or encoder fault.
 * @details \b Inputs: FPGA
 * @details \b Outputs: valveStatus[], currentValveEncPosition[]
 * @return none
 *************************************************************************/
void readValves( void )
{
    U32 i;
    S16 cmdPos[ NUM_OF_VALVES ];

    // Get latest valve status and positions from FPGA
    cmdPos[ H19_VALV ]                       = getH19CmdPosition();
    currentValveEncPosition[ H19_VALV ].data = (S32)getValveEncoderPosition( H19_VALV );
    valveStatus[ H19_VALV ].data             = getH19Status();
    cmdPos[ H1_VALV ]                        = getH1CmdPosition();
    currentValveEncPosition[ H1_VALV ].data  = (S32)getValveEncoderPosition( H1_VALV );
    valveStatus[ H1_VALV ].data              = getH1Status();

    // TODO check commanded position vs. commanded position read back from FPGA - alarm different for some persistent period of time

    // Check valves status for faults
    for ( i = FIRST_VALVE; i < NUM_OF_VALVES; i++ )
    {
        U32 status = getValveStatus( (VALVE_T)i );

        if ( ( status & VALVE_GEN_FAULT_BIT_MASK ) != 0 )
        {
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_PINCH_VALVE_FAULT, i, status )
        }
    }
}

/*********************************************************************//**
 * @brief
 * The setValveCmdChangePosition function sets the commanded magnitude (in
 * steps or microsteps as appropriate) for a position change of a given valve.
 * @details \b Message \Sent: MSG_ID_TD_EVENT for TD_EVENT_VALVE_POS_CHANGE
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given.
 * @details \b Inputs: none
 * @details \b Outputs: commandValvePosChange[]
 * @param valve Valve to set new commanded change in position for
 * @param mag Magnitude of position change for valve (in encoder counts)
 * @param dir Direction to move valve
 * @return none.
 *************************************************************************/
void setValveCmdChangePosition( VALVE_T valve, U16 mag, MOTOR_DIR_T dir )
{
    if ( valve < NUM_OF_VALVES )
    {
        S16 chg = (S16)mag * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 );
        U08 ctrl;
        F32 temp;

        // log event showing valve position change commanded
        SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POS_CHANGE, (U32)commandValvePosChange[ valve ], (U32)chg );
        // convert magnitude of change from encoder counts to microsteps
        temp = (F32)chg * VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION;
        chg = (S16)((S32)((temp) + FLOAT_TO_INT_ROUNDUP_OFFSET));
        mag = (S16)(abs(chg));
        // give FPGA valve change command
        commandValvePosChange[ valve ] = chg;
        if ( H1_VALV == valve )
        {
            ctrl = getH1Control();
            if ( MOTOR_DIR_REVERSE == dir )
            {
                setH1Control( ( ctrl | FPGA_PINCH_VALVES_REVERSE ) );
            }
            else
            {
                setH1Control( ( ctrl & ~FPGA_PINCH_VALVES_REVERSE ) );
            }
            setH1Position( mag );
        }
        else
        {
            ctrl = getH19Control();
            if ( MOTOR_DIR_REVERSE == dir )
            {
                setH19Control( ( ctrl | FPGA_PINCH_VALVES_REVERSE ) );
            }
            else
            {
                setH19Control( ( ctrl & ~FPGA_PINCH_VALVES_REVERSE ) );
            }
            setH19Position( mag );
        }
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE1, (U32)valve )
    }
}

/*********************************************************************//**
 * @brief
 * The getValveCmdTravel function gets the most recent commanded travel (in
 * encoder counts) for a given valve.
 * @note A negative return value indicates travel in the reverse (CCW) direction.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given.
 * @details \b Inputs: commandValvePosChange[]
 * @details \b Outputs: none
 * @param valve ID of valve to get commanded position for
 * @return Commanded position change (in encoder counts) for the given valve
 *************************************************************************/
S16 getValveCmdTravel( VALVE_T valve )
{
    S16 result = 0;

    if ( valve < NUM_OF_VALVES )
    {
        F32 temp = (F32)commandValvePosChange[ valve ] / VALVE_ENC_COUNT_2_MICRO_STEP_CONVERSION;  // convert magnitude of change from microsteps to encoder counts

        result = (S16)((S32)((temp) + FLOAT_TO_INT_ROUNDUP_OFFSET));
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE2, (U32)valve )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getValveEncoderPosition function gets the current actual position
 * (in encoder counts) for a given valve.
 * @note There are 1024 encoder counts per revolution.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given.
 * @details \b Inputs: currentValveEncPosition[]
 * @details \b Outputs: none
 * @param valve ID of valve to get actual position for
 * @return Actual position for the given valve
 *************************************************************************/
S16 getValveEncoderPosition( VALVE_T valve )
{
    S16 result = 0;

    if ( valve < NUM_OF_VALVES )
    {
        result = (S16)(currentValveEncPosition[ valve ].data);
        if ( OVERRIDE_KEY == currentValveEncPosition[ valve ].override )
        {
            result = (S16)(currentValveEncPosition[ valve ].ovData);
        }
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE3, (U32)valve )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getValveStatus function gets the current status for a given valve.
 *    Status bits:
 *       0-motor driver fault
 *       1-direction fault
 *       2-incorrect encoder sensor fault
 *       3..7-reserved
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve given.
 * @details \b Inputs: valveStatus[]
 * @details \b Outputs: none
 * @param valve ID of valve to get status for
 * @return Status for the given valve
 *************************************************************************/
static U08 getValveStatus( VALVE_T valve )
{
    U08 result = 0;

    if ( valve < NUM_OF_VALVES )
    {
        result = (U08)( valveStatus[ valve ].data & MASK_OFF_U32_MSBS );
        if ( OVERRIDE_KEY == valveStatus[ valve ].override )
        {
            result = (U08)( valveStatus[ valve ].ovData & MASK_OFF_U32_MSBS );
        }
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE4, (U32)valve )
    }

    return result;
}


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


/*********************************************************************//**
 * @brief
 * The testValveEncoderPositionOverride function overrides the valve encoder
 * position for a given valve.
 * @details \b Inputs: none
 * @details \b Outputs: currentValveEncPosition[]
 * @param message Override message from Dialin which includes an ID of
 * the valve to override and the position to override the valve to.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testValveEncoderPositionOverride( MESSAGE_T *message )
{
    BOOL result = u32ArrayOverride( message, (OVERRIDE_U32_T*)&currentValveEncPosition[0], NUM_OF_VALVES - 1, 0, HEX_32_BIT_FULL_SCALE );

    return result;
}

/*********************************************************************//**
 * @brief
 * The testValveStatusOverride function overrides the valve status for a
 * given valve.
 * @details \b Inputs: none
 * @details \b Outputs: valveStatus[]
 * @param message Override message from Dialin which includes an ID of
 * the valve to override and the status to override the valve to.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testValveStatusOverride( MESSAGE_T *message )
{
    BOOL result = u32ArrayOverride( message, &valveStatus[0], NUM_OF_VALVES - 1, 0, MASK_OFF_U32_MSBS );

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetValve function commands a given valve to change position by a
 * given number of encoder counts.
 * @note A negative travel value indicates the travel should be in reverse
 * (CCW) direction.
 * @details \b Inputs: none
 * @details \b Outputs: commandValvePosChange[]
 * @param message set message from Dialin which includes the state to set
 * the air pump to.
 * @return TRUE if set request is successful, FALSE if not
 *************************************************************************/
BOOL testSetValve( MESSAGE_T *message )
{
    BOOL result = FALSE;

    // Verify tester has logged in with TD
    if ( TRUE == isTestingActivated() )
    {
        // Verify payload length is valid
        if ( sizeof( VALVE_ROTARY_SET_CMD_PAYLOAD_T ) == message->hdr.payloadLen )
        {
            VALVE_ROTARY_SET_CMD_PAYLOAD_T payload;

            memcpy( &payload, message->payload, sizeof(VALVE_ROTARY_SET_CMD_PAYLOAD_T) );
            if ( payload.valve < NUM_OF_VALVES )
            {
                MOTOR_DIR_T dir = MOTOR_DIR_FORWARD;
                U16         mag;

                if ( payload.stepChange < 0 )
                {
                    dir = MOTOR_DIR_REVERSE;
                }
                mag = (U16)( abs( payload.stepChange ) );
                setValveCmdChangePosition( (VALVE_T)payload.valve, mag, dir );
                result = TRUE;
            }
        }
    }

    return result;
}

/**@}*/