#include "PID.h"
#include "Relays.h"
#include "USART.h"
#include "rs485.h"
extern u16 tem;
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;
u8 rs485speed[8] = {0x01, 0x06, 0x60, 0x00, 0x00, 0x09, 0xBB, 0xAA}; // speed control for compressor controller
int min_speed_count = 1500;
int max_speed_count = 6000;

void PID_Init()
{
	// pid.set_tem=tem;// user set temperature
	// if flash have not a vaild value, just set a default value
	if (pid.Kp < 1e-7) { pid.Kp = 40; }
	if (pid.Ki < 1e-7) { pid.Ki = 0.001; }
	if (pid.Kd < 1e-7) { pid.Kd = 340; }
	if (pid.tem_threshold < 0.0001) { pid.tem_threshold = 0.2; }

	pid.t = 500; // 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;
}

/**
 * send speed count to compressor controller
*/
void send_speed_signal(int speed) {
	rs485speed[4] = speed / 256;
	rs485speed[5] = speed % 256;
	
	RS485_3_Init(9600);
	RS485_3_Send_Data(rs485speed, 8);
	delay_xms(30);
	RS485_1_Init(9600);
}

void PID_Calc() // pid calc
{
	// 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;
	// }

	// // if (pid.set_tem > pid.now_tem)
	// // {
	// // 	pid.Ek = pid.set_tem - pid.now_tem;
	// // }
	// // else
	// // {
	// // 	pid.Ek = pid.now_tem - pid.set_tem;
	// // }
	// pid.Ek = pid.now_tem - pid.set_tem;
	// pid.Pout = pid.Kp * pid.Ek; // Proportional output

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

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

	// // 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;
	// }
	// pid.Ek_1 = pid.Ek; // udpate difference
	// pid.C1ms = 0;

	// // speed count
	// int speed_count = pid.OUT / 200.0 * (max_speed_count - min_speed_count) + min_speed_count;
	// if (speed_count > 6000) {
	// 	speed_count = 6000;
	// }

	// if (pid.now_tem < pid.set_tem + pid.tem_offset - pid.tem_threshold)
	// {
	// 	// Obtain the current deviation value
	// 	// when the target temperature is 1 degree Celsius higher than the actual temperature, heat up
	// 	// close compressor open heater
	// 	/*GPIO1->Alarm bell GPIO3->heater GPIO4->Fresh air fan GPIO5->humidifier GPIO6->compressor  */
	// 	// HC595_Send_Byte(gpio_state &= 0xDF);//close compressor		&=1101 1111	    0xDF
	// 	HC595_Send_Byte(gpio_state |= 0x04); // open heater    |=0000 0100  0x04
	// 	speed_count = 1000; // close compressor
	// 	hot_clod_flag = 2;
	// 	pid.Iout = 0;
	// } else if (pid.now_tem > pid.set_tem + pid.tem_offset - pid.tem_threshold && pid.now_tem < pid.set_tem + pid.tem_offset + pid.tem_threshold)
	// {
	// 	HC595_Send_Byte(gpio_state &= 0xFB);		  // close heater &=1111 1011  0xFB
	// 	speed_count = 1000; // close compressor

	// 	hot_clod_flag = 0;
	// 	// pid.Iout=0;
	// } else if (pid.now_tem > pid.set_tem + pid.tem_offset + pid.tem_threshold)
	// {
	// 	// Obtain the current deviation value
	// 	// when the target temperature is lower than the actual temperature, refrigerate
	// 	// open compressor close heater
	// 	HC595_Send_Byte(gpio_state &= 0xFB);		  // close heater &=1111 1011  0xFB


	// 	// // 0-200 correspond 0-100%, if pid.out=50, percentage means 25% //num=50*400/200=100 100/400=25%
	// 	// num = (((pid.OUT * 400) / pid.pwmcycle) - 1); // Conversion of pid.OUT and PWM Duty Cycle Values
	// 	// TIM_SetCompare3(TIM3, num / 4);
	// 	// printf("%d\r\n",num);
	// 	// HC595_Send_Byte(gpio_state|=0x20);//open compressor |=0010 0000
	// 	hot_clod_flag = 1;
	// 	// pid.Iout=0;
	// }

	// send_speed_signal(speed_count);
	send_speed_signal(1500);

	// if (hot_clod_flag == 1 && T <= tem - 3) // During the refrigeration process, the actual temperature drops by 0.3 degrees Celsius below the set temperature
	// {
	// 	HC595_Send_Byte(gpio_state&=0xDB);// close compressor and heater		   &=1101 1011       0xDB
	// 	// num = 0;
	// 	// TIM_SetCompare3(TIM3, 0);			 // close compressor
	// 	speed_count = 1000;
	// 	hot_clod_flag = 0;
	// }

	// if (hot_clod_flag == 2 && T >= tem) // while heat, T above tem
	// {
	// 	HC595_Send_Byte(gpio_state&=0xDB);//close compressor and heater    &=1101 1011       0xDB
	// 	num = 0;
	// 	TIM_SetCompare3(TIM3, 0);			 // close compressor
	// 	hot_clod_flag = 0;
	// }


	// HC595_Send_Byte(gpio_state&=0xDB);// close compressor and heater		   &=1101 1011       0xDB
}