#include "UserAPI.h"

#define OUTPUT	0
#define INPUT	1

void GoLeft(void);
void GoRight(void);
void AllStop(void);
void GoForward(void);
void goDump(void);

void IO_Initialization(void)
{
	DefineControllerIO ( 5, INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,INPUT ,OUTPUT ,OUTPUT ,OUTPUT ,OUTPUT ,OUTPUT ,OUTPUT ) ;
}
int flag; // 0 = auto, 1=RC
int i; // loop counter
int rcInput; // Input from Radio ch 6
int LeftLine; // Input from left line sensor input1
int rightLine; // Input from right Line sensor input 3
int centerLine; // Input from center line sensor

int FWD = 200; // Forward PWM value
int STP = 127; // Stop PWM value
int REV = 55; // Reverse PWM value
int SLOW = 185; // SLOW PWM MODe
int REVSLOW = 80; 
int lastLine = 0; // Last Direction of line detected
int min = 200; // lowest IR level detected
int max = 500; // highest IR level detected
int avg = 300; // min-max/2 = trigger IR level
int DumpFlag = 0; // 1 = time to dump!
int IRleft ; // IR Prox detector left
int IRright ; // IR Prox detector right


void main ( void )
{
      i = 0 ;
      flag = 1; ; // start in autonomous mode
      Wait ( 1000 ) ;
      while ( i == 0 ) // infinite loop
      {

            if ( flag == 0 ) // if autonomous mode
            {
                  rcInput = GetDigitalInput ( 9 ) ; // Switch modes
                  if ( rcInput == 0 ) // switch mode
                  {
                        flag = 1 ; // switch to RC mode
                        AllStop();
                        Wait ( 500 ) ; // Debounce the switch
                  }

                  LeftLine = GetAnalogInput ( 3 ) ;
                  centerLine = GetAnalogInput ( 2 ) ;
                  rightLine = GetAnalogInput ( 1 ) ;
                  if ( rightLine < min )
                  {
                        min = rightLine ;
                  }
                  if ( rightLine > min )
                  {
                        max= rightLine ;
                  }
                  avg = min + ((max-min) / 2) ;
                  avg = min+200 ;
                  if ( LeftLine <avg && rightLine < avg && centerLine < avg ) // all sensors activated at once
                  {
                        DumpFlag = 1 ;
                        GoForward();
                        Wait ( 400 ) ;
                        continue;
                  }
                  if ( centerLine < avg )
                  {
                        GoForward();
                        if ( DumpFlag == 1 )
                        {
                              goDump();
                        }
                        continue;
                  }
                  if ( rightLine  < avg ) // if value < 100 then we are over line
                  {
                        GoRight();
                        lastLine = 1 ; // Last Detected Right Turn
                        continue;
                  }
                  if ( LeftLine < avg ) // if value > 100 then we are over line
                  {
                        GoLeft();
                        lastLine = 0 ; // last detected left line
                        continue;
                  }
                  if ( lastLine == 0 ) // last line detected
                  {
                        SetMotor ( 1 , SLOW ) ;// this is a more agressive turn than a line correction
                        SetMotor ( 8 , SLOW ) ;
                        SetMotor ( 2 , FWD ) ;
                        SetMotor ( 7 , FWD ) ;
                        continue;
                  }
                  else
                  {
                        SetMotor ( 1 , REV ) ;// more agressive turn
                        SetMotor ( 8 , REV ) ;
                        SetMotor ( 2 , REVSLOW ) ;
                        SetMotor ( 7 , REVSLOW ) ;
                        continue;
                  }
            }// end of IF FLAG==0 block i.e. autonomous mode
            else // // rc mode
            {
                  Arcade4 ( 0 , 2 , 1 , 1 , 2 , 8 , 7 , 0 , 0 , 0 , 0 ) ;
                  MotorRcControl ( 0 , 6 , 6 , 0 ) ;
                  ServoRcControl ( 0 , 4 , 4 , 0 ) ;
                  ServoRcControl ( 0 , 3 , 3 , 0 ) ;
                  rcInput = GetDigitalInput ( 9 ) ; // switch modes
                  if ( rcInput ==0 )
                  {
                        flag = 0 ; // switch to autonomous mode
                        Wait ( 500 ) ; // Debounce the switch
                  }
            }
      }
}

void GoLeft()
{
		SetMotor ( 2 , SLOW ) ;
        SetMotor ( 7 , SLOW ) ;
        SetMotor ( 1 , 127 ) ;
        SetMotor ( 8 , 127 ) ;
	return;
}

void GoRight()
{
	SetMotor ( 1 , REVSLOW ) ;
    SetMotor ( 8 , REVSLOW ) ;
    SetMotor ( 2 , 127 ) ;
    SetMotor ( 7 , 127 ) ;
	return;
}

void AllStop()
{
	SetMotor(1,127);
	SetMotor(2,127);
	SetMotor(7,127);
	SetMotor(8,127);
	return;
}
void GoForward()
{
	SetMotor(1,REV);
	SetMotor(8,REV);
	SetMotor(7,FWD);
	SetMotor(2,FWD);
}
void goDump()
{
	int enc = 0; // encoder value
	DumpFlag = 0 ;
    AllStop();
    SetMotor ( 6 , 0 ) ;
    StartEncoder(1);
	enc = 0;
	PresetEncoder(1,0);
	while (enc <= 25)
	{
		enc = GetEncoder(1);
	}
    SetMotor ( 6 , 127 ) ;
    Wait(2000); // hold still, wait for paper to dump out
    SetMotor ( 6 , 255 ) ;
	enc = 0;
	StartEncoder(1);
	PresetEncoder(1,0);
    while (enc <= 25)
	{
		enc = GetEncoder(1);
	}
    SetMotor ( 6 , 127 ) ;
	StopEncoder(1);
	return;
}