/**********************************************************************//**
 *
 *  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
 *
 *  @date 25-Feb-2020
 *  @author S. Nejatali
 *
 *  @brief Processing sensor data.
 *
 **************************************************************************/

#include "LoadCell.h"
#include "SystemCommMessages.h"
#include "FPGA.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    LOAD_CELL_REPORT_PERIOD                     ///< Averaging load cell data over the reporting interval.
//#define ADC2GRAM                        (0.0894 / LOAD_CELL_SAMPLES_TO_AVERAGE)   ///< Conversion factor from ADC counts to grams.  Division for averaging is folded into this value.
#define ADC2GRAM                        (0.01183 / LOAD_CELL_SAMPLES_TO_AVERAGE)    ///< Conversion factor from ADC counts to grams.  Division for averaging is folded into this value.

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

static OVERRIDE_U32_T loadCellDataPublishInterval = { LOAD_CELL_REPORT_PERIOD, LOAD_CELL_REPORT_PERIOD, 0, 0 };  ///< Broadcast load cell data interval in ms/task 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 loadCellDataPublicationTimerCounter = 0;                                 ///< used to schedule load cell data publication 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.0;
    }
}

/*********************************************************************//**
 * @brief
 * The execLoadCell function gets load cell data from FPGA and advertises them over CAN.
 * @details
 * Inputs : none
 * Outputs : Advertising load call 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 ];

    // broadcast load cell data if we are at scheduled interval.
    if ( ++loadCellDataPublicationTimerCounter >= getLoadCellDataPublishInterval() )  // TODO - decouple averaging and conversion so that interval can be overridden
    {
        loadCellDataPublicationTimerCounter = 0;
        // calculate load cell average weights
        filteredLoadCellWeights[ LOAD_CELL_A1 ].data = (F32)(measuredLoadCellReadingsSum[ LOAD_CELL_A1 ]) * ADC2GRAM;   // division for averaging folded into ADC2GRAM
        filteredLoadCellWeights[ LOAD_CELL_A2 ].data = (F32)(measuredLoadCellReadingsSum[ LOAD_CELL_A2 ]) * ADC2GRAM;
        filteredLoadCellWeights[ LOAD_CELL_B1 ].data = (F32)(measuredLoadCellReadingsSum[ LOAD_CELL_B1 ]) * ADC2GRAM;
        filteredLoadCellWeights[ LOAD_CELL_B2 ].data = (F32)(measuredLoadCellReadingsSum[ LOAD_CELL_B2 ]) * ADC2GRAM;
        // broadcast load cell data
        broadcastLoadCellData( getLoadCellFilteredWeight( LOAD_CELL_A1 ), getLoadCellFilteredWeight( LOAD_CELL_A2 ),
                               getLoadCellFilteredWeight( LOAD_CELL_B1 ), getLoadCellFilteredWeight( LOAD_CELL_B2 ) );
        // 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;
    }
}

/*************************************************************************
 * @brief
 * The getLoadCellFilteredWeight function gets the measured filtered load cell \n
 * 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.
 *************************************************************************/
U32 getLoadCellFilteredWeight( U32 loadCellID )
{
    U32 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_SOFTWARE_FAULT );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getLoadCellDataPublishInterval function gets the load cell data \n
 * publication 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 \n
 * load cell A1.
 * @details
 * Inputs : none
 * Outputs : filteredLoadCellWeights[]
 * @param value : override filtered load cell weight
 * @param loadCellID : ID of the load cell to override.
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetLoadCellOverride( F32 value, U32 loadCellID )
{
    BOOL result = FALSE;

    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 \n
 * load cell A1.
 * @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 ( TRUE == isTestingActivated() )
    {
        result = TRUE;
        filteredLoadCellWeights[ loadCellID ].override = OVERRIDE_RESET;
        filteredLoadCellWeights[ loadCellID ].ovData = filteredLoadCellWeights[ loadCellID ].ovInitData;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetLoadCellDataPublishIntervalOverride function overrides the \n
 * 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 \n
 * 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;
}