#include "etpwm.h"
#include "Fans.h"
#include "TaskGeneral.h"
#include "Thermistors.h"
#include "SystemCommMessages.h"
#include "FPGA.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.

/// Fans self test states
typedef enum fans_Self_Test
{
    FANS_SELF_TEST_START = 0,   ///< Fans self test start state
    FANS_SELF_TEST_CHECK_RPM,   ///< Fans self test check RPM
    FAN_SELF_TEST_COMPLETE,     ///< 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 = 0,  ///< Fans exec state start
    FANS_EXEC_STATE_RUN,                ///< Fans exec state run
    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 fansSelfTestReslt = SELF_TEST_STATUS_IN_PROGRESS;          ///< Fans self test result
static FANS_SELF_TEST_STATES_T fansSelfTestState = FANS_SELF_TEST_START;             ///< Fans self test state
static FANS_EXEC_STATES_T fansExecState = FANS_EXEC_STATE_WAIT_FOR_POST;             ///< Fans exec state
static U32 fansControlCounter = 0;                                                   ///< Fans control interval counter
static U32 fansPublishCounter = 0;                                                   ///< Fans data publish 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 toggle2RPMCoefficient = 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 convertFansTogglePeriod2RPM( 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;
    fansSelfTestReslt = SELF_TEST_STATUS_IN_PROGRESS;
    fansSelfTestState = FANS_SELF_TEST_START;
    fansControlCounter = 0;
    fansPublishCounter = 0;
}

/*********************************************************************//**
 * @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:
            fansSelfTestState = handleSelfTestStart();
            break;

        case FANS_SELF_TEST_CHECK_RPM:
            fansSelfTestState = handleSelfTestCheckRPM();
            break;

        case FAN_SELF_TEST_COMPLETE:
            // 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;
            break;
    }

    return fansSelfTestReslt;
}

/*********************************************************************//**
 * @brief
 * The execFans function executes the execFans exec states.
 * @details Inputs: fansExecState
 * @details Outputs: fansExecState
 * @return none
 *************************************************************************/
void execFans( void )
{
    switch ( fansExecState )
    {
        case FANS_EXEC_STATE_WAIT_FOR_POST:
            fansExecState = handleExecStateWaitForPOST();
            break;

        case FANS_EXEC_STATE_RUN:
            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;
            break;
    }

    convertFansTogglePeriod2RPM();

    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 getFanRPM( FANS_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: fansStatus
 * @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_START;
    FANS_NAMES_T fan;

    // Get the raw toggle periods from FPGA and convert them to RPM. They all should be 0 upon staring
    // the device
    convertFansTogglePeriod2RPM();

    // Loop through all the fans to check their RPM
    // Upon starting the device the RPM of the fans should be 0
    for( fan = FAN_INLET_1; fan < NUM_OF_FANS_NAMES; fan++ )
    {
        if ( fansStatus.rpm[ fan ] > 0 )
        {
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_FAN_RPM_OUT_OF_RANGE, fan, fansStatus.rpm[ fan ] );
        }
    }

    // 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;

    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;
    FANS_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 )
    {
        convertFansTogglePeriod2RPM();

        // Loop through all the fans to check their RPM. They should 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 ] );
            }
        }

        // Turn off the fans, done with self test
        setInletFansDutyCycle( 0 );
        setOutletFansDutyCycle( 0 );
        state = FAN_SELF_TEST_COMPLETE;
    }

    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;

    // Wait for the self test to finish before starting the fans
    if ( fansSelfTestState == FAN_SELF_TEST_COMPLETE )
    {
        // 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;
    }

    // TODO REMOVE
    state = FANS_EXEC_STATE_RUN;
    // TODO REMOVE
    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;

    // 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 we are ramping down, set the target duty cycle to max allowed ramp down PWM
            fansStatus.targetDutyCycle -= FANS_MAX_ALLOWED_RAMP_DOWN_DELTA_DUTY_CYCLE;
        }

        // 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 getFansRPM 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 convertFansTogglePeriod2RPM( void )
{
    FANS_NAMES_T fan;
    U32 togglePeriod;

    // Loop through the fans and get the pulse of each of them
    for ( fan = FAN_INLET_1; fan < NUM_OF_FANS_NAMES; fan++ )
    {
        switch ( fan )
        {
            case FAN_INLET_1:
                togglePeriod = getFPGAInletFan1TogglePeriod();
                break;

            case FAN_INLET_2:
                togglePeriod = getFPGAInletFan2TogglePeriod();
                break;

            case FAN_INLET_3:
                togglePeriod = getFPGAInletFan3TogglePeriod();
                break;

            case FAN_OUTLET_1:
                togglePeriod = getFPGAOutletFan1TogglePeriod();
                break;

            case FAN_OUTLET_2:
                togglePeriod = getFPGAOutletFan2TogglePeriod();
                break;

            case FAN_OUTLET_3:
                togglePeriod = getFPGAOutletFan3TogglePeriod();
                break;

            default:
                // Invalid fan has been selected, raise an alarm
                SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_FAN_SELECTED, fan );
                break;
        }

        // If the pulse is close to 0 or 0, FPGA will report 0xFFFF
        // Otherwise, convert the pulse to RPM
        if ( togglePeriod == FANS_ZERO_RPM_TOGGLE_PERIOD_VALUE )
        {
            fansStatus.rpm[ fan ] = 0;
        }
        else
        {
            // Convert toggle period to RPM
            fansStatus.rpm[ fan ] = toggle2RPMCoefficient / togglePeriod;
        }
    }
}

/*********************************************************************//**
 * @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.fansTargetPWM = fansStatus.targetDutyCycle * 100;
        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;
}

/**@}*/