#include <MeMCore.h>



MeDCMotor motorL(M1); //positive go backward

MeDCMotor motorR(M2); //positive go forward

MeUltrasonicSensor ultraSensor(PORT_3);

uint8_t motorSpeed = 255;
bool turning = false;
unsigned long turnTimer = 0;
int ranMin = 200;//in ms
int ranMax = 2000;
unsigned long turnTimerDuration = random(ranMin, ranMax);
bool directionLeft = true;
int MinDistance = 20;//in cm

/*
 * if turning, keep turning until turn timer is over and kill turning
 * if not turning, check sensor and carry on moving forward
 *  if sensor within threshold, set turn timer random, direction random, set turning
 * 
 * 
 */

void setup()
{
  Serial.begin(9600);
}

void loop()
{
  if (turning){
    //timercheck
    if(millis()- turnTimer >= turnTimerDuration){
      //reset timer
      turning = false;
      motorL.run(-motorSpeed);
      motorR.run(motorSpeed);      
    }else{
      //keep on turning
    }
  }else{
    //not turning
    if(ultraSensor.distanceCm() < MinDistance)//sensorcheck
    {
      turnTimerDuration = random(ranMin, ranMax);
      directionLeft = random(2);
      
      if(directionLeft){
        motorL.run(motorSpeed);
        motorR.run(motorSpeed);
      }else{
        motorL.run(-motorSpeed);
        motorR.run(-motorSpeed);
      }

      turning = true;
      turnTimer = millis();
    }
    
    
  }
  
  /*
  Serial.print("Distance : ");
  Serial.print(ultraSensor.distanceCm() );
  Serial.println(" cm");
  //delay(100);//docs say at least that timer
  */
  /*motorL.run(motorSpeed); // value: between -255 and 255. 
  /motorR.run(motorSpeed); // value: between -255 and 255. 

  delay(2000);
  motorL.stop();
  motorR.stop();

  delay(100);
  motorL.run(-motorSpeed);
  motorR.run(-motorSpeed);

  delay(2000);
  motorL.stop();
  motorR.stop();

  delay(2000);
  */
}