#include <Servo.h>
Servo myservo; 

#define POT_IN A0

float float_map(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

float float_constrain(float value, float lower, float higher){
	if (value > higher) return higher;
	if (value < lower) return lower;
	return value;
}

void setup() {
  Serial.begin(9600);              // begins the serial communication
	myservo.attach(11);
	pinMode(POT_IN, INPUT);     // sets the potentiometer as an input and controller of the setpoint value 
}

void loop() {
	static float consigne = 0;
	float line_position = float_map(analogRead(POT_IN),0,1023,-1,1);
	myservo.write(float_map(consigne,-1,1,0,180));

 	consigne = computePID(line_position,1,0.7,0.01,1.2);
}


float computePID_old(float setpoint, float KP, float KI, float KD){
	static float output = 0;
	static float I_error = 0;
	static float D_error = 0;
	static float lastError = 0;

  	float error = setpoint - output; // calculates the error value
 
  	I_error += error; // error summation
  	D_error = lastError - error; // error variation
  	// as this loop will be called regularly, we can suppose 
  	// that dt is a constant and not include it in the above constants.
  	// Dt is alongside KP, KI, KD, in an implicit way.
 
  	// Implementation of the PID algorithm
  
  	output += KP*error + KI*I_error + KD*D_error;  
	lastError = error;   
 
  	// limits the control output to valid PWM values (10-255)
  	output = constrain(output, -1, 1);
 
  	// prints the speed and setpoint values
  	// RPM values
  	// use the graph funcion in the serial monitor!  
	
	//return output;
	return output;
}

float computePID(float error, float sampling_time, float KP, float KI, float KD){ // la position de ligne traduit l'erreur, pas besoin de la calculer
	static float output = 0, previous_error = 0, integral_error = 0, last_time = 0, delta = 0;
	if (millis() - last_time > sampling_time){		
		delta = error - previous_error;

		integral_error += error;
		integral_error = float_constrain(integral_error,-1/KI,1/KI);

		output = (error * KP) + (delta * KD) + (integral_error * KI);
		//output =  fabs(error) *((error * KP) + (delta * KD) + (integral_error * KI));
		//output = previous_error + (KP * delta) + (KP*error)/KI + KP*KD*(error - (2*previous_error) + previous_error);

		previous_error = error;

		output = constrain(output,-1,1);
		//uart.printf("%.2f %.2f %.2f %.2f %.2f \n", line_position, consigne, error*KP, integral_error * KI, delta * KD);
		Serial.print(error); Serial.print(",");
		//Serial.print(error * KP);Serial.print(",");
		//Serial.print(delta*KD); Serial.print(",");
		//Serial.print(integral_error * KI);Serial.print(",");
		Serial.println(output);
		last_time = millis();
	}
	return output;
}