/**************************************************************************
*
* 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.

#define H4_ROT_GEAR_RATIO       64.0F                                           ///< Blood pump rotor to motor gear ratio.
#define H4_ROT_PERIOD_SEC       0.001F                                          ///< Blood pump rotor feedback period in seconds.
#define H4_ROT_PERIOD_TO_HZ(p)  ( 1.0F / (F32)((S32)(p) * H4_ROT_PERIOD_SEC) )  ///< Blood pump rotor period to Hz conversion macro.
#define H4_ROT_PERIOD_TO_RPM(p) ((H4_ROT_PERIOD_TO_HZ(p)) * (F32)SEC_PER_MIN)   ///< Blood pump rotor period to RPM conversion macro.

#define H4_HOME_SPEED_RPM       400                                             ///< Blood pump set speed for rotor homing operation.

#define H6_STATUS_HOME_BIT_POS  0x1                                             ///< Bit position in H6 status register that indicates whether rotor is in home position.

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

static S32 pumpSetSpeedRPM;                                                     ///< Current set speed for the pump (in RPM).  Negative indicates reverse direction.
static F32 pumpMeasSpeedRPM;                                                    ///< Latest measured pump speed (in RPM).
static F32 pumpMeasRotSpeedRPM;                                                 ///< Latest measured pump rotor speed (in RPM).
static BOOL pumpHomeInProgress;                                                 ///< Flag indicates a pump home operation is in progress.

// ********** 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)
{
    pumpHomeInProgress = FALSE;
    pumpSetSpeedRPM = 0;
    pumpMeasSpeedRPM = 0.0F;
    pumpMeasRotSpeedRPM = 0.0F;

    setH4Direction( MOTOR_DIR_FORWARD );
    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, pumpHomeInProgress, FPGA
 * @details \b Outputs: pumpMeasSpeedRPM, pumpHomeInProgress
 * @return none
 *************************************************************************/
void readPeristalticPumps( void )
{
    U16 period = getH4Period();
    S16 rotPer = getH4RotorCount();

    // update measured pump motor speed
    if ( period != H4_ZERO_SPEED_PERIOD )
    {
        F32 Hz  = H4_PERIOD_TO_HZ( period );
        F32 rpm = H4_HZ_TO_RPM( Hz );

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

    // update measured pump rotor speed
    if ( rotPer != 0 )
    {
        pumpMeasRotSpeedRPM = H4_ROT_PERIOD_TO_RPM( rotPer );
    }
    else
    {
        pumpMeasRotSpeedRPM = 0.0F;
    }

    // monitor home command status if in progress, stop pump if rotor is in home position
    if ( ( TRUE == pumpHomeInProgress ) && ( TRUE == isPeristalticPumpHome() ) )
    {
        pumpHomeInProgress = FALSE;
        setPeristalticPumpSetSpeed( 0 );
    }
}

/*********************************************************************//**
 * @brief
 * The cmdPeristalticPumpHome function initiates a pump home operation.
 * @details \b Inputs: none
 * @details \b Outputs: pumpHomeInProgress, FPGA
 * @return TRUE if home operation is initiated, FALSE if not
 *************************************************************************/
BOOL cmdPeristalticPumpHome( void )
{
    BOOL result = FALSE;

    // Pump must be stopped before homing
    if ( ( 0 == pumpSetSpeedRPM ) && ( pumpHomeInProgress != TRUE ) )
    {
        result = TRUE;
        pumpHomeInProgress = TRUE;
        setPeristalticPumpSetSpeed( H4_HOME_SPEED_RPM );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The isPeristalticPumpHome function determines whether the rotor is currently
 * in the home position.
 * @details \b Inputs: FPGA
 * @details \b Outputs: none
 * @return none
 *************************************************************************/
BOOL isPeristalticPumpHome( void )
{
    U08  status = getH6Status() & H6_STATUS_HOME_BIT_POS;
    BOOL result = ( status != 0 ? TRUE : FALSE );

    return result;
}

/*********************************************************************//**
 * @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 )
{
    return pumpMeasSpeedRPM;
}

/*********************************************************************//**
 * @brief
 * The getPeristalticPumpMeasRotorSpeed function gets the latest measured pump
 * rotor speed in RPM.
 * @note A negative speed indicates reverse direction.
 * @details \b Inputs: pumpMeasRotSpeedRPM
 * @details \b Outputs: none
 * @return Latest measured pump rotor speed in RPM.
 *************************************************************************/
F32 getPeristalticPumpMeasRotorSpeed( void )
{
    return pumpMeasRotSpeedRPM;
}

/**@}*/