#include "etpwm.h"

#include "Fans.h"
#include "FPGA.h"
#include "PersistentAlarm.h"
#include "SystemCommMessages.h"
#include "TaskGeneral.h"
#include "Thermistors.h"

/**
 *  @addtogroup Fans
 *  @{
 */

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

#define FANS_MIN_DUTY_CYCLE                          0.1                                           ///< Fans min PWM.
#define FANS_MAX_DUTY_CYCLE                          0.95                                          ///< Fans max PWM.
#define MIN_ALLOWED_AMBIENT_TEMPERATURE              20                                            ///< Min allowed ambient temperature.
#define MAX_ALLOWED_AMBINET_TEMPERATURE              70                                            ///< Max allowed ambient temperature.
#define FANS_MAX_ALLOWED_RAMP_UP_DELTA_DUTY_CYCLE    0.3                                           ///< Fans max allowed ramp up PWM change.
#define FANS_MAX_ALLOWED_RAMP_DOWN_DELTA_DUTY_CYCLE  0.005                                         ///< Fans min allowed ramp down PWM change.

#define TOGGLE_PERIOD_RESOLUTION_SECONDS             0.0000025                                     ///< FPGA fans toggle period resolution in micro seconds.
#define ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION       4                                             ///< FPGA rotational to toggle period conversion coefficient.

#define FANS_DATA_PUBLISH_INTERVAL                   ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )     ///< Fans publish data time interval in counts.
#define FANS_CONTROL_INTERVAL                        ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )     ///< Fans control time interval in counts.
#define FANS_ZERO_RPM_TOGGLE_PERIOD_VALUE            0xFFFF                                        ///< Fans zero RPM toggle period value.
#define MIN_TARGET_RPM_IN_SELF_TEST                  1000                                          ///< Fans min target RPM that they should be during POST.
#define FANS_SELF_TEST_WAIT_INTERVAL                 ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )     ///< Fans self test wait time for the fans to get to RPM.
#define FANS_SELF_TEST_TARGET_PWM                    0.5                                           ///< Fans self test target PWM for testing the fans are running.
#define FANS_MAX_ALLOWED_RPM_OUT_OF_RANGE_INTERVAL   ( 3 * MS_PER_SECOND )                         ///< Fans max allowed RPM out of range time interval.
#define FANS_MAX_ALLOWED_RPM                         5000                                          ///< Fans max allowed RPM value.
#define FANS_MIN_ALLOWED_RPM                         150                                           ///< Fans max allowed RPM value.
#define FANS_MONITOR_INTERVAL_COUNT                  ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )     ///< Fans monitor time interval in counts.

/// Fans self test states
typedef enum fans_Self_Test
{
    FANS_SELF_TEST_START_STATE = 0,   ///< Fans self test start state
    FANS_SELF_TEST_CHECK_RPM_STATE,   ///< Fans self test check RPM state
    FAN_SELF_TEST_COMPLETE_STATE,     ///< Fans self test complete state
    NUM_OF_SELF_TEST_STATES,          ///< Number of fans self test
} FANS_SELF_TEST_STATES_T;

/// Fans exec states
typedef enum fans_Exec_States
{
    FANS_EXEC_STATE_WAIT_FOR_POST_STATE = 0,  ///< Fans exec state start state
    FANS_EXEC_STATE_RUN_STATE,                ///< Fans exec state run state
    NUM_OF_FANS_EXEC_STATES,                  ///< Number of fans exec states
} FANS_EXEC_STATES_T;

/// Fans status struct
typedef struct
{
    F32 targetDutyCycle;               ///< Fan's target duty cycle that was fed to the fans
    U32 rpm[ NUM_OF_FANS_NAMES ];      ///< Fan's current tachometers reading in RPM
} FAN_STATUS_T;

static FAN_STATUS_T fansStatus;                                                      ///< Fans status.
static SELF_TEST_STATUS_T fansSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS;         ///< Fans self test result.
static FANS_SELF_TEST_STATES_T fansSelfTestState = FANS_SELF_TEST_START_STATE;       ///< Fans self test state.
static FANS_EXEC_STATES_T fansExecState = FANS_EXEC_STATE_WAIT_FOR_POST_STATE;       ///< Fans exec state.
static U32 fansControlCounter = 0;                                                   ///< Fans control interval counter.
static U32 fansPublishCounter = 0;                                                   ///< Fans data publish interval counter.
static U32 fansMonitorCounter = 0;                                                   ///< Fans monitor interval counter.

/// Temperature to duty cycle conversion slope (duty cycle not in percent)
static const F32 SLOPE = ( FANS_MAX_DUTY_CYCLE - FANS_MIN_DUTY_CYCLE ) / ( MAX_ALLOWED_AMBINET_TEMPERATURE - MIN_ALLOWED_AMBIENT_TEMPERATURE );

/// FGPA Toggle to RPM conversion coefficient
static const F32 TOGGLE_PERIOD_2_RPM_COEFFICIENT = SEC_PER_MIN / ( TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION );

static OVERRIDE_U32_T fansPublishInterval = { FANS_DATA_PUBLISH_INTERVAL, FANS_DATA_PUBLISH_INTERVAL, 0, 0 };   ///< Fans publish time interval override

static FANS_SELF_TEST_STATES_T handleSelfTestStart( void );
static FANS_SELF_TEST_STATES_T handleSelfTestCheckRPM( void );

static FANS_EXEC_STATES_T handleExecStateWaitForPOST( void );
static FANS_EXEC_STATES_T handleExecStateRun( void );

static void setInletFansDutyCycle( F32 pwm );
static void setOutletFansDutyCycle( F32 pwm );
static F32 getMaximumTemperature( void );
static void convertTogglePeriod2RPM( void );
static void monitorFans( void );
static U32 getPublishFansDataInterval( void );
static void publishFansData( void );

/*********************************************************************//**
 * @brief
 * The initFans function initializes the fans module.
 * @details Inputs: fansExecState, fansSelfTestReslt, fansSelfTestState,
 * fansStatus, fansControlCounter, fansPublishCounter
 * @details Outputs: fansExecState, fansSelfTestReslt, fansSelfTestState,
 * fansStatus, fansControlCounter, fansPublishCounter
 * @return none
 *************************************************************************/
void initFans( void )
{
    // Initialize the variables
    fansExecState      = FANS_EXEC_STATE_WAIT_FOR_POST_STATE;
    fansSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS;
    fansSelfTestState  = FANS_SELF_TEST_START_STATE;
    fansExecState      = FANS_EXEC_STATE_WAIT_FOR_POST_STATE;
    fansControlCounter = 0;
    fansPublishCounter = 0;
    fansMonitorCounter = 0;

    // Initialize a persistent alarm for fans RPM out of range
    initPersistentAlarm( ALARM_ID_DG_FAN_RPM_OUT_OF_RANGE, FANS_MAX_ALLOWED_RPM_OUT_OF_RANGE_INTERVAL, FANS_MAX_ALLOWED_RPM_OUT_OF_RANGE_INTERVAL );
}

/*********************************************************************//**
 * @brief
 * The execFansSelfTest function executes the fans self test.
 * @details Inputs: fansSelfTestState, fansSelfTestReslt
 * @details Outputs: fansSelfTestState, fansSelfTestReslt
 * @return Status of self test
 *************************************************************************/
SELF_TEST_STATUS_T execFansSelfTest( void )
{
    switch ( fansSelfTestState )
    {
        case FANS_SELF_TEST_START_STATE:
            fansSelfTestState = handleSelfTestStart();
            break;

        case FANS_SELF_TEST_CHECK_RPM_STATE:
            fansSelfTestState = handleSelfTestCheckRPM();
            break;

        case FAN_SELF_TEST_COMPLETE_STATE:
            // Done with POST. Do nothing
            break;

        default:
            // Wrong state called
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_FAN_INVALID_SELF_TEST_STATE, fansSelfTestState );
            fansSelfTestState = FAN_SELF_TEST_COMPLETE_STATE;
            break;
    }

    return fansSelfTestResult;
}

/*********************************************************************//**
 * @brief
 * The execFans function executes the execFans exec states.
 * @details Inputs: fansExecState
 * @details Outputs: fansExecState
 * @return none
 *************************************************************************/
void execFans( void )
{
    // Monitor the fans
    monitorFans();

    switch ( fansExecState )
    {
        case FANS_EXEC_STATE_WAIT_FOR_POST_STATE:
            fansExecState = handleExecStateWaitForPOST();
            break;

        case FANS_EXEC_STATE_RUN_STATE:
            fansExecState = handleExecStateRun();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_FAN_INVALID_EXEC_STATE, fansExecState );
            fansExecState = FANS_EXEC_STATE_RUN_STATE;
            break;
    }

    publishFansData();
}

/*********************************************************************//**
 * @brief
 * The getFanRPM function returns the RPM a the selected fan.
 * @details Inputs: fansStatus
 * @details Outputs: none
 * @param selected fan to read its RPM
 * @return RPM of the selected fan
 *************************************************************************/
F32 getMeasuredFanRPM( FAN_NAMES_T fan )
{
    F32 rpm = 0.0;

    // Check if the called fan is in range
    if ( fan < NUM_OF_FANS_NAMES )
    {
        rpm = fansStatus.rpm[ fan ];
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_FAN_SELECTED, fan );
    }

    return rpm;
}

/*********************************************************************//**
 * @brief
 * The handleSelfTestStart function handles the start state of the fans'
 * self test.
 * @details Inputs: none
 * @details Outputs: none
 * @return next state of self test
 *************************************************************************/
static FANS_SELF_TEST_STATES_T handleSelfTestStart( void )
{
    FANS_SELF_TEST_STATES_T state =  FANS_SELF_TEST_CHECK_RPM_STATE;

    // Set the fans to the target PWM for the next stage
    setInletFansDutyCycle( FANS_SELF_TEST_TARGET_PWM );
    setOutletFansDutyCycle( FANS_SELF_TEST_TARGET_PWM );
    state = FANS_SELF_TEST_CHECK_RPM_STATE;

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleSelfTestCheckRPM function handles the check RPM state of the
 * fans' self test.
 * @details Inputs: fansStatus
 * @details Outputs: none
 * @return next state of self test
 *************************************************************************/
static FANS_SELF_TEST_STATES_T handleSelfTestCheckRPM( void )
{
    FANS_SELF_TEST_STATES_T state = FANS_SELF_TEST_CHECK_RPM_STATE;

    FAN_NAMES_T fan;

    // Wait for the fans to run for a certain period of time before checking their RPM
    if ( ++fansControlCounter > FANS_SELF_TEST_WAIT_INTERVAL )
    {
        // Assuming the test will pass
        fansSelfTestResult = SELF_TEST_STATUS_PASSED;

        convertTogglePeriod2RPM();

        // Loop through all the fans to check their RPM. They should be above the specified RPM
        for( fan = FAN_INLET_1; fan < NUM_OF_FANS_NAMES; fan++ )
        {
            if ( fansStatus.rpm[ fan ] < MIN_TARGET_RPM_IN_SELF_TEST )
            {
                SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_FAN_RPM_OUT_OF_RANGE, fan, fansStatus.rpm[ fan ] );

                fansSelfTestResult = SELF_TEST_STATUS_FAILED;
            }
        }

        // Turn off the fans, done with self test
        setInletFansDutyCycle( 0.0 );
        setOutletFansDutyCycle( 0.0 );

        state = FAN_SELF_TEST_COMPLETE_STATE;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleFansExecStateStart function handles the start state of the
 * fans exec state machine.
 * @details Inputs: none
 * @details Outputs: none
 * @return the next state of the exec state machine
 *************************************************************************/
static FANS_EXEC_STATES_T handleExecStateWaitForPOST( void )
{
    FANS_EXEC_STATES_T state = FANS_EXEC_STATE_WAIT_FOR_POST_STATE;

    // Wait for the self test to finish before starting the fans
    if ( fansSelfTestState == FAN_SELF_TEST_COMPLETE_STATE )
    {
        // Start the fans with minimum PWM. The control will decide the next PWM automatically.
        setInletFansDutyCycle( FANS_MIN_DUTY_CYCLE );
        setOutletFansDutyCycle( FANS_MIN_DUTY_CYCLE );
        state = FANS_EXEC_STATE_RUN_STATE;
    }

#ifndef _VECTORCAST_ // To skip the wait for POST and start running.
    // TODO REMOVE
    state = FANS_EXEC_STATE_RUN_STATE;
    // TODO REMOVE
#endif
    return state;
}

/*********************************************************************//**
 * @brief
 * The handleFansExecStateRun function handles the run state of the
 * fans exec state machine.
 * @details Inputs: fansStatus, fansControlCounter
 * @details Outputs: fansStatus, fansControlCounter
 * @return the next state of the exec state machine
 *************************************************************************/
static FANS_EXEC_STATES_T handleExecStateRun( void )
{
    FANS_EXEC_STATES_T state = FANS_EXEC_STATE_RUN_STATE;

    // Check if it is time to check for the control
    if( ++fansControlCounter > FANS_CONTROL_INTERVAL )
    {
        // Get the maximum temperature among all the thermistors and temperature sensors to run fan from the hottest temperature
        F32 temperature = getMaximumTemperature();

        //TODO REMOVE FOR TESTING
        temperature = getThermistorTemperatureValue( THERMISTOR_ONBOARD_NTC );
        //TODO REMOVE FOR TESTING only

        // Solve the linear equation to calculate the duty cycle from temperature
        F32 dutyCycle = ( SLOPE * ( temperature - MIN_ALLOWED_AMBIENT_TEMPERATURE ) ) + FANS_MIN_DUTY_CYCLE;

        // Check whether the duty cycle is not outside of the range and cap it if needed
        if ( dutyCycle < FANS_MIN_DUTY_CYCLE )
        {
            dutyCycle = FANS_MIN_DUTY_CYCLE;
        }
        if ( dutyCycle > FANS_MAX_DUTY_CYCLE )
        {
            dutyCycle = FANS_MAX_DUTY_CYCLE;
        }

        // If the fans calculated duty cycle is greater than the previous calculated duty cycle, we are ramping up
        // otherwise, we are ramping down
        if ( dutyCycle >= fansStatus.targetDutyCycle )
        {
            // If the delta duty cycle from the previous duty cycle is greater than the max allowed ramp up duty cycle,
            // otherwise, only add the delta duty cycle
            if ( ( dutyCycle - fansStatus.targetDutyCycle ) >= FANS_MAX_ALLOWED_RAMP_UP_DELTA_DUTY_CYCLE )
            {
                fansStatus.targetDutyCycle += FANS_MAX_ALLOWED_RAMP_UP_DELTA_DUTY_CYCLE;
            }
            else
            {
                fansStatus.targetDutyCycle = dutyCycle;
            }
        }
        else
        {
            // If the delta duty cycle from the previous duty cycle is greater than the max allowed ramp down duty cycle,
            // otherwise, only add the delta duty cycle
            if ( ( fansStatus.targetDutyCycle - dutyCycle ) >= FANS_MAX_ALLOWED_RAMP_DOWN_DELTA_DUTY_CYCLE )
            {
                // If we are ramping down, set the target duty cycle to max allowed ramp down duty cycle
                fansStatus.targetDutyCycle -= FANS_MAX_ALLOWED_RAMP_DOWN_DELTA_DUTY_CYCLE;
            }
            else
            {
                fansStatus.targetDutyCycle = dutyCycle;
            }
        }

        // Set the PWM to inlet and outlet fans
        setInletFansDutyCycle( fansStatus.targetDutyCycle );
        setOutletFansDutyCycle( fansStatus.targetDutyCycle );

        // Reset the counter
        fansControlCounter = 0;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The setInletFansPWM function sets the inlet fans PWM.
 * @details Inputs: none
 * @details Outputs: none
 * @param PWM that will be set
 * @return none
 *************************************************************************/
static void setInletFansDutyCycle( F32 pwm )
{
    etpwmSetCmpA( etpwmREG6, (U32)( (S32)( ( pwm * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief
 * The setOutletFansPWM function sets the outlet fans PWM.
 * @details Inputs: none
 * @details Outputs: none
 * @param PWM that will be set
 * @return none
 ************************************************************************/
static void setOutletFansDutyCycle( F32 pwm )
{
    etpwmSetCmpB( etpwmREG6, (U32)( (S32)( ( pwm * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief
 * The getMaximumTemperature function runs through the thermistors driver
 * and finds the maximum temperature.
 * @details Inputs: none
 * @details Outputs: none
 * @return maximum temperature of the thermistors and sensors
 ************************************************************************/
static F32 getMaximumTemperature( void )
{
    F32 temperature;
    THERMISTORS_TEMP_SENSORS_T thermistor;
    F32 maxTemperature = 0.0;

    // Loop through the sensors and thermistors
    for ( thermistor = THERMISTOR_ONBOARD_NTC; thermistor < NUM_OF_THERMISTORS; thermistor++ )
    {
        // Get the value
        temperature = getThermistorTemperatureValue( thermistor );

        // If the latest temperature read is greater than the max temperature,
        // set the maximum temperature there
        if ( temperature > maxTemperature )
        {
            maxTemperature = temperature;
        }
    }

    return maxTemperature;
}

/*********************************************************************//**
 * @brief
 * The convertTogglePeriod2RPM function runs through the list of the fans
 * to get the FPGA pulse from them and converts them to RPM.
 * @details Inputs: fansStatus, toggle2RPMCoefficient
 * @details Outputs: fansStatus
 * @return none
 ************************************************************************/
static void convertTogglePeriod2RPM( void )
{
    FAN_NAMES_T fan;
    U32 togglePeriods[ NUM_OF_FANS_NAMES ];

    togglePeriods[ FAN_INLET_1 ]  = getFPGAInletFan1TogglePeriod();
    togglePeriods[ FAN_INLET_2 ]  = getFPGAInletFan2TogglePeriod();
    togglePeriods[ FAN_INLET_3 ]  = getFPGAInletFan3TogglePeriod();
    togglePeriods[ FAN_OUTLET_1 ] = getFPGAOutletFan1TogglePeriod();
    togglePeriods[ FAN_OUTLET_2 ] = getFPGAOutletFan2TogglePeriod();
    togglePeriods[ FAN_OUTLET_3 ] = getFPGAOutletFan3TogglePeriod();

    for ( fan = FAN_INLET_1; fan < NUM_OF_FANS_NAMES; fan++ )
    {
        // If the pulse is close to 0 or 0, FPGA will report 0xFFFF
        // Otherwise, convert the pulse to RPM
        if ( togglePeriods[ fan ] == FANS_ZERO_RPM_TOGGLE_PERIOD_VALUE )
        {
            fansStatus.rpm[ fan ] = 0;
        }
        else
        {
            // Convert toggle period to RPM
            fansStatus.rpm[ fan ] = TOGGLE_PERIOD_2_RPM_COEFFICIENT / togglePeriods[ fan ];
        }
    }
}

/*********************************************************************//**
 * @brief
 * The monitorFans function monitors the fans for RPM.
 * @details Inputs: fansStatus
 * @details Outputs: none
 * @return none
 *************************************************************************/
static void monitorFans( void )
{
    FAN_NAMES_T fan;

    if ( ++fansMonitorCounter >= FANS_MONITOR_INTERVAL_COUNT )
    {
        convertTogglePeriod2RPM();

        /*for ( fan = FAN_INLET_1; fan < NUM_OF_FANS_NAMES; fan++ )
        {
            BOOL const fanRpmOutOfRange = ( fansStatus.rpm[ fan ] >= FANS_MAX_ALLOWED_RPM ) || ( fansStatus.rpm[ fan ] <= FANS_MIN_ALLOWED_RPM );
            checkPersistentAlarm( ALARM_ID_DG_FAN_RPM_OUT_OF_RANGE, fanRpmOutOfRange, fansStatus.rpm[ fan ], FANS_MAX_ALLOWED_RPM );
        }*/

        fansMonitorCounter = 0;
    }
}

/*********************************************************************//**
 * @brief
 * The getPublishFansDataInterval function gets the fans data publish interval.
 * @details Inputs: fansPublishInterval
 * @details Outputs: none
 * @return data publish time interval in counts
 *************************************************************************/
static U32 getPublishFansDataInterval( void )
{
    U32 result = fansPublishInterval.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The publishFansData function publishes the fans data at the specified
 * time interval.
 * @details Inputs: dataPublishCounter
 * @details Outputs: dataPublishCounter
 * @return none
 *************************************************************************/
static void publishFansData( void )
{
    if ( ++fansPublishCounter > getPublishFansDataInterval() )
    {
        FANS_DATA_T fansData;

        fansData.fansTargetDutyCycle = fansStatus.targetDutyCycle * FRACTION_TO_PERCENT_FACTOR;
        fansData.fanInlet1RPM        = fansStatus.rpm[ FAN_INLET_1 ];
        fansData.fanInlet2RPM        = fansStatus.rpm[ FAN_INLET_2 ];
        fansData.fanInlet3RPM        = fansStatus.rpm[ FAN_INLET_3 ];
        fansData.fanOutlet1RPM       = fansStatus.rpm[ FAN_OUTLET_1 ];
        fansData.fanOutlet2RPM       = fansStatus.rpm[ FAN_OUTLET_2 ];
        fansData.fanOutlet3RPM       = fansStatus.rpm[ FAN_OUTLET_3 ];

        broadcastFansData( &fansData );

        fansPublishCounter = 0;
    }
}


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


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

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

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

    return result;
}

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

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

    return result;
}

/**@}*/