/*
Drive forward and turn left or right when border is detected
  - Only reflectionsensor 0 and 5 are used.
*/
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <NewPing.h>

#define LED 13

// this might need to be tuned for different lighting conditions, surfaces, etc.
#define QTR_THRESHOLD  500//1800 //

// these might need to be tuned for different motor types
#define REVERSE_SPEED     200 // 0 is stopped, 400 is full speed
#define TURN_SPEED        300
#define FORWARD_SPEED     390 * speedModifier
#define REVERSE_DURATION  200 // ms
#define TURN_DURATION     420 // ms

ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12

#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];

ZumoReflectanceSensorArray sensors;


//______________SONAR_____________\\

const int echoPin = A4;
const int triggerPin = A4;
const int maxDistance = 50;
int speedModifier = 1.0;

NewPing sonar(triggerPin, echoPin, maxDistance);


void setup()
{
   sensors.init();
   button.waitForButton();

   Serial.begin(9600);
}


void loop()
{
  //tornado();
  //torpedo();
  attackMode();
  //findCenter();
  //findOpponent();
//  sensors.read(sensor_values);
//
//  if (sensor_values[0] > QTR_THRESHOLD)
//  {
//    // if leftmost sensor detects line, reverse and turn to the right
//    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
//    delay(REVERSE_DURATION);
//    motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
//    delay(TURN_DURATION);
//    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
//  }
//  else if (sensor_values[5] > QTR_THRESHOLD)
//  {
//    // if rightmost sensor detects line, reverse and turn to the left
//    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
//    delay(REVERSE_DURATION);
//    motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
//    delay(TURN_DURATION);
//    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
//  }
//  else
//  {
//    // otherwise, go straight
//    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
//  }
}

//___________________________METHODS______________________________________\\

int findCenter() {
  goToEdge();
  reverse();
  turn180();

  long startTimer = millis();
  goToEdge();
  long endTimer = millis();
  int centerTime = (endTimer - startTimer)/2;
  reverse();
  turn180();

  //Finds center and places
  goForward(centerTime);
  return centerTime;
}

void findOpponent() {
  motors.setSpeeds(0.5*TURN_SPEED,-TURN_SPEED*0.5); //Roterer

  unsigned int time = sonar.ping();
  float distance = sonar.convert_cm(time);
  Serial.println(distance);

  if (distance == 0.0) { // sonar gives zero when outside range
   }
   else {
      goToEdge();
      reverse();
   }
}

void goToEdge(){ // Goes straight forward until it reaches the edge
  sensors.read(sensor_values);
  while (sensor_values[0] > QTR_THRESHOLD && sensor_values[5] > QTR_THRESHOLD) {
    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
    sensors.read(sensor_values);
   }
  motors.setSpeeds(0,0);
}

void turn180(){ // Turns around 180 degrees
  motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
  delay(TURN_DURATION); // HVOR LENGE SKAL DEN VÆRE FOR Å SNU 180 GRADER?
  motors.setSpeeds(0,0);
}

void reverse(){
  motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
  delay(REVERSE_DURATION);
  motors.setSpeeds(0,0);
}

void goForward(int ms){
  motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED + 5);
  delay(ms);
  motors.setSpeeds(0,0);
}

void goBackward(int ms){
  motors.setSpeeds(-FORWARD_SPEED, -FORWARD_SPEED -5 );
  delay(ms);
  motors.setSpeeds(0,0);
}

void attackMode(){
 motors.setSpeeds(0.5*TURN_SPEED,-TURN_SPEED*0.5); //Roterer

  unsigned int time = sonar.ping();
  float distance = sonar.convert_cm(time);
  Serial.println(distance);

  if (distance == 0.0) { // sonar gives zero when outside range
   }
   else {
      if (sensor_values[0] > QTR_THRESHOLD && sensor_values[5] > QTR_THRESHOLD){
        motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
      }
   }
}