#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <MeMCore.h>


// declare motors
MeDCMotor _leftMotor(9);
MeDCMotor _rightMotor(10);
MeUltrasonicSensor ultrasonic(PORT_3);

// declare motor speeds
int _leftMotorSpeed = 0;
int _rightMotorSpeed = 0;

void setup()
{
  Serial.begin(9600);
  // set no speed for left and right motors 
  _leftMotor.run((9)==M1?-(0):(0));
  _rightMotor.run((10)==M1?-(0):(0));
}

void loop()
{
  double distance_in = ultrasonic.distanceInch();
  double distance_cm = ultrasonic.distanceCm();
  Serial.print("Distance: ");
  Serial.print(distance_cm);
  Serial.print(" cm | ");
  Serial.print(distance_in);
  Serial.println(" inches");


  // Move forward
  if (distance_cm > 20) {
      Serial.println("Move forward");
      move(1500, 1500, 100);
  } else{
  // Move backwards
      Serial.println("Move backward");
      move(-100, -100, 500); 

      Serial.println("Turning right");
      move(100, -100, 500);


      delay(100);
  
  // Turn right
  //Serial.println("Turn right");
  //move(100, -100, 2000);
  }

  // Turn left
  //Serial.println("Turn left");
  //move(-100, 100, 2000);
 
}

void move(int leftMotorSpeed, int rightMotorSpeed, int duration)
{
  _leftMotorSpeed = leftMotorSpeed;
  _rightMotorSpeed = rightMotorSpeed;
  
  _leftMotor.run((9)==M1?-(_leftMotorSpeed):(_leftMotorSpeed));
  _rightMotor.run((10)==M1?-(_rightMotorSpeed):(_rightMotorSpeed));
  delay(duration);
  _leftMotor.stop();
  _rightMotor.stop();
  
}