#include "temp_control.h"
#include "app_state.h"   /* TO7, TO7_PID */

/* ---  PID_Controller_Temp и Advanced_Controller_Temp из main.c --- */

uint16_t Advanced_Controller_Temp(LDx_SetupTypeDef * LDx_curr_setup, LDx_ParamTypeDef * LDx_results, uint8_t num)
{
	// Main idea:
	// I is responsible to maintaining constant temperature difference between laser and room temperature.
	// As room temperature can be approximated as constant at current-varying time -- I should be kept constant too.
	// As current through laser diode heats it -- we can estimate excessive power on laser diode and trim peltier current according to it.
	// So, equation should be look like this:
	//   x_output = x_output_original + I(laser)*(a + (t - b)c)
	//     t -- cycle phase
	//     a,b,c -- constants
	//
	// How can we control laser diode temperature?
	//   -- We can set laser to fixed current at the time we need to measure.
	//   Then we should measure wavelength.
	// Calibration sequence:
	//   1) n



	int e_pid;
	float P_coef_current;//, I_coef_current;
	float e_integral;
	int x_output;

	e_pid =  (int) LDx_results->LD_CURR_TEMP - (int) LDx_curr_setup->LD_TEMP;

	e_integral = LDx_results->e_integral;

	if((e_pid < 3000) && (e_pid > - 3000)){
		e_integral += LDx_curr_setup->I_coef_temp * (float)(e_pid) * (float)(TO7 - TO7_PID) / (float) 100;//100 - timer is too fast
	}
	P_coef_current = LDx_curr_setup->P_coef_temp;

	if (e_integral > 32000){
			e_integral = 32000;
	}
	else if (e_integral < - 32000){
			e_integral = -32000;
	}
	LDx_results->e_integral = e_integral;

	x_output = 32768 + P_coef_current * e_pid + (int)e_integral;//32768 - P_coef_current * e_pid - (int)e_integral;//

	if(x_output < 1000){
		x_output = 8800;
	}
	else if(x_output > 56800){
		x_output = 56800;
	}

	if (num==2)
		TO7_PID = TO7;//Save current time only on 2nd laser

	return (uint16_t)x_output;
}


uint16_t PID_Controller_Temp(LDx_SetupTypeDef * LDx_curr_setup, LDx_ParamTypeDef * LDx_results, uint8_t num)
{			
	int e_pid;
	float P_coef_current;//, I_coef_current;
	float e_integral;
	int x_output;
	
	e_pid =  (int) LDx_results->LD_CURR_TEMP - (int) LDx_curr_setup->LD_TEMP;

	e_integral = LDx_results->e_integral;
	
	if((e_pid < 3000) && (e_pid > - 3000)){
		e_integral += LDx_curr_setup->I_coef_temp * (float)(e_pid) * (float)(TO7 - TO7_PID) / (float) 100;//100 - timer is too fast
	}
	P_coef_current = LDx_curr_setup->P_coef_temp;
	
	if (e_integral > 32000){
			e_integral = 32000;
	}
	else if (e_integral < - 32000){
			e_integral = -32000;
	}
	LDx_results->e_integral = e_integral;
	
	x_output = 32768 + P_coef_current * e_pid + (int)e_integral;//32768 - P_coef_current * e_pid - (int)e_integral;//
	
	if(x_output < 1000){
		x_output = 8800;
	}
	else if(x_output > 56800){
		x_output = 56800;
	}
	
	if (num==2)
		TO7_PID = TO7;//Save current time only on 2nd laser
	
	return (uint16_t)x_output;
}