/**************************************************************************
*
* 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    PeristalticPump.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 "PeristalticPump.h"

/**
 *  @addtogroup PeristalticPump
 *  @{
 */

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

#define MAX_PUMP_SPEED_RPM      3080U                                           ///< Maximum speed (in RPM) that a peristaltic pump can be set to.
#define H4_PUMP_CMD_GAIN        0.9F                                            ///< Gain for blood pump commanded RPM that must be applied before sending to FPGA.
#define H4_PUMP_CMD_OFFSET      80.0F                                           ///< Offset for blood pump commanded RPM that must be applied before sending to FPGA.
#define H4_PUMP_CMD_CAL(r)      ((r) * H4_PUMP_CMD_GAIN + H4_PUMP_CMD_OFFSET )  ///< Macro to calibrate a commanded blood pump RPM before sending to FPGA.

#define H4_PERIOD_SEC           0.00001F                                        ///< Blood pump feedback period in seconds.
#define H4_HZ_TO_RPM_SCALAR     10.0F                                           ///< Blood pump Hz to RPM scalar.
#define H4_PERIOD_TO_HZ(p)      ( 1.0F / (F32)((S32)(p) * H4_PERIOD_SEC) )      ///< Blood pump period to Hz conversion macro.
#define H4_HZ_TO_RPM(h)         ((h) * H4_HZ_TO_RPM_SCALAR)                     ///< Blood pump Hz to RPM conversion macro.
#define H4_ZERO_SPEED_PERIOD    0xFFFF                                          ///< Blood pump period reported when pump is stopped.

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

static S32 pumpSetSpeedRPM;                                                     ///< Current set speed for the pump (in RPM).  Negative indicates reverse direction.
static OVERRIDE_F32_T pumpMeasSpeedRPM;                                         ///< Latest measured pump speed (in RPM).
static BOOL pumpHomeRequested;                                                  ///< Flag indicates a pump home operation has been requested.

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

/*********************************************************************//**
 * @brief
 * The initPeristalticPumpDriver function initializes the peristaltic pump
 * driver unit.
 * @details \b Inputs: none
 * @details \b Outputs: Peristaltic pump driver unit initialized.
 * @return none
 *************************************************************************/
void initPeristalticPumpDriver(void)
{
    pumpHomeRequested = FALSE;
    pumpSetSpeedRPM = 0;
    pumpMeasSpeedRPM.data = 0.0F;
    pumpMeasSpeedRPM.ovData = 0.0F;
    pumpMeasSpeedRPM.ovInitData = 0.0F;
    pumpMeasSpeedRPM.override = OVERRIDE_RESET;

    setH4Direction( MOTOR_DIR_FORWARD );
    resetH4HomeRequest();
    setH4SetSpeed( 0 );
    setH4Enabled( TRUE );
}

/*********************************************************************//**
 * @brief
 * The readPeristalticPumps function gets the current readings for the
 * peristaltic pump from the FPGA.
 * @note This function should be called periodically to maintain fresh
 * sensor readings for the pump.
 * @details \b Inputs: pumpSetSpeedRPM, pumpHomeRequested, FPGA
 * @details \b Outputs: pumpMeasSpeedRPM, pumpHomeRequested
 * @return none
 *************************************************************************/
void readPeristalticPumps( void )
{
    U16 period = getH4Period();
    F32 Hz     = H4_PERIOD_TO_HZ(period);
    F32 rpm    = H4_HZ_TO_RPM(Hz);

    // update measured pump speed
    if ( period != H4_ZERO_SPEED_PERIOD )
    {
        if ( pumpSetSpeedRPM < 0 )  // TODO - can we get a real measured direction to base measured speed sign on?
        {
            pumpMeasSpeedRPM.data = rpm * -1.0F;
        }
        else
        {
            pumpMeasSpeedRPM.data = rpm;
        }
    }
    else
    {
        pumpMeasSpeedRPM.data = 0.0F;
    }

    // clear home command if previously requested
    if ( TRUE == pumpHomeRequested )
    {
        pumpHomeRequested = FALSE;
        resetH4HomeRequest();
    }
}

/*********************************************************************//**
 * @brief
 * The cmdPeristalticPumpHome function initiates a pump home operation.
 * @details \b Inputs: none
 * @details \b Outputs: pumpHomeRequested, FPGA
 * @return none
 *************************************************************************/
void cmdPeristalticPumpHome( void )
{
    pumpHomeRequested = TRUE;
    homeH4();
}

/*********************************************************************//**
 * @brief
 * The setPeristalticPumpSetSpeed function sets the target pump speed (in RPM).
 * @note A negative set speed indicates reverse (CCW) direction.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given set speed is out
 * of range.
 * @details \b Inputs: none
 * @details \b Outputs: pumpSetSpeedRPM
 * @param rpm Speed (in RPM) to set as pump target speed.
 * @return TRUE if set speed command successful, FALSE if not.
 *************************************************************************/
BOOL setPeristalticPumpSetSpeed( S32 rpm )
{
    BOOL result = FALSE;

    if ( abs( rpm ) < MAX_PUMP_SPEED_RPM )
    {
        F32 calRpm      = H4_PUMP_CMD_CAL( (F32)rpm );
        U16 setRpm      = (U16)( abs( (S32)calRpm ) );
        MOTOR_DIR_T dir = MOTOR_DIR_FORWARD;

        if ( rpm < 0 )
        {
            dir = MOTOR_DIR_REVERSE;
        }
        pumpSetSpeedRPM = rpm;
        setH4Direction( dir );
        setH4SetSpeed( setRpm );
        result = TRUE;
    }
    else
    {
        SET_ALARM_WITH_2_F32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, (F32)SW_FAULT_ID_PERISTALTIC_PUMP_SET_SPEED_OUT_OF_RANGE, rpm )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getPeristalticPumpSetSpeed function gets the current pump set
 * speed in RPM.
 * @note A negative speed indicates reverse direction.
 * @details \b Inputs: pumpSetSpeedRPM
 * @details \b Outputs: none
 * @return Latest pump set speed in RPM.
 *************************************************************************/
S32 getPeristalticPumpSetSpeed( void )
{
   return pumpSetSpeedRPM;
}

/*********************************************************************//**
 * @brief
 * The getPeristalticPumpMeasSpeed function gets the latest measured pump
 * speed in RPM.
 * @note A negative speed indicates reverse direction.
 * @details \b Inputs: pumpMeasSpeedRPM
 * @details \b Outputs: none
 * @return Latest measured pump speed in RPM.
 *************************************************************************/
F32 getPeristalticPumpMeasSpeed( void )
{
    F32 result = getF32OverrideValue( &pumpMeasSpeedRPM );

    return result;
}

/**@}*/