#include <math.h>
#include <stdlib.h>
#include "PID.h"
#include "Relays.h"
#include "USART.h"
#include "rs485.h"
#define MODE_H 1
#define MODE_L 0
float cold_tem = 0;
float red_tem = 0;
float ti;
float ki = 0.001;
float kd = 340;
extern u8 hot_clod_flag;
extern u8 gpio_state;
extern int T;
unsigned int num = 0;
PID pid;
int min_speed_count = 2000;
int max_speed_count = 4800;

/**
 * PID init
*/
void PID_Init()
{
	if (abs(pid.out_tem) < 1e-5 && abs(pid.out_humidity) < 1e-5) {
		return;
	}
	
	// // if flash have not a vaild value, just set a default value
	// if (pid.Kp < 1e-7) { pid.Kp = 9.6; }
	// if (pid.Ki < 1e-7) { pid.Ki = 0.01; }
	// if (pid.Kd < 1e-7) { pid.Kd = 340; }
	// if (pid.tem_threshold < 0.0001) { pid.tem_threshold = 0.2; }

	pid.Ek = 0;
	pid.Ek_prev = 0;
	pid.SEk = 0;
	pid.Pout = 0;
	pid.Iout = 0;
	pid.Dout = 0;
	pid.OUT = 0;
	pid.OUT0 = 0;
	
	pid.h_percent = 0;
	pid.c_speed = 0;

	pid.t = 1000; // PID calc period
	// pid.Ti=5000000;// integral time
	// pid.Td=1000;// differential time
	pid.pwmcycle = 200; // pwm cycle 200
	pid.OUT0 = 1;
	pid.C1ms = 0;

	pid.max_compressor_tem = 30;

	

	pid.hp_h = 6;
	pid.hi_h = 0.02;
	pid.hd_h = 0;
	pid.h_base_h = 0;

	pid.hp_l = 9;
	pid.hi_l = 0.015;
	pid.hd_l = 0;
	pid.h_base_l = 30;

	pid.hp = 0;
	pid.hi = 0;
	pid.hd = 0;
	pid.h_base = 0;

	pid.cp = 4;
	pid.ci = 0.01;
	pid.cd = 0;
	pid.c_base = 37;

	


}

/**
 * set compressor speed count
 * range of speed count: 0-6000, if speed count lower than 1500, the compressor will stop
*/
void set_compressor_power(int speed) {
	u8 data[8] = {0x01, 0x06, 0x60, 0x00, 0x00, 0x09, 0xBB, 0xAA}; // speed control for compressor controller
	if (speed > max_speed_count) {
		speed = max_speed_count;
	}
	if (speed < 0) {
		speed = 0;
	}
	data[4] = speed / 256;
	data[5] = speed % 256;
	GetCRC16(data, 6, data + 6, data + 7);
	
	RS485_3_Init(9600);
	delay_xms(30);
	RS485_3_Send_Data(data, 8);
	delay_xms(30);
	RS485_1_Init(9600);
}

/**
 * set heater percent
 * range of heater percent: 0-100
*/
void set_heater_power(int percent) {
	u8 data[8] = { 0x10, 0x06, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
	if (percent > 100) {
		percent = 100;
	}
	if (percent < 0) {
		percent = 0;
	}
	data[4] = percent / 256;
	data[5] = percent % 256;

	GetCRC16(data, 6, data + 6, data + 7);

	RS485_1_Init(9600);
	delay_xms(30);
	RS485_1_Send_Data(data, 8);
	delay_xms(30);
}

/**
 * heater power calc
*/
int calc_hp(float delta_t, float Error_calc, float DelEk, int p_hb, float pid_hp, float pid_hi, float pid_hd) {
	int p_h = p_hb + pid_hp * delta_t + pid_hi * Error_calc + pid_hd * DelEk;
	if (p_h > 100) {
		return 100;
	}
	if (p_h < 0) {
		return 0;
	}
	return p_h;
}

/**
 * compressor power percent calc
*/
int calc_cp(float delta_t, float Error_calc, float DelEk, int cb, float cp, float ci, float cd) {
	int percent = cb - cp * delta_t + ci * Error_calc + cd * DelEk;
	if (percent > 100) {
		return 100;
	}
	if (percent < 0) {
		return 0;
	}
	return percent;
}

/**
 * compressor speed calc
*/
int calc_compressor_speed(int percent, int v_min, int v_max) {
	int v = percent * v_max / 100.0;
	if (v > v_max) {
		return v_max;
	}
	if (v < v_min) {
		return v_min;
	}
	
	return v;
}

void PID_Calc() // pid calc
{
	int mode = 0;
	if (pid.set_tem > pid.max_compressor_tem) {
		// h mode
	    mode = MODE_H;
	} else {
		// l mode
		mode = MODE_L;
	}
	// int min_speed_count = 1800;
	// int max_speed_count = 4800;
	float DelEk; // The difference between the last two deviations
	// float td;
	float out;
	// if (pid.C1ms < (pid.t)) // The calculation cycle has not yet arrived
	// {

	// 	return;
	// }
	
	float delta_t = pid.set_tem - pid.now_tem;

	if (mode == MODE_H) {
		pid.hp = pid.hp_h;
		pid.hi = pid.hi_h;
		pid.hd = pid.hd_h;
		pid.h_base = pid.h_base_h;

		// if now temp is close to set temp, the heater will be less power
		// TODO::在外界温度温差比较大（45-22=23度）时，加热功率也可能不够
		if (pid.set_tem - pid.now_tem < 3) {
			pid.hp = pid.hp_h * 0.6;
		}
		// TODO::在外界温度温差比较大（45-22=23度）时，加热功率不够
		if (pid.set_tem - pid.now_tem < 1) {
			pid.hp = pid.hp_h * 0.3;
		}
		// TODO::要改成根据外界温度调节参数
	}

	// l mode
	if (mode == MODE_L) {
		pid.hp = pid.hp_l;
		pid.hi = pid.hi_l;
		pid.hd = pid.hd_l;
		pid.h_base = pid.h_base_l;

		// if now temp is close to set temp, the heater will be less power
		// if (pid.set_tem - pid.now_tem < 3) {
		// 	pid.hp = pid.hp_l * 0.6;
		// }
		if (abs(pid.out_tem) > 1e-5) {
			// hp_l = 3
			// pid.hp = -0.02 * pid.hp_l * (pid.out_tem - pid.set_tem) + pid.hp_l;

			// increase hp when the delta_tem is large
			float delta_tem = pid.now_tem - pid.set_tem;
			if (delta_tem > - 2) {
				// pid.hp = pid.hp * 0.5;
				pid.hp = -0.09 * delta_tem + pid.hp_l / 2;
			} else {
				pid.hp = (-pid.hp_l / 6 + 0.06) * delta_tem + pid.hp_l / 6 + 0.3;
			}
		}
	}
	
	pid.Ek = pid.set_tem - pid.now_tem;
	pid.Pout = pid.Kp * pid.Ek; // Proportional output

	pid.SEk += pid.Ek; // Total historical deviation

	DelEk = pid.Ek - pid.Ek_prev; // The difference between the last two deviations

	// h mode
	if (mode == MODE_H) {
		// when set temp is larger then real temp, the heater will be less power
		// in h mode, when we want to heat over 5 degrees, just use hp
		if (pid.set_tem > pid.now_tem + 5) {
			pid.hi = 0;
			pid.hd = 0;
			pid.SEk = 0;
		}
	}

	// l mode
	if (mode == MODE_L) {
		// make integral smaller when the temp is too low and the SEk is too small 
		if (pid.now_tem < pid.set_tem - 2 && pid.SEk <= - (pid.h_base / 2) / pid.hi) {
			pid.SEk /= 10;
		}
		// SEk limit, updated func, remain a little heater power when the compressor is running in full state
		if (pid.SEk < - (pid.h_base / 2) / pid.hi) {
			pid.SEk = - (pid.h_base / 2) / pid.hi;
		}
	}

	
	// when the compressor is in full status, then the integral will be 0
	if (pid.c_speed == max_speed_count) {
		pid.SEk = 0;
	}
	float Error_calc = pid.SEk;
	if (Error_calc < - (pid.h_base + pid.hp * delta_t) / pid.hi) {
		Error_calc = - (pid.h_base + pid.hp * delta_t) / pid.hi;
	}
	if (pid.c_speed == max_speed_count) {
		Error_calc = 0;
	}



	// ti=pid.t/pid.Ti;
	// ki=ti*pid.Kp;

	pid.Iout = pid.Ki * pid.SEk; // integral output

	// 	td=pid.Td/pid.t;
	// 	kd=pid.Kp*td;

	pid.Dout = pid.Kd * DelEk; // difference output
	if (pid.Dout < 0)
	{
		pid.Dout = 0 - pid.Dout;
	}

	// out= pid.Pout+pid.Iout+ pid.Dout;
	out = pid.Pout;
	if (out > pid.pwmcycle)
	{
		pid.OUT = pid.pwmcycle;
	}
	else if (out <= 0)
	{
		pid.OUT = pid.OUT0;
	}
	else
	{
		pid.OUT = out;
	}

	// When the target tem is greater then max compressor tem, the compressor will stop
	if (pid.set_tem > pid.max_compressor_tem) {
	    pid.c_speed = 0;
	} else {
		// if outer temp is got, we calc pid by outer temp
		if (abs(pid.out_tem) > 1e-5) {
			pid.cp = (pid.out_tem - pid.set_tem) * 0.28;
			// pid.cp = (pid.out_tem - pid.set_tem) * 0.2;
		}
		// use nagetive error and error diff when calc compressor power
		int p_c = calc_cp(delta_t, - Error_calc, - DelEk, pid.c_base, pid.cp, pid.ci, pid.cd);
		pid.c_speed = calc_compressor_speed(p_c, min_speed_count, max_speed_count);
	}

	// heater percent
	pid.h_percent = calc_hp(delta_t, Error_calc, DelEk, pid.h_base, pid.hp, pid.hi, pid.hd);
	// in h mode, if current temp is close to set temp, the heater will close
	if (pid.now_tem > pid.set_tem - 0.2 && pid.set_tem > pid.max_compressor_tem) {
	    pid.h_percent = 0;
	}
	// close heater when compressor is running in full state
	if (pid.c_speed == max_speed_count) {
		pid.h_percent = 0;
	}

	set_compressor_power(pid.c_speed);
	set_heater_power(pid.h_percent);

	
	pid.Ek_prev = pid.Ek; // udpate difference
	pid.C1ms = 0;

}