#include "L298N_MotorDriver.h"
#include <HCSR04.h>

// MOTOR DRIVER
// Make a motor object 
// Arduino Pin 3 ( pin must have PWM capability), 
//   is connected to the driver board pin EN (enable)
// Arduino Pin 2, is connected to the driver board pin IN1 (H-bridge path 1)
// Arduino Pin 4, is connected to the driver board pin IN2 (H-bridge path 2)
// Set the pins for one motor which uses this dual driver board.
L298N_MotorDriver motor(9,8,7); 

int motorMinSpeed   = 60;
int motorMaxSpeed   = 255;
int motorSpeed = motorMinSpeed;
boolean motorDirection = true;
byte speedInc = 1;
int rampDelay = 10;
boolean motorMoving = false;

// Switches between counting up and down 
// to make a ramp up and ramp down for the speed
bool countUp = true; 
byte reversals = 0; //count number of reversals of direction

// SONAR
byte triggerPin = 13;
byte echoPin = 12;
double* distances;
double triggerDistance = 30.00;

byte limitSwitchPin = 3; 

boolean debugMotor = false;
boolean debugSonar = true;
boolean debugRadio = true;

void setup() {
  Serial.begin(9600);
  pinMode(limitSwitchPin, INPUT_PULLUP);
  HCSR04.begin(triggerPin, echoPin);
  Serial.print("Resetting motor...");
  motor.setSpeed(255);      // Sets the speed for the motor. 0 - 255
  motor.setDirection(!motorDirection);  // Sets the direction ( depending on the wiring )
  motor.enable(); //revert motor to original position
  delay(9000);
  Serial.println("done!");
  motor.disable();             // Turns the motor off
}

void loop() {
  if(debugMotor){
    Serial.print("Motor is moving = ");
    Serial.print(motorMoving);
    Serial.print(" (speed: ");
    Serial.print(motorSpeed);
    Serial.print(", reversals: ");
    Serial.print(reversals);
    Serial.println(")");
  }
  if(motorMoving == false){
    distances = HCSR04.measureDistanceCm();
    if(debugSonar){
      Serial.print("1: ");
      Serial.print(distances[0]);
      Serial.println(" cm");
    
      Serial.println("---");
    }
    if(!(distances[0] < 0.00) && (distances[0] < triggerDistance)){
      motorMoving = true;
      motor.enable();             // Turns the motor on
    }
    delay(250);
  }
    
  if(motorMoving == true){
    motor.setSpeed(motorSpeed);  // Sets the new speed
    motor.setDirection(motorDirection);  // Sets the direction ( depending on the wiring )
    delay(rampDelay);              // Wait a bit to make the ramp longer
    
    if(motorSpeed > motorMaxSpeed)
    {
    	// Changed the rising ramp to a falling ramp
    	countUp = false;
    }
    if(motorSpeed < motorMinSpeed)
    {
    	// Changed the falling ramp to a rising ramp
    	countUp = true;
     reversals++;
     motorDirection = !motorDirection;
      if(reversals > 1)
      {
        reversals = 0;
        motorMoving = false;
        motorSpeed = motorMinSpeed;
        motor.disable();
      }
    }
    if(digitalRead(limitSwitchPin) == LOW)
    {
      motor.setSpeed(255);      // Sets the speed for the motor. 0 - 255
      motor.setDirection(false);  // Sets the direction ( depending on the wiring )
      motor.enable(); //revert motor to original position
      delay(9000);
      motor.disable();
      reversals = 0;
      motorMoving = false;
      motorSpeed = motorMinSpeed;
      motor.setDirection(motorDirection);
    }
    
    if(countUp)
    {
  	  motorSpeed = motorSpeed + speedInc;
    }
    else
    {
  	  motorSpeed = motorSpeed - speedInc;
    }
  }
}