#include "ir_handle.h"
#include "fans.h"

#define A 5//TODO:
#define B 4//TODO:
#define NUM_SENSORS 5
#define THRESHOLD 100

uint8_t height_set[] = {1,1,1,1,1};  //TODO: test the blimp to find the desire set value for the altitude
uint8_t new_ir_result[5];
uint8_t old_ir_result[5]; //store received 5 channel TDMA signal 
uint8_t k[5];  // parameter refer to the change rate of blimp sensed by IR
int8_t LFan_speed; 
int8_t RFan_speed;
int8_t VFan_speed; //VFan_speed is new speed according to IR sensor results


/**  AUTONOMOUS ALTITUDE CONTROL
*    Use proportional control to change
*    the blimp to a perticular height by varify 
*    vertical fan's speed with 
*	 proportional to current height change rate.  
*    It can update Fan_Vertical untill the error is less than the threshod
*/

/** AUTONOMOUS DERECTION CONTROL
* Change the speed and direction of the vans excluding the
  vertical fan when the  detected singal from front,back,left,right 
  is changed.
*/

void IR_FeedbackCTL(void)
{      
    uint8_t sensor_index;
    uint8_t error[5];
    int16_t error_sum;// TODO: deside THRESHOLD   
    uint8_t  L_parameter = 5;  //b 0101 parameter for Left_fan 
    uint8_t	R_parameter = 9;   //b 1001 parameter for Right fan regualte
    error_sum = THRESHOLD;
    
    //initial old_ir_result[5];
     for (sensor_index = 0; sensor_index < NUM_SENSORS; sensor_index++){
     	old_ir_result[sensor_index]= height_set[sensor_index];
	    } 
    
    // Is error acceptable? regulate VFAN until error_sum is acceptable
    while(!(-1*THRESHOLD< error_sum< THRESHOLD))
    {	 uint16_t regulate_sum = 0;
         uint16_t regulateL_sum =0 ; // used for regulate direction;
         uint16_t regulateR_sum =0 ; // used for regulate direction;
        
        error_sum = 0;
	    //Read all 5 sensors sample result 
	   
	    for (sensor_index = 0; sensor_index < NUM_SENSORS; sensor_index++) 
	    {
			new_ir_result[sensor_index] = ir_sensor_get_last_value(sensor_index);
			error[sensor_index]=new_ir_result[sensor_index]-height_set[sensor_index]; 
			
			//the near the block the less theNew_ir_result
			k[sensor_index]=old_ir_result[sensor_index]-new_ir_result[sensor_index];
			old_ir_result[sensor_index]=new_ir_result[sensor_index];
			regulate_sum += error[sensor_index]*k[sensor_index];
			error_sum += error[sensor_index];//// caculate the whole error	
		
			//direction control
			if(sensor_index!= 4){// if sensor_index is the vertical fan, yes break
				regulateL_sum += k[sensor_index]*((L_parameter>>sensor_index)&1);
				regulateL_sum -= k[sensor_index]*((L_parameter>>sensor_index)^1);
				regulateR_sum += k[sensor_index]*((R_parameter>>sensor_index)&1);
				regulateR_sum -= k[sensor_index]*((R_parameter>>sensor_index)^1);
			}
	    }
		if(-1*THRESHOLD<error_sum<THRESHOLD){VFan_speed = 0;
		}else{ VFan_speed = current_speed[FAN_VERTICAL] - regulate_sum/A;} //TODO: deside A
		set_fan(FAN_VERTICAL,VFan_speed);
		
		//direction control
		LFan_speed = current_speed[FAN_LEFT] - regulateL_sum/B;
		RFan_speed = current_speed[FAN_RIGHT] - regulateL_sum/B;
		set_fan(FAN_LEFT,LFan_speed);
		set_fan(FAN_RIGHT,RFan_speed);
    }
}