/**************************************************************************
*
* 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    PressureSensor.c
*
* @author  (last)      Sean
* @date    (last)      22-Aug-2024
*
* @author  (original)  Sean
* @date    (original)  22-Aug-2024
*
***************************************************************************/

#include "FpgaTD.h"
#include "Messaging.h"
#include "PersistentAlarm.h"
#include "PressureCommon.h"
#include "PressureSensor.h"
#include "Utilities.h"

/**
 *  @addtogroup PressureSensor
 *  @{
 */

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

#define PRES_SENSORS_ZERO_OFFSET                    ( 1638.0F )                             ///< Zero offset for pressure sensor readings.
#define PRES_SENSORS_DIVISOR                        ( 14745.0F - PRES_SENSORS_ZERO_OFFSET ) ///< Divisor for pressure sensor conversion from counts to bars.
#define PRESSURE_MIN_PSI                            ( -30.0F )                              ///< Minimum of scale for pressure sensor reading (in PSI).
#define PRESSURE_MAX_PSI                            ( 30.0F )                               ///< Maximum of scale for pressure sensor reading (in PSI).
#define PSI_TO_MMHG                                 ( 51.7149F )                            ///< Conversion factor for converting PSI to mmHg.

#define PRES_SENSORS_COUNT_ERROR_TIMEOUT_MS         ( 2 * MS_PER_SECOND )                   ///< Pressure sensors read and error count timeout in milliseconds.

#define FPGA_PRESSURE_READING_BIT_COUNT             14                                      ///< Number of bits in a pressure reading from the FPGA.
#define FPGA_PRESSURE_STATUS_BITS_MASK              0xC000                                  ///< Bit mask for status bits of pressure reading register.
#define FPGA_PRESSURE_READING_BITS_MASK             0x3FFF                                  ///< Bit mask for pressure bits of pressure reading register.

#define PRESSURE_NORMAL_OP                          0                                       ///< Pressure status bits indicate normal operation.
#define PRESSURE_CMD_MODE                           1                                       ///< Pressure status bits indicate sensor in command mode.
#define PRESSURE_STALE_DATA                         2                                       ///< Pressure status bits indicate data is stale (no new data since last fpga read).
#define PRESSURE_DIAG_CONDITION                     3                                       ///< Pressure status bits diagnostic condition (alarm).

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

static OVERRIDE_F32_T currentPressureReadings[ NUM_OF_PRESSURE_SENSORS ];                   ///< Current pressure sensor pressure readings (overrideable).
static OVERRIDE_F32_T currentPresTempReadings[ NUM_OF_PRESSURE_SENSORS ];                   ///< Current pressure sensor temperature readings (overrideable).
static OVERRIDE_U32_T lastPressureReadCounter[ NUM_OF_PRESSURE_SENSORS ];                   ///< Last pressure sensor read count (Overrideable).
static OVERRIDE_U32_T lastPressureErrorCounter[ NUM_OF_PRESSURE_SENSORS ];                  ///< Last pressure sensor error count (Overrideable).
static U32 currentPressureStatus[ NUM_OF_PRESSURE_SENSORS ];                                ///< Current status of pressure sensors.

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

static void checkPressureSensors( void );
static F32 convertPressureRdg2mmHg( U16 counts );
static U32 getPressureStatusFromFPGARegReading( U16 fpgaReg );

/*********************************************************************//**
 * @brief
 * The initPressureSensor function initializes the Pressure Sensor unit.
 * @details \b Inputs: none
 * @details \b Outputs: Pressure Sensor unit is initialized.
 * @return none
 *************************************************************************/
void initPressureSensor( void )
{
    U32 i;

    // Initialize override structures for each pressure sensor
    for ( i = 0; i < (U32)NUM_OF_PRESSURE_SENSORS; i++ )
    {
        currentPressureReadings[ i ].data = 0.0F;
        currentPressureReadings[ i ].ovData = 0.0F;
        currentPressureReadings[ i ].ovInitData = 0.0F;
        currentPressureReadings[ i ].override = OVERRIDE_RESET;

        currentPresTempReadings[ i ].data = 0.0F;
        currentPresTempReadings[ i ].ovData = 0.0F;
        currentPresTempReadings[ i ].ovInitData = 0.0F;
        currentPresTempReadings[ i ].override = OVERRIDE_RESET;

        lastPressureReadCounter[ i ].data = 0;
        lastPressureReadCounter[ i ].ovData = 0;
        lastPressureReadCounter[ i ].ovInitData = 0;
        lastPressureReadCounter[ i ].override = OVERRIDE_RESET;

        lastPressureErrorCounter[ i ].data = 0;
        lastPressureErrorCounter[ i ].ovData = 0;
        lastPressureErrorCounter[ i ].ovInitData = 0;
        lastPressureErrorCounter[ i ].override = OVERRIDE_RESET;

        currentPressureStatus[ i ] = PRESSURE_STALE_DATA;
    }

    // Initialize the FPGA persistent alarms
    initFPGAPersistentAlarm( FPGA_PERS_ERROR_ARTERIAL_PRESSURE_SENSOR, ALARM_ID_TD_ARTERIAL_SENSOR_TIMEOUT_FAULT,
                             PRES_SENSORS_COUNT_ERROR_TIMEOUT_MS, PRES_SENSORS_COUNT_ERROR_TIMEOUT_MS );
    initFPGAPersistentAlarm( FPGA_PERS_ERROR_VENOUS_PRESSURE_SESNOR, ALARM_ID_TD_VENOUS_SENSOR_TIMEOUT_FAULT,
                             PRES_SENSORS_COUNT_ERROR_TIMEOUT_MS, PRES_SENSORS_COUNT_ERROR_TIMEOUT_MS );
}

/*********************************************************************//**
 * @brief
 * The readPressureSensors function gets the current pressure reading
 * for a all pressure sensors from the FPGA and also reads the freshness
 * and error counters to verify that the pressure sensors are being read
 * by the FPGA without issue.
 * @note This function should be called periodically to maintain fresh
 * sensor readings for all pressure sensors.
 * @details \b Inputs: FPGA
 * @details \b Outputs: none
 * @return none
 *************************************************************************/
void readPressureSensors( void )
{
    U16 h2 = getH2Pressure();
    U16 h14 = getH14Pressure();

    // Update status of pressure sensors
    currentPressureStatus[ H2_PRES ] = getPressureStatusFromFPGARegReading( h2 );
    currentPressureStatus[ H14_PRES ] = getPressureStatusFromFPGARegReading( h14 );

    // Update and convert raw pressures to mmHg
    currentPressureReadings[ H2_PRES ].data = convertPressureRdg2mmHg( h2 );
    currentPressureReadings[ H14_PRES ].data   = convertPressureRdg2mmHg( h14 );

    // Update and convert raw pressure sensor temperatures to deg C
    currentPresTempReadings[ H2_PRES ].data = convertPressureTempReading2DegC( getH2Temperature() );
    currentPresTempReadings[ H14_PRES ].data = convertPressureTempReading2DegC( getH14Temperature() );

    // Update read and error counters for each pressure sensor
    lastPressureReadCounter[ H2_PRES ].data = (U32)getH2ReadCounter();
    lastPressureReadCounter[ H14_PRES ].data = (U32)getH14ReadCounter();
    lastPressureErrorCounter[ H2_PRES ].data = (U32)getH2ErrorCounter();
    lastPressureErrorCounter[ H14_PRES ].data = (U32)getH14ErrorCounter();

    // Monitor pressure sensor health
    checkPressureSensors();
}

/*********************************************************************//**
 * @brief
 * The convertPressureRdg2mmHg function converts a raw pressure count from
 * the FPGA and converts it to mmHg.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @param counts the raw pressure reading in counts from the FPGA
 * @return the pressure in mmHg
 *************************************************************************/
static F32 convertPressureRdg2mmHg( U16 counts )
{
    S16 rdg  = (S16)( counts & FPGA_PRESSURE_READING_BITS_MASK );
    F32 presPSI;
    F32 mmHg;

    // If the arterial pressure status is normal, convert the counts to pressure in mmHg
    presPSI = ( ( (F32)rdg - PRES_SENSORS_ZERO_OFFSET ) *( PRESSURE_MAX_PSI - PRESSURE_MIN_PSI ) / PRES_SENSORS_DIVISOR ) + PRESSURE_MIN_PSI;
    mmHg    = presPSI * PSI_TO_MMHG;

    return mmHg;
}

/*********************************************************************//**
 * @brief
 * The getPressureStatusFromFPGARegReading function extracts the status
 * from the FPGA register reading.
 * @details \b Inputs: none
 * @details \b Outputs: none
 * @param fpgaReg the value read from the FPGA register
 * @return the status portion of the FPGA register value
 *************************************************************************/
static U32 getPressureStatusFromFPGARegReading( U16 fpgaReg )
{
    U16 rdg    = fpgaReg & FPGA_PRESSURE_STATUS_BITS_MASK;                  // mask off reading bits
    U32 result = rdg >> FPGA_PRESSURE_READING_BIT_COUNT;                    // shift status bits to lsb

    return result;
}

/*********************************************************************//**
 * @brief
 * The checkPressureSensors function checks the read and error counters for
 * each pressure sensor.
 * @details \b Alarm: ALARM_ID_TD_ARTERIAL_SENSOR_TIMEOUT_FAULT if the
 * arterial pressure sensor is not able to be read.
 * @details \b Alarm: ALARM_ID_TD_VENOUS_SENSOR_TIMEOUT_FAULT if the
 * venous pressure sensor is not able to be read.
 * @details \b Inputs: lastPressureReadCounter, lastPressureErrorCounter
 * @details \b Outputs: none
 * @return none
 *************************************************************************/
static void checkPressureSensors( void )
{
    checkFPGAPersistentAlarms( FPGA_PERS_ERROR_ARTERIAL_PRESSURE_SENSOR, getPressureSensorReadCount( H2_PRES ) );
    checkFPGAPersistentAlarms( FPGA_PERS_ERROR_VENOUS_PRESSURE_SESNOR, getPressureSensorReadCount( H14_PRES ) );
    checkFPGAPersistentErrorCountAlarm( FPGA_PERS_ERROR_ARTERIAL_PRESSURE_SENSOR, getPressureSensorErrorCount( H2_PRES ) );
    checkFPGAPersistentErrorCountAlarm( FPGA_PERS_ERROR_VENOUS_PRESSURE_SESNOR, getPressureSensorErrorCount( H14_PRES ) );

    // verify status of pressure sensors
    if ( currentPressureStatus[ H2_PRES ] != PRESSURE_NORMAL_OP )
    {
        // TODO - alarm?
    }
    if ( currentPressureStatus[ H14_PRES ] != PRESSURE_NORMAL_OP )
    {
        // TODO - alarm?
    }
}

/*********************************************************************//**
 * @brief
 * The getPressure function gets the current pressure (in mmHg) for a given
 * pressure sensor.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given sensor is invalid.
 * @details \b Inputs: currentPressureReadings
 * @details \b Outputs: none
 * @param sensor ID of pressure sensor to get pressure reading for.
 * @return The current pressure (in mmHg) of the given pressure sensor.
 *************************************************************************/
F32 getPressure( PRESSURE_SENSORS_T sensor )
{
    F32 result = 0.0F;

    if ( sensor < NUM_OF_PRESSURE_SENSORS )
    {
        result = getF32OverrideValue( &currentPressureReadings[ sensor ] );
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PRESSURE_SENSOR_INVALID_SENSOR1, sensor )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getPressureSensorTemperature function gets the current pressure sensor
 * temperature (in deg C) for a given pressure sensor.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given sensor is invalid.
 * @details \b Inputs: currentPresTempReadings
 * @details \b Outputs: none
 * @param sensor ID of pressure sensor to get temperature reading for.
 * @return The current pressure sensor temperature (in deg C) of the given pressure sensor.
 *************************************************************************/
F32 getPressureSensorTemperature( PRESSURE_SENSORS_T sensor )
{
    F32 result = 0.0F;

    if ( sensor < NUM_OF_PRESSURE_SENSORS )
    {
        result = getF32OverrideValue( &currentPresTempReadings[ sensor ] );
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PRESSURE_SENSOR_INVALID_SENSOR2, sensor )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getPressureSensorReadCount function gets the current pressure sensor
 * read count for a given pressure sensor.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given sensor is invalid.
 * @details \b Inputs: lastPressureReadCounter
 * @details \b Outputs: none
 * @param sensor ID of pressure sensor to get read count for.
 * @return The current pressure sensor read count of a given pressure sensor.
 *************************************************************************/
U32 getPressureSensorReadCount( PRESSURE_SENSORS_T sensor )
{
    U32 result = 0;

    if ( sensor < NUM_OF_PRESSURE_SENSORS )
    {
        result = getU32OverrideValue( &lastPressureReadCounter[ sensor ] );
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PRESSURE_SENSOR_INVALID_SENSOR3, sensor )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getPressureSensorErrorCount function gets the current pressure sensor
 * error count for a given pressure sensor.
 * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given sensor is invalid.
 * @details \b Inputs: lastPressureErrorCounter
 * @details \b Outputs: none
 * @param sensor ID of pressure sensor to get error count for.
 * @return The current pressure sensor error count of a given pressure sensor.
 *************************************************************************/
U32 getPressureSensorErrorCount( PRESSURE_SENSORS_T sensor )
{
    U32 result = 0;

    if ( sensor < NUM_OF_PRESSURE_SENSORS )
    {
        result = getU32OverrideValue ( &lastPressureErrorCounter[ sensor ] );
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PRESSURE_SENSOR_INVALID_SENSOR4, sensor )
    }

    return result;
}


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


/*********************************************************************//**
 * @brief
 * The testPressureSensorOverride function overrides the pressure reading
 * for a given pressure sensor.
 * @details \b Inputs: none
 * @details \b Outputs: currentPressureReadings[]
 * @param message Override message from Dialin which includes an ID of
 * the sensor to override and the value to override the sensor to.
 * @return TRUE if override request is successful, FALSE if not
 *************************************************************************/
BOOL testPressureSensorOverride( MESSAGE_T *message )
{
    BOOL result = f32ArrayOverride( message, &currentPressureReadings[0], NUM_OF_PRESSURE_SENSORS - 1 );

    return result;
}

/**@}*/