/**************************************************************************
*
* 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_MOTOR_DRIVER_FAULT_BIT_MASK           0x10                    ///< Bit mask for valve motor driver fault.
#define VALVE_MOTOR_STALL_BIT_MASK                  0x40                    ///< Bit mask for valve stall.

/// Bit mask for all (OR'd) valve faults.
#define VALVE_GEN_FAULT_BIT_MASK                    ( VALVE_MOTOR_DRIVER_FAULT_BIT_MASK | VALVE_MOTOR_STALL_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 commandValvePos[ NUM_OF_VALVES ];                                ///< Current commanded valve positions.
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++ )
    {
        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;
    }
    commandValvePos[ H1_VALV ] = getH1EncoderPosition();
    commandValvePos[ H19_VALV ] = getH19EncoderPosition();
}

/*********************************************************************//**
 * @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;

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

    // TODO - compare commanded vs. read position

    // 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 )  TODO - resolve issue and restore alarm
        }
    }
}

/*********************************************************************//**
 * @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: commandValvePos[]
 * @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 )
    {
        U16 mag8 = ( ( mag % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( mag >> 3 ) << 3 : ( ( mag >> 3 ) << 3 ) + ROTARY_VALVE_MICROSTEP_FRACTION );   // round to multiple of 8 to maximize holding torque
        S16 chgSteps = (S16)mag8 * ( MOTOR_DIR_REVERSE == dir ? -1 : 1 );                 // convert to signed change in position - negative value indicates reverse direction

        // log event showing valve position change commanded
        SEND_EVENT_WITH_2_U32_DATA( TD_EVENT_VALVE_POS_CHANGE, (U32)commandValvePos[ valve ], (U32)chgSteps );
        // calculate and record new commanded valve position
        commandValvePos[ valve ] = getValveEncoderPosition( valve ) + chgSteps;
        // give FPGA new commanded valve position
        if ( H1_VALV == valve )
        {
            setH1Position( commandValvePos[ valve ] );
        }
        else
        {
            setH19Position( commandValvePos[ valve ] );
        }
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_VALVES_INVALID_VALVE1, (U32)valve )
    }
}

/*********************************************************************//**
 * @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)getS32OverrideValue( &currentValveEncPosition[ valve ] );
    }
    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 = getU08OverrideValue( &valveStatus[ valve ] );
    }
    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;
}

/**@}*/