/**************************************************************************
*
* Copyright (c) 2019-2020 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 LoadCell.c
*
* @author (last) Quang Nguyen
* @date (last) 26-Aug-2020
*
* @author (original) Saeed Nejatali
* @date (original) 25-Feb-2020
*
***************************************************************************/

#include "FPGA.h"
#include "LoadCell.h"
#include "SystemCommMessages.h"
#include "TaskPriority.h"

/**
 *  @addtogroup LoadCells
 *  @{
 */

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

#define LOAD_CELL_REPORT_PERIOD         (100 / TASK_PRIORITY_INTERVAL)                  ///< Broadcast load cell values message every 100 ms.
/// Conversion factor from ADC counts to grams.
static const F32 ADC2GRAM = (0.0894 * 1.1338);
#define LOAD_CELL_ZERO_OFFSET           -1500.0                                         ///< Zero offset (in grams).  TODO - right now, this is empty reservoir weight.
#define LOAD_CELL_FILTER_ALPHA          0.05                                            ///< Alpha factor for the alpha filter used on load cell readings.

#define SIZE_OF_SMALL_LOAD_CELL_AVG     100                                             ///< Small load cell moving average has 100 raw samples @ 10ms intervals (1-second).
#define SIZE_OF_LARGE_LOAD_CELL_AVG     40                                              ///< Large load cell moving average has 40 samples from small filter @ 100ms intervals (4-second).

/// Load cell data structure.
typedef struct
{
    U32 rawReading;                                                 ///< Latest raw load cell reading
    OVERRIDE_F32_T weight;                                          ///< Latest load cell weight
    F32 autoCalOffset;                                              ///< Load cell auto-calibration offset

    F32 smallFilterReadings[ SIZE_OF_SMALL_LOAD_CELL_AVG ];         ///< Load cell samples for small load cell moving average
    F32 smallFilterTotal;                                           ///< Small filter rolling total - used to calc small load cell moving average
    F32 smallFilteredWeight;                                        ///< Load cell small filtered (100 100Hz raw sample) weight

    F32 largeFilterReadings[ SIZE_OF_LARGE_LOAD_CELL_AVG ];         ///< Load cell samples for large load cell moving average
    F32 largeFilterTotal;                                           ///< Large filter rolling total - used to calc small load cell moving average
    F32 largeFilteredWeight;                                        ///< Load cell large filtered (40 10Hz filtered sample) weight
} LOADCELL_T;

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

static OVERRIDE_U32_T loadCellDataPublishInterval = { LOAD_CELL_REPORT_PERIOD,
                                                      LOAD_CELL_REPORT_PERIOD, 0, 0 };  ///< Broadcast load cell data publish interval.
static LOADCELL_T loadcells[ NUM_OF_LOAD_CELLS ];                                       ///< Load cell data structures.
static U32 loadCellFilterTimerCount = 0;                                                ///< Load cell filtering timer count.
static U32 loadCellDataPublicationTimerCounter = 0;                                     ///< Load cell data publication timer counter to CAN bus.

static U32 smallReadingsIdx;                                                            ///< Index for next sample in load cell small rolling average sample array.
static U32 largeReadingsIdx;                                                            ///< Index for next sample in load cell large rolling average sample array.

// TODO - gain and offset for load cells should be read from NV Data calibration record.

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

static U32 getLoadCellDataPublishInterval( void );

/*********************************************************************//**
 * @brief
 * The initLoadCell function initializes the LoadCell module.
 * @details Inputs: none
 * @details Outputs: LoadCell module initialized.
 * @return none
 *************************************************************************/
void initLoadCell( void )
{
    U32 i;
    U32 j;

    smallReadingsIdx = 0;
    largeReadingsIdx = 0;

    for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ )
    {
        loadcells[ i ].rawReading = 0;

        loadcells[ i ].weight.data = 0.0;
        loadcells[ i ].weight.ovData = 0.0;
        loadcells[ i ].weight.ovInitData = 0.0;
        loadcells[ i ].weight.override = OVERRIDE_RESET;

        loadcells[ i ].autoCalOffset      = 0.0;

        loadcells[ i ].largeFilterTotal = 0.0;
        loadcells[ i ].largeFilteredWeight = 0.0;

        loadcells[ i ].smallFilterTotal = 0.0;
        loadcells[ i ].smallFilteredWeight = 0.0;

        for ( j = 0; j < SIZE_OF_SMALL_LOAD_CELL_AVG; j++ )
        {
            loadcells[ i ].smallFilterReadings[ j ] = 0.0;
        }

        for ( j = 0; j < SIZE_OF_LARGE_LOAD_CELL_AVG; j++ )
        {
            loadcells[ i ].largeFilterReadings[ j ] = 0.0;
        }
    }
}

/*********************************************************************//**
 * @brief
 * The execLoadCell function gets load cell data from FPGA, applies filters,
 * and advertises them over CAN.
 * @details Inputs: none
 * @details Outputs: Filtered and advertised load cell data.
 * @return none
 *************************************************************************/
void execLoadCell( void )
{
    U32 ii;

    // update sums for load cell average calculations
    loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].rawReading = getFPGALoadCellA1();
    loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].rawReading = getFPGALoadCellA2();
    loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].rawReading = getFPGALoadCellB1();
    loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].rawReading = getFPGALoadCellB2();

    // Rolling average of last 100 raw samples in small filter
    for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii )
    {
        loadcells[ ii ].weight.data = (F32)loadcells[ ii ].rawReading * ADC2GRAM + LOAD_CELL_ZERO_OFFSET - loadcells[ ii ].autoCalOffset;

        // Update small filter with new weight sample
        loadcells[ ii ].smallFilterTotal -= loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ];
        loadcells[ ii ].smallFilterReadings[ smallReadingsIdx ] = getLoadCellWeight( (LOAD_CELL_ID_T)ii );
        loadcells[ ii ].smallFilterTotal += getLoadCellWeight( (LOAD_CELL_ID_T)ii );
        loadcells[ ii ].smallFilteredWeight = loadcells[ ii ].smallFilterTotal / (F32)SIZE_OF_SMALL_LOAD_CELL_AVG;
    }

    smallReadingsIdx = INC_WRAP( smallReadingsIdx, 0, SIZE_OF_SMALL_LOAD_CELL_AVG - 1 );

    // filter every 100ms
    if ( ++loadCellFilterTimerCount >= LOAD_CELL_REPORT_PERIOD )
    {
        for ( ii = 0; ii < NUM_OF_LOAD_CELLS; ++ii )
        {
            // Update large filter with new small filter weight sample
            loadcells[ ii ].largeFilterTotal -= loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ];
            loadcells[ ii ].largeFilterReadings[ largeReadingsIdx ] = loadcells[ ii ].smallFilteredWeight;
            loadcells[ ii ].largeFilterTotal += loadcells[ ii ].smallFilteredWeight;
            loadcells[ ii ].largeFilteredWeight = loadcells[ ii ].largeFilterTotal / (F32)SIZE_OF_LARGE_LOAD_CELL_AVG;
        }

        loadCellFilterTimerCount = 0;
        largeReadingsIdx = INC_WRAP( largeReadingsIdx, 0, SIZE_OF_LARGE_LOAD_CELL_AVG - 1 );
    }

    // broadcast load cell data if we are at scheduled interval.
    if ( ++loadCellDataPublicationTimerCounter >= getLoadCellDataPublishInterval() )
    {
        loadCellDataPublicationTimerCounter = 0;

        // broadcast small filtered load cell data
        broadcastLoadCellData( loadcells[ LOAD_CELL_RESERVOIR_1_PRIMARY ].smallFilteredWeight,
                               loadcells[ LOAD_CELL_RESERVOIR_1_BACKUP ].smallFilteredWeight,
                               loadcells[ LOAD_CELL_RESERVOIR_2_PRIMARY ].smallFilteredWeight,
                               loadcells[ LOAD_CELL_RESERVOIR_2_BACKUP ].smallFilteredWeight );
    }
}

/*********************************************************************//**
 * @brief
 * The tareLoadCell function sets the load cell auto calibration offset
 * for a given load cell ID.
 * @details Inputs: none
 * @details Outputs: load cell autoCalOffset
 * @param loadCellID ID of load cell to set calibration offset for
 *************************************************************************/
void tareLoadCell( LOAD_CELL_ID_T loadCellID )
{
    // Add old auto calibration offset to get back to actual weight value
    loadcells[ loadCellID ].autoCalOffset = ( loadcells[ loadCellID ].smallFilteredWeight + loadcells[ loadCellID ].autoCalOffset );
}

/*********************************************************************//**
 * @brief
 * The resetLoadCellOffset function resets the load cell auto calibration offset
 * to zero for a given load cell ID.
 * @details Inputs: none
 * @details Outputs: load cell autoCalOffset
 * @param loadCellID ID of load cell to set calibration offset for
 *************************************************************************/
void resetLoadCellOffset( LOAD_CELL_ID_T loadCellID )
{
    loadcells[ loadCellID ].autoCalOffset = 0.0;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellWeight function gets the measured load cell weight for
 * a given load cell ID.
 * @details Inputs: load cell weight
 * @details Outputs: none
 * @param loadCellID ID of load cell to get weight for
 * @return the load cell weight for the given load cell ID.
 *************************************************************************/
F32 getLoadCellWeight( LOAD_CELL_ID_T loadCellID )
{
    F32 result = 0;

    if ( loadCellID < NUM_OF_LOAD_CELLS )
    {
        if ( OVERRIDE_KEY == loadcells[ loadCellID ].weight.override )
        {
            result = loadcells[ loadCellID ].weight.ovData;
        }
        else
        {
            result = loadcells[ loadCellID ].weight.data;
        }
    }
    else
    {
        activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellSmallFilteredWeight function gets the small filtered load cell
 * weight for a given load cell ID.
 * @details Inputs: load cell filtered weight
 * @details Outputs: none
 * @param loadCellID ID of load cell to get small filtered weight for
 * @return the small filtered load cell weight for the given load cell ID.
 *************************************************************************/
F32 getLoadCellSmallFilteredWeight( LOAD_CELL_ID_T loadCellID )
{
    F32 result = 0;

    if ( loadCellID < NUM_OF_LOAD_CELLS )
    {
        result = loadcells[ loadCellID ].smallFilteredWeight;
    }
    else
    {
        activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellSmallFilteredWeight function gets the large filtered load cell
 * weight for a given load cell ID.
 * @details Inputs: load cell filtered weight
 * @details Outputs: none
 * @param loadCellID ID of load cell to get large filtered weight for
 * @return the large filtered load cell weight for the given load cell ID.
 *************************************************************************/
F32 getLoadCellLargeFilteredWeight( LOAD_CELL_ID_T loadCellID )
{
    F32 result = 0;

    if ( loadCellID < NUM_OF_LOAD_CELLS )
    {
        result = loadcells[ loadCellID ].largeFilteredWeight;
    }
    else
    {
        activateAlarmNoData( ALARM_ID_DG_SOFTWARE_FAULT );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellDataPublishInterval function gets the load cell data publish interval.
 * @details Inputs: loadCellDataPublishInterval
 * @details Outputs: none
 * @return the current load cell data publication interval (in ms/task interval).
 *************************************************************************/
static U32 getLoadCellDataPublishInterval( void )
{
    U32 result = loadCellDataPublishInterval.data;

    if ( OVERRIDE_KEY == loadCellDataPublishInterval.override )
    {
        result = loadCellDataPublishInterval.ovData;
    }

    return result;
}


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


/*********************************************************************//**
 * @brief
 * The testSetLoadCellOverride function overrides the measured load cell data.
 * @details Inputs: none
 * @details Outputs: load cell filtered weight
 * @param loadCellID ID of the load cell to override
 * @param value override filtered load cell weight
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetLoadCellOverride( U32 loadCellID, F32 value )
{
    BOOL result = FALSE;

    if ( loadCellID < NUM_OF_LOAD_CELLS )
    {
        if ( TRUE == isTestingActivated() )
        {
            result = TRUE;
            loadcells[ loadCellID ].weight.ovData   = value;
            loadcells[ loadCellID ].weight.override = OVERRIDE_KEY;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetLoadCellOverride function resets the override of the load cell.
 * @details Inputs: none
 * @details Outputs: load cell filtered weight
 * @param loadCellID ID of the load cell to override
 * @return TRUE if reset successful, FALSE if not
 *************************************************************************/
BOOL testResetLoadCellOverride( U32 loadCellID )
{
    BOOL result = FALSE;

    if ( loadCellID < NUM_OF_LOAD_CELLS )
    {
        if ( TRUE == isTestingActivated() )
        {
            result = TRUE;
            loadcells[ loadCellID ].weight.override = OVERRIDE_RESET;
            loadcells[ loadCellID ].weight.ovData   = loadcells[ loadCellID ].weight.ovInitData;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetLoadCellDataPublishIntervalOverride function overrides the
 * load cell data publish interval.
 * @details Inputs: none
 * @details Outputs: loadCellDataPublishInterval
 * @param value override load cell data publish interval with (in ms)
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetLoadCellDataPublishIntervalOverride( U32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        U32 intvl = value / TASK_PRIORITY_INTERVAL;

        result = TRUE;
        loadCellDataPublishInterval.ovData = intvl;
        loadCellDataPublishInterval.override = OVERRIDE_KEY;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetLoadCellDataPublishIntervalOverride function resets the override
 * of the load cell data publish interval.
 * @details Inputs: none
 * @details Outputs: loadCellDataPublishInterval
 * @return TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetLoadCellDataPublishIntervalOverride( void )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        result = TRUE;
        loadCellDataPublishInterval.override = OVERRIDE_RESET;
        loadCellDataPublishInterval.ovData = loadCellDataPublishInterval.ovInitData;
    }

    return result;
}

/**@}*/