#define RightForward portd.f5
#define LeftForward portd.f7
#define RightReverse portd.f4
#define LeftReverse portd.f6

//Global Variables
unsigned short cnt;

//Function Prototypes
void SanityBlink(int BlinkGreenLedThisManyTimes, int BlinkDelay);
void InitializePortsAndPeripherals(void);
void Toggle(void);
void MoveForward(long amount);
void SpinRight(long degrees);
void SpinLeft(long degrees);
void CommandCompleted(void);


void interrupt() {
  if (PIR1.TMR2IF) {
    cnt++ ;                   // increment counter
    PIR1.TMR2IF = 0;          // clears TMR2IF
    TMR2  =   0;
    }
}

void main()
{
  InitializePortsAndPeripherals();

  SanityBlink(10, 100);
  
  cnt = 0;                    // initialize cnt


  MoveForward(40);

  SpinRight(90);
  
  MoveForward(40);

  SpinRight(90);

  MoveForward(40);

  SpinRight(90);

  MoveForward(40);

  SpinLeft(360);

  SanityBlink(3,100);
}

void SpinRight(long degrees)
{
  long i;
  long TranslatedDegrees;

  TranslatedDegrees = degrees /4;

  for(i=0;i<TranslatedDegrees;i++)
  {
    LeftForward = 1;
    RightReverse = 1;

    delay_ms(12);

    LeftForward = 0;
    RightReverse = 0;

    delay_ms(20);
  }
   CommandCompleted();
}
void SpinLeft(long degrees)
{
  long i;
  long TranslatedDegrees;

  TranslatedDegrees = degrees /4;

  for(i=0;i<TranslatedDegrees;i++)
  {
    RightForward = 1;
    LeftReverse = 1;

    delay_ms(12);

    RightForward = 0;
    LeftReverse = 0;

    delay_ms(20);
  }
   CommandCompleted();
}

void MoveForward(long amount)
{
  long i;
  
  for(i=0;i<amount;i++)
  {
    LeftForward = 1;
    RightForward = 1;

    delay_ms(20);

    LeftForward = 0;
    RightForward = 0;

    delay_ms(20);
  }
  CommandCompleted();
}

void SanityBlink(int BlinkGreenLedThisManyTimes, int BlinkDelay)
{
  int i;
  //int BlinkGreenLedThisManyTimes;
  //BlinkGreenLedThisManyTimes = 5;

  for(i=0;i<BlinkGreenLedThisManyTimes;i++)
  {
    portb.f2 = 1;
    delay_ms(300);
    portb.f2 = 0;
    delay_ms(300);
  }
}


void Toggle() {
  PORTB = ~PORTB;             // toggle PORTB
  PORTD = ~PORTD;             // toggle PORTD
}

void CommandCompleted()
{
  portd.f0 = 1;
  delay_ms(300);
  portd.f0=0;
  portd.f1=1;
  delay_ms(300);
  portd.f1=0;
}

void InitializePortsAndPeripherals()
{
  ANSEL  = 0;                 // Configure AN pins as digital I/O
	ANSELH = 0;
	
	//PORTS
  PORTB = 0x00;               // Initialize PORTB
  TRISB =   0;                // PORTB is output

  PORTD = 0x00;               // Initialize PORTD
  TRISD =   0;                // PORTD is output

  //Timers
 // T2CON = 0xFF;               // Timer2 settings
 // TMR2  =   0;                // Initialize Timer2 register

  //Interrupts
 // PIE1.TMR2IE = 1;            // enable interupt
 // INTCON = 0xC0;              // Set GIE, PEIE
}