/*CODE YOU CAN USE
 * just copy and paste in any order.
 * Be sure to change the numbers
wait(2); //number time in seconds 0.1 to 5. You can use decimal numbers
forward(0.5, 50); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
backward(0.5, 50); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
left(0.5, 50); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
right(0.5, 50); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
spinRight(0.5, 30); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
spinLeft(0.5, 100); //first number is time in seconds 0.1 to 2, second number is speed from 0 to 100
accelForward(50); //number is max speed from 0 to 100
deccelForward(50); //number is max speed from 0 to 100):
accelBackward(50); //number is max speed from 0 to 100);
deccelBackward(50); //number is max speed from 0 to 100);
*/

void doOnce(){
  //code you put in here will happen only once
  wait(2);
  paintWheels();
  wait(5);
  //put your one time code here



  //yep up there
}
void doAgainandAgain(){
  //code you put in here will happen again and again


  //yep up there
}









//oops dont touch anything down here

//define pin numbers
#define LeftMotorA D4
#define LeftMotorB D5
#define LeftMotorEnable D3
#define RightMotorA D6
#define RightMotorB D7
#define RightMotorEnable D8

const int SpeedMin = 180;
const int SpeedMax = 1023;
const int DurationMin = 100;
const int DurationMax = 2000;
const int WaitMin = 100;
const int WaitMax = 5000;

void feedWatchdog(){
  ESP.wdtFeed();
}
void accelForward(int topSpeed){
  // accelerate from zero to maximum speed
  for (int i = 0; i <= topSpeed; i=(i+(topSpeed/10))){
    forward(0.1,i);
  }
}
void deccelForward(int topSpeed){
  // decelerate from maximum speed to zero
  for (int i = topSpeed; i >= 0; i=(i-(topSpeed/10))){
    forward(0.1,i);
  }
}

void accelBackward(int topSpeed){
  // accelerate from zero to maximum speed
  for (int i = 0; i <= topSpeed; i=(i+(topSpeed/10))){
    backward(0.1,i);
  }
}
void deccelBackward(int topSpeed){
  // decelerate from maximum speed to zero
  for (int i = topSpeed; i >= 0; i=(i-(topSpeed/10))){
    backward(0.1,i);
  }
}

void paintWheels(){

  accelForward(100);
  forward(1,100);
  deccelForward(100);
  accelBackward(100);
  backward(1,100);
  deccelBackward(100);
  /*//old option
  forward(3,50);
  delay(300);
  backward(3,50);
  */
}

void enableSpeed(int val){
  val = mapSpeed(val);
  analogWrite(LeftMotorEnable, val);
  analogWrite(RightMotorEnable, val);
}

void disableSpeed(){
  analogWrite(LeftMotorEnable, LOW);
  analogWrite(RightMotorEnable, LOW);
}

void pinsLow(){
  digitalWrite(LeftMotorA, LOW);
  digitalWrite(LeftMotorB, LOW);
  digitalWrite(RightMotorA, LOW);
  digitalWrite(RightMotorB, LOW);
}

void stopMotors(){
  disableSpeed();
  pinsLow();
}

void leftBackward(){
  digitalWrite(LeftMotorA, HIGH);
  digitalWrite(LeftMotorB, LOW);
}
void leftForward(){
  digitalWrite(LeftMotorA, LOW);
  digitalWrite(LeftMotorB, HIGH);
}
void rightBackward(){
  digitalWrite(RightMotorA, LOW);
  digitalWrite(RightMotorB, HIGH);
}
void rightForward(){
  digitalWrite(RightMotorA, HIGH);
  digitalWrite(RightMotorB, LOW);
}
void forward(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  leftForward();
  rightForward();
  motorDuration(duration);
  stopMotors();
}
void backward(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  leftBackward();
  rightBackward();
  motorDuration(duration);
  stopMotors();
}
void left(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  rightForward();
  motorDuration(duration);
  stopMotors();
}
void right(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  leftForward();
  motorDuration(duration);
  stopMotors();
}
void spinLeft(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  leftBackward();
  rightForward();
  motorDuration(duration);
  stopMotors();
}
void spinRight(float duration, int fast){
  stopMotors();
  enableSpeed(fast);
  leftForward();
  rightBackward();
  motorDuration(duration);
  stopMotors();
}

int mapSpeed(int val){
  val = constrain(val, 1, 100);
  val = map(val, 1, 100, SpeedMin, SpeedMax);
  return val;
}

int mapDuration(int val){
  val = constrain(val, 0.1, 2.0);
  val = map(val, 0.1, 2.0, DurationMin, DurationMax);
  val = (int) val;
  return val;
}

void motorDuration(float val){
  val = mapDuration(val);
  val = (int) val;
  feedWatchdog();
  delay(val);
}

void wait(float val){
  val = constrain(val, 0.1, 5.0);
  val = map(val, 0.1, 5.0, WaitMin, WaitMax);
  val = (int) val;
  feedWatchdog();
  digitalWrite(LED_BUILTIN, LOW);
  delay(val);
  digitalWrite(LED_BUILTIN, HIGH);
}

/* how to map and constrain

map(value, fromLow, fromHigh, toLow, toHigh)

sensVal = constrain(sensVal, LowVal, HighVal);
// limits range of sensor values to between 10 and 150

*/


void setup() {
  // put your setup code here, to run once:

  //set pinouts
  //   LED pin
  pinMode(LED_BUILTIN, OUTPUT);
  //   LeftMotor Pins ,OUTPUT);
  pinMode(LeftMotorA,OUTPUT);
  pinMode(LeftMotorB,OUTPUT);
  pinMode(LeftMotorEnable,OUTPUT);
  //   RightMotor Pins
  pinMode(RightMotorA,OUTPUT);
  pinMode(RightMotorB,OUTPUT);
  pinMode(RightMotorEnable,OUTPUT);

  ESP.wdtDisable();
  feedWatchdog();

  doOnce();
}

void loop() {
  // put your main code here, to run repeatedly:
  doAgainandAgain();

  //tiny loop delay
  delay(50);
}