 #define Function_2	56
 #define Function_3      57
 
-//object instantition 
+//object instantition
 Servo Driveleft;
 Servo Driveright;
 Servo Turret;

};

//global variables
int boxcar[MovingAverageWidth*NoAverageDataSets];
int boxcar_index[NoAverageDataSets];

int priority =0;
int driveVal =0;

struct boxcar pings[1];
struct w  walls[2];


void setup() {
  
  Driveleft.attach(Dleft);
  Driveleft.write(DleftStop);
  Driveright.attach(Dright);
  Driveright.write(DrightStop);
 // Turret.attach(turret);
 // Turret.write(TurretCenter);
  
  pinMode(LDRLeft, INPUT);
  pinMode(LDRCenter, INPUT);
  pinMode(LDRRight, INPUT);
  pinMode(LDRTop, INPUT);
  pinMode(PB1,INPUT);
  
  pinMode(SharpIrEnable, OUTPUT);
  pinMode(LEDRed, OUTPUT);
  
  Serial.begin(9600);  
  
  //ini the moving average array
  for(int i = 0; i < MovingAverageWidth; i++){      
      boxcar_addValue(analogRead(LDRTop),BrightnessTop);
      delay(2);
  }    
  //ini the moving average for sonars
  for(int p =1;p<=4;p++){
      for(int i = 0; i < MovingAverageWidth; i++){   
          readSonar(p);
          delay(2);
      }
  }  
}


void loop(){
    driveVal =0;
    priority =0;
    //Serial.println("BOOP LOOP");
    boxcar_addValue(analogRead(LDRTop),BrightnessTop);
    readSonar(1);
    readSonar(2);
    readSonar(3);
    readSonar(4);
    //Serial.print("LDRTop = ");
    //Serial.println(boxcarGetAverage(BrightnessTop));
    //Serial.print("sonar 3 says ");
    //Serial.println(boxcarGetAverage(3));
    //Serial.println(sonar1.ping());
    //Serial.println("avoid");
    avoider();
    //Serial.println("sense");
    sensitivity();
    //Serial.println("follow");
    //followWall();
    
    Serial.print(priority);
    Serial.println(driveVal);
    motor_basic(driveVal);
    delay(500);
    motor_basic(1);
    delay(500);
}


void followWall(){
    int range = pings[3].average;
    if( range <= 7){
        motor_queue(2,3);
    }else if(range >= 7){
        motor_queue(2,2);
    }
}


void avoider(){
    //int leftRange = boxcarGetAverage(2);
    //int rightRange = boxcarGetAverage(3);
   if(pings[1].average <= 5 || pings[2].average <= 5 || pings[0].average <= 5 || pings[3].average <= 5){
      motor_queue(3,3);
       //delay(500);
   }
}


void sensitivity(){
    int brightness = boxcarGetAverage(BrightnessTop);
    if ( brightness < Dark){
        motor_queue(4,0);
    }else if(brightness > Light){
        //motor_basic(2);
    }else{
        motor_queue(0,1);
    }    
}


void readSonar(int pingNo){
    int range = 0;
    switch (pingNo) {
        case 1:
            pings[0] = sonar1.ping_cm();
             
            break;
        case 2:
            pings[1] = sonar2.ping_cm();
            break;
        case 3:
            pings[2] = sonar3.ping_cm();
            break;
        case 4:
            pings[3] = sonar4.ping_cm();
            break;
    } 
      //Serial.println(pings[1]);
      //Serial.println(pings[3]+" " + pings[2]);
    //boxcar_addValue(range,pingNo);
}


void remote(int val) {
  switch (val) {
      case Forward:
          //move forward
          motor_basic(1);
          break;
      case Backward:
          //move backwards
          motor_basic(1);
          break;
      case Turret_Left:
          //move turret left
          break;
      case Turret_Right:
          //move Turret right
          break;
      case Pivot_Left:
          //pivot robot left
          motor_basic(2);
          break;
      case Pivot_Right:
          //pivot robot right
          motor_basic(3);
          break;
      case Stop:
          //stop robot
          motor_basic(0);
          break;
      case Function_1:
          //implement function 1
          break;
      case Function_2:
          //implement function 2
          break;
      case Function_3:
          //implement function 2
          break;
      default:
          //stuff perhaps
          break;
  }
}

void motor_queue(int pri, int mot){
    if(pri > priority){
        priority = pri;
        driveVal = mot;
    }
}

   
void motor_basic(int dir) {
  switch (dir) {
    case 0:
      //Stop motors
      Driveleft.write(DleftStop );
      Driveright.write(DrightStop);
      break;    
    case 1:
      //Drive forwards
      Driveleft.write(DleftStop + Speed);

};

struct boxcar {
  int vals[NoAverageDataSets];
  int index;
  int delta;
};
