//
// TRUCKBOT
//

#include <Servo.h>

// SN754410 Quad Half-H Driver
int motorPin1= 3;      // 1A
int motorPin2 = 5;     // 2A
int steeringPin1 = 9;  // 4A
int steeringPin2 = 10; // 3A
// Rangefinder Servo
int servopin = 12;
int ServoDirection = 1;
int ServoPos = 0;

long mapArray[RANGEFINGER_FOV];  // stores measure from each degree in the 90 degree FOV
long optionArray[MAX_PATH_OPTIONS];

bool ready = FALSE;              // wait until ready to move

// constants
const int SERVO_START_POS = 45;
const int SERVODELAY = 2;
const int RANGEFINGER_FOV = 90;
const int MAX_PATH_OPTIONS = 5;  // for now, hard/soft right/left and forward.
const int CRUISE_SPEED = 150;    // pwm motor speed, 150/255

const int LEFT = -1;
const int FORWARD = 0;
const int RIGHT = 1;
const int REVERSE = 1;
const int STOP = -1;
const int HARD = 250;
const int SOFT = 150;

void setup()
{
  memset(mapArray, 0, sizeof(mapArray)) // initialize mapArray to 0
  Serial.begin(96000);
  myservo.attach(servoPin);  // attaches the servo on pin 9 to the servo object
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(steeringPin1, OUTPUT);
  pinMode(steeringPin2, OUTPUT);
  myservo.write(SERVO_START_POS);
}

void loop()
{
  buildMapArray(pos);         // populate mapArray[pos] with distance from rangefinder
  if(ServoPos == 45 || [ServoPos == 135)   // servo sweep 45-135 = 90degree FOV
    {
      ServoDirection *= -1;   // servo return sweep
      shrinkArray();          // shrink Maparray to OptionArray[MAX_PATH_OPTIONS]
      calcNextAction();
     }
  while(ready){
    move();}
  ServoPos = ServoPos + ServoDirection;
  myservo.write(pos);
  delay(SERVODELAY);
}

void calcNextAction(){

int dLongest = 0;  // variable to record index of longest distance in mapArry
int dShortest = 0; // variable to record index of shortest distance in mapArry

  for(i=0;i<=MAX_PATH_OPTIONS;i++)
  {
    // Determine longest/shortest distance indexs
    if (mapArray[dLongest] < mapArray[i]){dLongest = i;}
    if (mapArray[dShortest] > mapArray[i]){dShortest = i;}
  }
  switch(i){
    case 0: move(FORWARD, CRUISE_SPEED*.4, LEFT, HARD);
    case 1: move(FORWARD, CRUISE_SPEED*.2, LEFT, SOFT);
    case 2: move(FORWARD, CRUISE_SPEED, FORWARD, HARD);
    case 3: move(FORWARD, CRUISE_SPEED*.2, RIGHT, SOFT);
    case 4: move(FORWARD, CRUISE_SPEED*.4, RIGHT, HARD);
  }
}

void move(int vMotor, byte sMotor, int vSteering, byte sSteering){
  switch (vMotor){
  case 0:
    digitalWrite(motorPin1, LOW);
    analogWrite(motorPin2, sMotor);
    break;
  case -1:
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    break;
  case 1:
    analogWrite(motorPin1, sMotor);
    digitalWrite(motorPin2, LOW);
    break;
  }
  switch (vSteering){
  case -1:
    analogWrite(steeringPin1, sSteering);
    digitalWrite(steeringPin2, LOW);
    break;
  case 0:
    digitalWrite(steeringPin1, LOW);
    analogWrite(steeringPin2, LOW );
    break;
  case 1:
    digitalWrite(steeringPin1, LOW);
    digitalWrite(steeringPin2, sSteering);
    break;
  }
}

void shrinkArray(){
int i = 0;
int n = 0;
int count = 0;
int divisor = RANGEFINGER_FOV / MAX_PATH_OPTIONS;

  for(i=0;i<=MAX_PATH_OPTIONS;i++){
    for(n=0;n<=divisor;n++){OptionArray[i]+=mapArray[count+n];}
    OptionArray[i] =/ divisor;
    count += divisor;
    }
  }
}
void buildMap(){
long duration, inches, cm;
  pinMode(pingPin, OUTPUT);      // RangeFinder is triggered by a HIGH pulse of 2 or more microseconds.
  digitalWrite(pingPin, LOW);    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  mapArray[ServoPos] = cm;       // record measurement in mapArray
}