#include "manuever.h"

bool alignR()
{
int prev_Perror = 0;
int Ierror = 0;
uint8_t stability = 0;
int cycles = 0;
while (stability < 50) {
float kp = .5, ki = 0.01, kd = 0;
//if (read0() == 's') return 0;
	  uint16_t	IRfr = analogRead(1),
				IRbr = analogRead(2),
				IRfl = analogRead(3),
				IRbl = analogRead(4);

	int Perror  = IRfr - IRbr;
	         Ierror += Perror;
	int Derror  = Perror - prev_Perror;
			 prev_Perror = Perror; 	 

	int newVal = Perror*kp + Ierror*ki + Derror*kd;

	//printNln(0,newVal);
    if(abs(newVal > 5)) 
	{
	  stability = 0;
	  cycles++;
	}
	else 
	{
	  stability++;
	  if (stability > 25) cycles = 0;
	}

	if (cycles > 50) 
	{
		//printS(0,"UNSTABLE");
    	return 0;
	}
	if (newVal > 100) spinL(100);
	else if (newVal < - 100) spinR(100);
	else if (newVal > 1) spinL(newVal);
	else if (newVal < -1) spinR(-newVal);
	else stop();
	delay(60);
	}
	 // printS(0,"STEADY");
	  delay(50);
	  return 1;
}

bool alignL()
{
int prev_Perror = 0;
int Ierror = 0;
uint8_t stability = 0;
int cycles = 0;
while (stability < 50) {
float kp = .5, ki = 0.01, kd = 0;
//if (read0() == 's') return 0;
	  uint16_t	IRfr = analogRead(1),
				IRbr = analogRead(2),
				IRfl = analogRead(3),
				IRbl = analogRead(4);

	int Perror  = IRbl - IRfl;
	         Ierror += Perror;
	int Derror  = Perror - prev_Perror;
			 prev_Perror = Perror; 	 

	int newVal = Perror*kp + Ierror*ki + Derror*kd;

	//printNln(0,newVal);
    if(abs(newVal > 5)) 
	{
	  stability = 0;
	  cycles++;
	}
	else 
	{
	  stability++;
	  if (stability > 25) cycles = 0;
	}

	if (cycles > 50) 
	{
		//printS(0,"UNSTABLE");
    	return 0;
	}
	if (newVal > 100) spinL(100);
	else if (newVal < - 100) spinR(100);
	else if (newVal > 1) spinL(newVal);
	else if (newVal < -1) spinR(-newVal);
	else stop();
	delay(60);
	}
	  //printS(0,"STEADY");
	  delay(50);
	  return 1;
}