/*
 * PWM_regulation.c
 *
 * Created: 22.4.2016 10:46:25
 *  Author: Chenda
 */ 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>


#include "../API_common_defs.h"
#include "../../Common/DataOperations.h"
#include "../../conf/conf_brd.h"
#include "../../lib_mcu/TCwPWM/TCwPWM_drv.h"

#include "PWM_regulation.h"

/************************************************************************/
/* Global variables                                                     */
/************************************************************************/
static struct stRegAPI_layer *spAPI;

static uint8_t Operation_Mode = REG_API_SUSPEND;

// requested values
static float OldReqCurrent;
static float NewReqCurrent;
static float OldReqVoltage;
static float NewReqVoltage;

// measured values
static float OldMeasCurrent;
static float NewMeasCurrent;
static float OldMeasTermVoltage;
static float NewMeasTermVoltage;
static float OldMeasPostRegVoltage;
static float NewMeasPostRegVoltage;

// checking and supporting measurement
static float Volt_PreReg;

// big action happened
static uint8_t StabilizationGap;

// fine regulating variables
static uint16_t CalcRegVal_U;
static uint16_t CalcRegVal_I;

// interrupt variables
static volatile bool Reg_I_On = false;
static volatile uint16_t New_RegI_val = 0;
static volatile bool Reg_U_On = false;
static volatile uint16_t New_RegU_val = 0;
/************************************************************************/
/* Private functions prototypes                                         */
/************************************************************************/
void TRAFO_WINDING(void);
void Load_Values(void);
void Save_Values(void);
void Regulation_Tasks(void);
void Compensation_Task(void);
void Output_Control(void);
void SetUp_OpMode(void);
void SafetyCheck_Tasks(void);

// regulation function
void REG_SUSPEND(void);
void REG_REGULATING(void);
void REG_CURRENT(void);
void REG_VOLTAGE(void);

// support functions
uint8_t CheckMeasValInTol(void);
/************************************************************************/
/* Public functions                                                     */
/************************************************************************/
void PWM_REG_INIT(struct stRegAPI_layer *spRegAPIL)
{
	spAPI = spRegAPIL;
	// output configuration
	
	mUReg_Enab();
	mUReg_Clr();
	mIReg_Enab();
	mIReg_Clr();
	mVoltRegInDrv_Enab();
	mVoltRegInDrv_20V();
	mVoltOutDrv_Enab();
	mVoltOutDrv_Off();
	mCompensation1_Enab();
	mCompensation1_Off();
	mCompensation2_Enab();
	mCompensation2_Off();
	mCompensation3_Enab();
	mCompensation3_Off();
	mCompensation4_Enab();
	mCompensation4_Off();
	// timer configuration
	mTimer4Mode_FPWM_ICR();
	mTimer4InpCapRef_Upd(TIMER4_PERIOD);
	mTimer4IntIcr_Clear();
	mTimer4IntIcr_Enab();
	mTimer4CompRegA_Upd(0);
	mTimer4IntOcrA_Clear();
	mTimer4IntOcrA_Enab();
	mTimer4CompRegB_Upd(0);
	mTimer4IntOcrB_Clear();
	mTimer4IntOcrB_Enab();
	
	mTimer4Run_Clk_1();
}

void PWM_REG(void)
{
	if (spAPI->Meas.MeasureRoundFinished) {
		spAPI->Meas.MeasureRoundFinished = false;
		if (StabilizationGap) StabilizationGap--;
		TRAFO_WINDING();
		Load_Values();
		SetUp_OpMode();
		//Operation_Mode = REG_API_REGULATING;
		Regulation_Tasks();
		Compensation_Task();
		Output_Control();
		Save_Values();
	}
	/*
	1)	check post reg voltage for following trafo winding switch hysteresis criteria
	2)	if requested value is changing -> fast regulation mode (regulation without feedback)
	2.1)else figure out the regulation mode for fine tunning (U,I)
	3)	check if new measure values are ready (spAPI->Meas.MeasureRoundFinished), if not, skip fine regulation
	4)	do fine regulation if not fast regulation mode
	*/
	
	// Safety check - over voltage, over current, prereg voltage
	//SafetyCheck_Tasks();
}
/************************************************************************/
/* Private functions                                                    */
/************************************************************************/
void TRAFO_WINDING(void)
{
	if (!StabilizationGap) {
		if (mIsVoltRegInDrv_20V()) {
			// Low voltage winding configuration
			if (NewMeasPostRegVoltage > TRAFO_SWITCH_HIGH_VOLTAGE) {
				mVoltRegInDrv_40V();
				if (StabilizationGap < STAB_GAP_TRAFO_SW_HIGH) StabilizationGap = STAB_GAP_TRAFO_SW_HIGH;
			}
		}
		else {
			// High voltage winding configuration
			if (NewMeasPostRegVoltage < TRAFO_SWITCH_LOW_VOLTAGE) {
				mVoltRegInDrv_20V();
				if (StabilizationGap < STAB_GAP_TRAFO_SW_LOW) StabilizationGap = STAB_GAP_TRAFO_SW_LOW;
			}
		}
	}
}

void Load_Values(void)
{
	DAT_OPS_16int_to_Float(&NewReqCurrent,&spAPI->Req.ReqI.Int,&spAPI->Req.ReqI.Tenth);
	DAT_OPS_16int_to_Float(&NewReqVoltage,&spAPI->Req.ReqU.Int,&spAPI->Req.ReqU.Tenth);
	// checking correct requested values
	if (NewReqVoltage > PS_MAX_VOLTAGE) {
		NewReqVoltage = PS_MAX_VOLTAGE;
		DAT_OPS_Float_to_16int(&NewReqVoltage,&spAPI->Req.ReqU.Int,&spAPI->Req.ReqU.Tenth);
	}
	if (NewReqVoltage < 0) {
		NewReqVoltage = 0;
		DAT_OPS_Float_to_16int(&NewReqVoltage,&spAPI->Req.ReqU.Int,&spAPI->Req.ReqU.Tenth);
	}
	if (NewReqVoltage > 0 && NewReqVoltage < PS_MIN_VOLTAGE) {
		NewReqVoltage = PS_MIN_VOLTAGE;
		DAT_OPS_Float_to_16int(&NewReqVoltage,&spAPI->Req.ReqU.Int,&spAPI->Req.ReqU.Tenth);
	}
	if (NewReqCurrent > PS_MAX_CURRENT) {
		NewReqCurrent = PS_MAX_CURRENT;
		DAT_OPS_Float_to_16int(&NewReqCurrent,&spAPI->Req.ReqI.Int,&spAPI->Req.ReqI.Tenth);
	}
	if (NewReqCurrent < 0) {
		NewReqCurrent = 0;
		DAT_OPS_Float_to_16int(&NewReqCurrent,&spAPI->Req.ReqI.Int,&spAPI->Req.ReqI.Tenth);
	}
	if (NewReqCurrent > 0 && NewReqCurrent < PS_MIN_CURRENT) {
		NewReqCurrent = PS_MIN_CURRENT;
		DAT_OPS_Float_to_16int(&NewReqCurrent,&spAPI->Req.ReqI.Int,&spAPI->Req.ReqI.Tenth);
	}
	
	DAT_OPS_16int_to_Float(&NewMeasCurrent,&spAPI->Meas.MeasC.Int,&spAPI->Meas.MeasC.Tenth);
	DAT_OPS_16int_to_Float(&NewMeasTermVoltage,&spAPI->Meas.MeasTermU.Int,&spAPI->Meas.MeasTermU.Tenth);
	DAT_OPS_16int_to_Float(&NewMeasPostRegVoltage,&spAPI->Meas.MeasPostRegU.Int,&spAPI->Meas.MeasPostRegU.Tenth);
	DAT_OPS_16int_to_Float(&Volt_PreReg,&spAPI->Meas.MeasPreRegU.Int,&spAPI->Meas.MeasPreRegU.Tenth);
}

void Save_Values(void)
{
	OldReqCurrent = NewReqCurrent;
	OldReqVoltage = NewReqVoltage;
	
	OldMeasCurrent = NewMeasCurrent;
	OldMeasTermVoltage = NewMeasTermVoltage;
	OldMeasPostRegVoltage = NewMeasPostRegVoltage;
}

void Output_Control(void)
{
	if (PS_OUTPUT_SWITCH_PRESENT && spAPI->Event != REG_API_ERROR) {
		if (spAPI->Req.OutputOn && 
			(Operation_Mode == REG_API_REG_CURRENT || Operation_Mode == REG_API_REG_VOLTAGE) &&
			!StabilizationGap) {
				// requested state - ON
				mVoltOutDrv_On();
		}
		if (!spAPI->Req.OutputOn) {
			// requested state - OFF
			 mVoltOutDrv_Off();
		}
	}
}

void Regulation_Tasks(void)
{
	if (spAPI->Event != REG_API_ERROR)
	{
		if (Operation_Mode == REG_API_SUSPEND) REG_SUSPEND();
		if (Operation_Mode == REG_API_REGULATING) REG_REGULATING();
		if (!StabilizationGap) {
			if (Operation_Mode == REG_API_REG_CURRENT) REG_CURRENT();
			if (Operation_Mode == REG_API_REG_VOLTAGE) REG_VOLTAGE();
		}
	}
	else {
		Reg_I_On = false;
		Reg_U_On = false;
	}
}

void Compensation_Task(void)
{
	
}

void SetUp_OpMode(void)
{
	uint8_t tmp;
	/*
	0)	requested values are 0, or one of requested value is 0 -> REG_API_SUSPEND
	1)	requested values changed - RAW regulating -> REG_API_REGULATING
	2)	no one of measured value equals to one of requested value -> REG_API_REG_VOLTAGE
	3)	measured voltage on terminal/post reg is within tolerance of req voltage -> REG_API_REG_VOLTAGE
	4)	measured current is within tolerance of req current -> REG_API_REG_CURRENT
	*/
	mStatusLed_Off();
	tmp = CheckMeasValInTol();
	
	if (NewReqCurrent == 0 && NewReqVoltage == 0) Operation_Mode = REG_API_SUSPEND;
	else if(NewReqCurrent != OldReqCurrent || NewReqVoltage != OldReqVoltage)  Operation_Mode = REG_API_REGULATING;
	else if( (tmp & 0b001) == 0b001 ) Operation_Mode = REG_API_REG_CURRENT;
	else if( (tmp & 0b010) == 0b010 ) Operation_Mode = REG_API_REG_VOLTAGE;
	else Operation_Mode = REG_API_REG_VOLTAGE;
}

void SafetyCheck_Tasks(void)
{
	if (mIsVoltRegInDrv_20V()) {
		if (Volt_PreReg < TRAFO_LOW_WINDING_VOLTAGE) {
			spAPI->Event = REG_API_ERROR;
			spAPI->ErrorNumber = REG_API_E_LOW_WIND_V;
		}
	}
	else {
		if (Volt_PreReg < TRAFO_HIGH_WINDING_VOLTAGE) {
			spAPI->Event = REG_API_ERROR;
			spAPI->ErrorNumber = REG_API_E_HIGH_WIND_V;
		}
	}
	if (NewMeasPostRegVoltage > (PS_MAX_VOLTAGE + 2)) {
		// PS over voltage
		spAPI->Event = REG_API_ERROR;
		spAPI->ErrorNumber = REG_API_E_OVERVOLTAGE;
	}
	if (NewMeasCurrent > (PS_MAX_CURRENT + 0.5)) {
		// PS over current
		spAPI->Event = REG_API_ERROR;
		spAPI->ErrorNumber = REG_API_E_OVERCURRENT;
	}
	
	if (spAPI->Event == REG_API_ERROR) {
		mStatusLed_On();
		mVoltOutDrv_Off();
	}
}

/************************************************************************/
/* Regulation function                                                  */
/************************************************************************/
void REG_SUSPEND(void)		// DONE - not tested
{
	// requested values are 0
	Reg_I_On = false;
	New_RegI_val = 0;
	Reg_U_On = false;
	New_RegU_val = 0;
	StabilizationGap = STAB_GAP_SUSPEND_MODE;
}

void REG_REGULATING(void)	// DONE - not tested
{
	// New requested value - calculate new raw values
	float tmp;
	
	// requested voltage calculating
	//tmp = (1.0432*NewReqVoltage) - 0.2298;
	//tmp *= SNS_POST_REG_VOLTAGE_GAIN;
	tmp = NewReqVoltage * 1000;
	tmp *= SNS_POST_REG_VOLTAGE_GAIN;	// requested voltage after equaling to sensor divider
	tmp /= AVCC_VOLTAGE;
	tmp /= 1000;
	tmp = TIMER4_PERIOD * tmp;			// PWM calculating according to ratio
	
	if (tmp < 0) tmp = 0;
	New_RegU_val = (uint16_t)tmp;
	CalcRegVal_U = New_RegU_val;
	if (New_RegU_val > (TIMER4_PERIOD-TIMER4_PROTECTION_VALUE)) New_RegU_val = TIMER4_PERIOD-TIMER4_PROTECTION_VALUE;
	if (New_RegU_val > 0) Reg_U_On = true;
	else Reg_U_On = false;
	
	// requested current calculating
	tmp = NewReqCurrent * 1000;
	tmp *= SNS_CURR_RESISTOR;
	tmp *= SNS_CURRENT_GAIN;				// requested current after equaling to sensor divider
	tmp /= AVCC_VOLTAGE;
	tmp /= 1000;
	tmp = TIMER4_PERIOD * tmp;			// PWM calculating according to ratio
	
	if (tmp < 0) tmp = 0;
	New_RegI_val = (uint16_t) tmp;
	CalcRegVal_I = New_RegI_val;
	if (New_RegI_val > (TIMER4_PERIOD-TIMER4_PROTECTION_VALUE)) New_RegI_val = TIMER4_PERIOD-TIMER4_PROTECTION_VALUE;
	if (New_RegI_val > 0) Reg_I_On = true;
	else Reg_I_On = false;
	
	StabilizationGap = STAB_GAP_REGULATION_MODE;
}

void REG_CURRENT(void)
{
	// fine current tunning
	float tmp;
	//float CorrVal;
	uint16_t NewRegValI;
	//int16_t tmp1;
	
	tmp = NewReqCurrent - NewMeasCurrent;
	//CorrVal = tmp * 1000;
	//CorrVal *= SNS_CURR_RESISTOR;		// differential voltage on resistor
// 	CorrVal *= SNS_CURRENT_GAIN;			// different of voltages of request and measured
// 	CorrVal /= 1000;
// 	
// 	CorrVal /= ADC_BIT_VOLTAGE;
// 	CorrVal /= 2;
	NewRegValI = New_RegI_val;
	
	
	if ( tmp < (-CURRENT_TOL) ) {
		// do regulation
		//mStatusLed_On();
		//New_RegI_val-= -(CorrVal);
		//New_RegI_val--;
		if (NewRegValI > 0) NewRegValI--;
		//StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
	
	if ( tmp > CURRENT_TOL ) {
		//mStatusLed_On();
		// do regulation
		//New_RegI_val+= CorrVal;
		//New_RegI_val++;
		if (NewRegValI < 0xfff) NewRegValI++;
		//StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
	
	//tmp1 = CalcRegVal_I - NewRegValI;
	//tmp1 = abs(tmp1);
	//if (tmp1 < REG_I_VAL_RANGE && NewRegValI != New_RegI_val) {
	if (NewRegValI != New_RegI_val) {
		mStatusLed_On();
		if (NewRegValI > (TIMER4_PERIOD-TIMER4_PROTECTION_VALUE)) NewRegValI = TIMER4_PERIOD-TIMER4_PROTECTION_VALUE;
		New_RegI_val = NewRegValI;
		StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
}

void REG_VOLTAGE(void)
{
	// fine voltage tunning
	float tmp;
	//float CorrVal;
	//float Gain;
	uint16_t NewRegValU;
	int16_t tmp1;
	
	//mStatusLed_On();
	
	if (PS_OUTPUT_SWITCH_PRESENT && !mIsVoltOutDrv_Off() && PS_TERMINAL_MEASURING) {
		tmp = NewMeasTermVoltage;
		//Gain = SNS_TERM_VOLTAGE_GAIN;
	}
	else {
		tmp = NewMeasPostRegVoltage;
		//Gain = SNS_POST_REG_VOLTAGE_GAIN;
	}
	
	// making voltage different
	tmp = NewReqVoltage - tmp;
	
	//CorrVal = Gain * tmp;
	//CorrVal /= ADC_BIT_VOLTAGE;
	//CorrVal /= 2;
	NewRegValU = New_RegU_val;
	
	if ( tmp < (-VOLTAGE_TOL) ) {
		//if ( tmp < (-CURRENT_TOL) ) {
		// do regulation
		//mStatusLed_On();
		//New_RegI_val-= -(CorrVal);
		//New_RegI_val--;
		if (NewRegValU > 0) NewRegValU--;
		//StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
	
	if ( tmp > VOLTAGE_TOL ) {
		//if ( tmp > CURRENT_TOL ) {
		//mStatusLed_On();
		// do regulation
		//New_RegI_val+= CorrVal;
		//New_RegI_val++;
		if (NewRegValU < 0xfff) NewRegValU++;
		//StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
	
	tmp1 = CalcRegVal_U - NewRegValU;
	tmp1 = abs(tmp1);
	if (tmp1 < REG_U_VAL_RANGE && NewRegValU != New_RegU_val) {
		mStatusLed_On();
		if (NewRegValU > (TIMER4_PERIOD-TIMER4_PROTECTION_VALUE)) NewRegValU = TIMER4_PERIOD-TIMER4_PROTECTION_VALUE;
		New_RegU_val = NewRegValU;
		StabilizationGap += STAB_GAP_CURRENT_PREC_MODE;
	}
	//mStatusLed_On();
}

/************************************************************************/
/* Support private functions                                            */
/************************************************************************/
uint8_t CheckMeasValInTol(void)
{
	uint8_t ValueInTol = 0;
	
	if( (NewReqCurrent + CURRENT_TOL) > NewMeasCurrent) {
		if ( (NewReqCurrent - CURRENT_TOL) < NewMeasCurrent) ValueInTol |= 0b001;
		else ValueInTol |= 0b010;
	} 
	else {
		ValueInTol |= 0b001;
	}

	return ValueInTol;
}

/************************************************************************/
/* Interrupt handling                                                   */
/************************************************************************/
// Voltage regulation
ISR(TIMER4_COMPA_vect)
{
	mUReg_Clr();
}

// Current regulation
ISR(TIMER4_COMPB_vect)
{
	mIReg_Clr();
}

ISR(TIMER4_CAPT_vect)
{
	
	if (Reg_I_On) {
		mIReg_Set();
	}
	if (Reg_U_On) {
		mUReg_Set();
	}
	mTimer4CompRegA_Upd(New_RegU_val);
	mTimer4CompRegB_Upd(New_RegI_val);
	
}

ISR(TIMER4_OVF_vect)
{
	/*
	if (Reg_I_On) {
		mIReg_Set();
	}
	if (Reg_U_On) {
		mUReg_Set();
	}
	mTimer4CompRegA_Upd(New_RegU_val);
	mTimer4CompRegB_Upd(New_RegI_val);
	*/
}