/* Navigation module */
/* By: Benjie Nelson, Nora Levinson, David Sirkin */
/* Dr. Von Sirlevson's Automated Locomotive Computation Engine */

/**************************************************/
/* Includes                                       */
/**************************************************/

#include <timers12.h>

#include "ME218_E128.h"
#include "S12eVec.h"
#include "ADS12e.H"

#include "Navigation.h"
#include "GameDefs.h"
		    
/**************************************************/
/* Module Variables                               */
/**************************************************/

/* Store the current beacon DC and completed runs */

static unsigned int beaconDC;
static char completedRuns;

/**************************************************/
/* High-Level Navigation Functions                */
/**************************************************/

/* 'Pulsing' CCW rotate to overpower turn inertia */

void LookForStart() {
  static char redLightGreenLight;

  
  if(redLightGreenLight) {
    LeftWheel(-60);
    RightWheel(60);

    redLightGreenLight = 0;
  }
  else {
    Stop();

    redLightGreenLight = 1;
  }
}

void AlignWithDispenser(char eventArg) { 
  static int peak, counter;

  static char corrected;
 
  if(eventArg == CLEAR) {

    SetTimer(SM_TMR, 5);
    peak = counter = corrected = 0;
  }

  else {
    int str = BeaconStr();
    SetTimer(SM_TMR, 5);

    
    if(peak == str)
      counter++;
  
    if((peak > str && str > 400)) {

      Stop();
      Wait(150);
    
      if(!corrected) {
        corrected = 1;

      
        if(fieldSide == A) {
          if(completedRuns > 0) {

            LeftWheel(-35);
            RightWheel(35);
            Wait(100);
          }

          else {
            LeftWheel(35);
            RightWheel(-35);

            Wait(100);
          }
        }
        if(fieldSide == B) {
          if(completedRuns > 0) {

            LeftWheel(35);
            RightWheel(-35);
            Wait(100);
          }

          else{
            LeftWheel(-35);
            RightWheel(35);
            Wait(100);
          }
        }
      }

      LeftWheel(40);
      RightWheel(40);
      SetTimer(WAIT_TIMER, 2000);

      
      while(!BumpIntoFT()){
       if(Expired(WAIT_TIMER))
         ReAlignWithDispenser();  
      }
    }

    if(peak < str) {    
      peak = str;  
      counter = 0;
    }
  }
}

void ReAlignWithDispenser(void) {
  LeftWheel(-40);
  RightWheel(-40);

  Wait(1200);
      
  LeftWheel(-40);
  RightWheel(40);

  Wait(300);
      
  FollowBeacon(CLEARCW);
  SetTimer(SM_TMR, 2000);

      
  while(!(Expired(SM_TMR) || BumpIntoFT())) {
    FollowBeacon(RUN);

	
    if(BumpIntoFB()) {
      LeftWheel(-40);
      RightWheel(-40);

      Wait(1200);
      FollowBeacon(CLEARCW);
	}
  }
}

void FollowBeacon(char clearFlag) {

  static char wheelHigh = 55, wheelLow = 35;

  static char LRFlag = 0, peakPassedFlag = 0;

  static int peak, value;
  
  if(clearFlag) {

    peak = 0;
    peakPassedFlag = 0;
    if(clearFlag == CLEARCW)

      LRFlag = 1;
    else
      LRFlag = 0;
  }

  value = BeaconStr();
 
  if(value > peak) {

    peak = value;
    peakPassedFlag = 1;
  }
  if(value > peak * 9 / 10) {

    peakPassedFlag = 1; 
  }
  if(LRFlag) {
    LeftWheel(wheelHigh);

    RightWheel(wheelLow);

    if(peakPassedFlag) {
      if(value < peak * 2 / 3) {

        TMRS12_InitTimer(FOLLOW, FOLLOW_TIME);
        peakPassedFlag = 0;

        LRFlag = 0;
      }
    }
  }
  else {
    LeftWheel(wheelLow);

    RightWheel(wheelHigh);
    
    if(peakPassedFlag) {
      if(value < peak * 2 / 3){

        TMRS12_InitTimer(FOLLOW, FOLLOW_TIME);
        peakPassedFlag = 0;

        LRFlag = 1;
      }
    }
  }
  if(Expired(FOLLOW)) {
    Stop();

    Wait(750);
	TMRS12_InitTimer(FOLLOW, FOLLOW_TIME);
	peakPassedFlag = 0;     
    LRFlag = !LRFlag;
  }
}

/**************************************************/
/* Basket Control Functions                       */
/**************************************************/

/* PWM sets the basket DC to the desired position */

void SetBasket(char position) {
  PWMDTY3 = position * PWM_PRB / 100;
}

/* PWM up/down end positions found experimentally */

void DropBalls(void) {
  int i;

  
  for(i = 88; i > 40; i-=4) {

    SetBasket(i);
    Wait(300);
  }
  for(i = 40; i < 88; i+=4) {

    SetBasket(i);
    Wait(50);
  }
}

/**************************************************/
/* Drive Mode Functions                           */
/**************************************************/

/* Set left/right wheels to desired speed or stop */

void LeftWheel(int DC) {

  DC = DC * -1;
  if(DC >= 0) {

    PWMDTY0 = DC * PWM_PRA / 100;
    PTU &= BIT1LO;
  }

  else {
    PWMDTY0 = PWM_PRA + (DC * PWM_PRA / 100);

    PTU |= BIT1HI;    
  }  
}

void RightWheel(int DC) {

  if(DC <= 0) {
    PWMDTY4 = PWM_PRA + (DC * PWM_PRA / 100);

    PTU |= BIT5HI;
  }
  else {
    PWMDTY4 = DC * PWM_PRA / 100;

    PTU &= BIT5LO;    
  }
}

void Stop(void) {
  LeftWheel(0);

  RightWheel(0);  
}