/**************************************************************************
*
* 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) Sean Nash
* @date (last) 19-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.
#define LOAD_CELL_SAMPLES_TO_AVERAGE    (100 / TASK_PRIORITY_INTERVAL)                  ///< Averaging load cell data over the reporting interval.
#define LOAD_CELL_AVERAGE_MULTIPLIER    (1.0 / (F32)LOAD_CELL_SAMPLES_TO_AVERAGE)       ///< Optimization - multiplying is faster than dividing.
// TODO - gain and offset for load cells should be read from NV Data calibration record.
#define ADC2GRAM                        (0.0894 * 1.1338)                               ///< Conversion factor from ADC counts to grams.
#define LOAD_CELL_ZERO_OFFSET           -1215.0                                         ///< Zero offset (in grams).  TODO - right now, this is empty reservoir weight.

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

static OVERRIDE_U32_T loadCellDataPublishInterval = { LOAD_CELL_REPORT_PERIOD,
                                                      LOAD_CELL_REPORT_PERIOD, 0, 0 };  ///< Broadcast load cell data publish interval.
static U32 measuredLoadCellReadingsRaw[ NUM_OF_LOAD_CELLS ];                            ///< Latest measured raw load cell readings.
static U32 measuredLoadCellReadingsSum[ NUM_OF_LOAD_CELLS ];                            ///< Raw load cell sums for averaging.
static OVERRIDE_F32_T filteredLoadCellWeights[ NUM_OF_LOAD_CELLS ];                     ///< Latest filtered load cell weights.
static U32 loadCellFilterTimerCount = 0;                                                ///< Load cell filtering timer count.
static U32 loadCellDataPublicationTimerCounter = 0;                                     ///< Load cell data publication timer counter to CAN bus.

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

static U32 getLoadCellDataPublishInterval( void );

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

    for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ )
    {
        filteredLoadCellWeights[ i ].data = 0.0;
        filteredLoadCellWeights[ i ].ovData = 0.0;
        filteredLoadCellWeights[ i ].ovInitData = 0.0;
        filteredLoadCellWeights[ i ].override = OVERRIDE_RESET;

        measuredLoadCellReadingsSum[ i ] = 0;
        measuredLoadCellReadingsRaw[ i ] = 0;
    }
}

/*********************************************************************//**
 * @brief
 * The execLoadCell function gets load cell data from FPGA, applies filters,
 * and advertises them over CAN.
 * @details
 * Inputs : none
 * Outputs : Filtered and advertised load cell data.
 * @return none
 *************************************************************************/
void execLoadCell( void )
{
    // get latest raw load cell readings
    measuredLoadCellReadingsRaw[ LOAD_CELL_A1 ] = getFPGALoadCellA1();
    measuredLoadCellReadingsRaw[ LOAD_CELL_A2 ] = getFPGALoadCellA2();
    measuredLoadCellReadingsRaw[ LOAD_CELL_B1 ] = getFPGALoadCellB1();
    measuredLoadCellReadingsRaw[ LOAD_CELL_B2 ] = getFPGALoadCellB2();
    // update sums for load cell average calculations
    measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_A1 ];
    measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_A2 ];
    measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_B1 ];
    measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] += measuredLoadCellReadingsRaw[ LOAD_CELL_B2 ];

    // filter every 100ms
    if ( ++loadCellFilterTimerCount >= LOAD_CELL_SAMPLES_TO_AVERAGE )
    {
        loadCellFilterTimerCount = 0;

        // calculate load cell average weights
        filteredLoadCellWeights[ LOAD_CELL_A1 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] *
                LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET;
        filteredLoadCellWeights[ LOAD_CELL_A2 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] *
                LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET;
        filteredLoadCellWeights[ LOAD_CELL_B1 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] *
                LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET;
        filteredLoadCellWeights[ LOAD_CELL_B2 ].data = (F32)measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] *
                LOAD_CELL_AVERAGE_MULTIPLIER * ADC2GRAM + LOAD_CELL_ZERO_OFFSET;

        // reset sums for next averaging
        measuredLoadCellReadingsSum[ LOAD_CELL_A1 ] = 0;
        measuredLoadCellReadingsSum[ LOAD_CELL_A2 ] = 0;
        measuredLoadCellReadingsSum[ LOAD_CELL_B1 ] = 0;
        measuredLoadCellReadingsSum[ LOAD_CELL_B2 ] = 0;
    }

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

        // broadcast load cell data
        broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_A1 ), getLoadCellFilteredWeight( LOAD_CELL_A2 ),
                               getLoadCellFilteredWeight( LOAD_CELL_B1 ), getLoadCellFilteredWeight( LOAD_CELL_B2 ) );
    }
}

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

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellDataPublishInterval function gets the load cell data publish interval.
 * @details
 * Inputs : loadCellDataPublishInterval
 * 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
 * Outputs : filteredLoadCellWeights[]
 * @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;
            filteredLoadCellWeights[ loadCellID ].ovData   = value;
            filteredLoadCellWeights[ loadCellID ].override = OVERRIDE_KEY;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetLoadCellOverride function resets the override of the load cell.
 * @details
 * Inputs : none
 * Outputs : filteredLoadCellWeights[]
 * @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;
            filteredLoadCellWeights[ loadCellID ].override = OVERRIDE_RESET;
            filteredLoadCellWeights[ loadCellID ].ovData   = filteredLoadCellWeights[ loadCellID ].ovInitData;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetLoadCellDataPublishIntervalOverride function overrides the
 * load cell data publish interval.
 * @details
 * Inputs : none
 * 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
 * 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;
}

/**@}*/