//#include <avr/io.h>
#include <avr/wdt.h> // for watchdog timer function
//#include <PololuQTRSensors.h> // for line sensor

//#define NUM_SENSORS   8     // number of sensors used
//#define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low


//function prototypes make them return values later
void readrange(); // range sensor
//void accel(); // accelerometer
//void line(); // line sensor
void motorforward(); // motor forward control

  // global variables can later be moved into a class header file
  const int xPin = 9; // x pin for accelerometer
  const int yPin = 10; // y pin for accelerometer
  int analogInPin = A0; // pin for range sensor
  int m1count; // count for motor 1
  int m2count;
  int m1_1 = 2; // motor 1 pin 2
  int m1_2 = 3; // motor 1 pin 3
  int m1_3 = 4; // motor 1 pin 4
  int m1_4 = 5; // motor 1 pin 5

  int m2_1 = 6; // motor 2 pin 6
  int m2_2 = 7; // motor 2 pin 7
  int m2_3 = 8; // motor 2 pin 8
  int m2_4 = 9; // motor 2 pin 9

  // (line sensor instance variable) Sensors connected to pins 5 to 12
//PololuQTRSensorsRC qtrrc((unsigned char[]) {3,4,5,6,7,8,9,10},  NUM_SENSORS,
//                          TIMEOUT, QTR_NO_EMITTER_PIN);
//unsigned int sensorValues[NUM_SENSORS];

void setup() { // setup pins

  //wdt_enable(WDTO_2S); // threshold interval of 2 seconds for watchdog timer (can go up to 8 seconds)
  // if no signal for more than 2 seconds, reboot MCU
  // wdt_reset();

  Serial.begin(9600); //9600 baud rate

  pinMode(analogInPin, INPUT); // range sensor analog input 0
  pinMode(xPin, INPUT); // accelerometer x digital input 9
  pinMode(yPin, INPUT); // accelerometer y digital input 10

  pinMode(m1_1, OUTPUT);
  pinMode(m1_2, OUTPUT);
  pinMode(m1_3, OUTPUT);
  pinMode(m1_4, OUTPUT);

  pinMode(m2_1, OUTPUT);
  pinMode(m2_2, OUTPUT);
  pinMode(m2_3, OUTPUT);
  pinMode(m2_4, OUTPUT);

}

void loop() { // call functions

motorforward();
//readrange(); // range sensor function
//accel(); // accelerometer function
//line(); // line sensor function
}

void readrange(){  // range sensor function

  float sensorValue; // value of sensor
  int temp; // temporary variable for storing value

  temp = analogRead(analogInPin);
  sensorValue = (6787.0 /((float)temp - 3.0)) - 4.0; // conversion to cm
 // approximately 10 to 80 cm

if(sensorValue <= 80 && sensorValue >= 0){
Serial.println(sensorValue);
}
else {
 Serial.println("out of range");
}

delay(300);
 // pass the sensorValue variable to the motor function

}

void accel(){  // accelerometer function
  int pulseX;
  int pulseY;
  int accelerationX; // in milli-g's
  int accelerationY; // in milli-g's

  pulseX = pulseIn(xPin,HIGH);  // pulseIn function from library
  pulseY = pulseIn(yPin,HIGH);
  // convert the pulse width into acceleration
  // earth gravity (1000 milli-g's)
  // 1/1000 of earth's gravity
  accelerationX = ((pulseX / 10) - 500) * 8;
  accelerationY = ((pulseY / 10) - 500) * 8;

  // formatting and printing
  Serial.print("X=");
  Serial.print(accelerationX);
  Serial.print("\t Y=");
  Serial.print(accelerationY);
  Serial.println();

  delay(300);
}

//void line(){ // line sensor function

// qtrrc.read(sensorValues);

  // print the sensor values
//  unsigned char i;
//  for (i = 0; i < NUM_SENSORS; i++)
//  {
//    Serial.print(sensorValues[i]);
//    Serial.print("   ");
//  }
//  Serial.println(" ");
//  delay(250);

//}

// motor forward function
void motorforward(){

  digitalWrite(m1_1, HIGH);
  digitalWrite(m1_2, LOW);
  digitalWrite(m1_3, LOW);
  digitalWrite(m1_4, LOW);
  delay(20);
  digitalWrite(m1_1, LOW);
  digitalWrite(m1_2, HIGH);
  digitalWrite(m1_3, LOW);
  digitalWrite(m1_4, LOW);
  delay(20);
  digitalWrite(m1_1, LOW);
  digitalWrite(m1_2, LOW);
  digitalWrite(m1_3, HIGH);
  digitalWrite(m1_4, LOW);
  delay(20);
  digitalWrite(m1_1, LOW);
  digitalWrite(m1_2, LOW);
  digitalWrite(m1_3, LOW);
  digitalWrite(m1_4, HIGH);
  delay(20);

  digitalWrite(m2_1, HIGH);
  digitalWrite(m2_2, LOW);
  digitalWrite(m2_3, LOW);
  digitalWrite(m2_4, LOW);
  delay(20);
  digitalWrite(m2_1, LOW);
  digitalWrite(m2_2, HIGH);
  digitalWrite(m2_3, LOW);
  digitalWrite(m2_4, LOW);
  delay(20);
  digitalWrite(m2_1, LOW);
  digitalWrite(m2_2, LOW);
  digitalWrite(m2_3, HIGH);
  digitalWrite(m2_4, LOW);
  delay(20);
  digitalWrite(m2_1, LOW);
  digitalWrite(m2_2, LOW);
  digitalWrite(m2_3, LOW);
  digitalWrite(m2_4, HIGH);
  delay(20);
  }