/*
  Cael Beckley, Pranav Pandrangi
  ENGR 103
  6-7-2024
  Drawing Robot
*/


#include <esp32-hal-ledc.h>
#define BB1 4
#define BB2 0
#define BB3 2
#define BB4 15

#define LMF 19
#define LMB 18
#define RMF 17
#define RMB 16

void setup() {
  //Set up pins
  Serial.begin(115200);
  pinMode(LMF, OUTPUT);
  ledcAttach(LMF, 30000, 8);
  pinMode(LMB, OUTPUT);
  ledcAttach(LMB, 30000, 8);
  pinMode(RMF, OUTPUT);
  ledcAttach(RMF, 30000, 8);
  pinMode(RMB, OUTPUT);
  ledcAttach(RMB, 30000, 8);
  pinMode(BB1, INPUT_PULLUP);
  pinMode(BB2, INPUT_PULLUP);
  pinMode(BB3, INPUT_PULLUP);
  pinMode(BB4, INPUT_PULLUP);
}

int Forward = 0;
int Left = 0;
int Right = 0;
int LastState = 0;
int Direction = 1;

void loop() {
  //Read the four buttons for input
  int Button1, Button2, Button3, Button4;
  Button1 = !digitalRead(BB1);
  Button2 = !digitalRead(BB2);
  Button3 = !digitalRead(BB3);
  Button4 = !digitalRead(BB4);

  //Forward starts at highest speed, then decelerates
  //Helps it tip only when starting, not stopping
  if (Button2 && Forward == 0){
    Forward = 255;
  }
  else if (Button2){
    Forward -= 5;
    if (Forward < 220){
      Forward = 220;
    }
  }
  else {
    Forward = 0;
  }

  //Sets states for the turning buttons
  if (Button1){
    Left = 1;
  }
  else if (Left){
    Left = 0;
  }
  if (Button3){
    Right = 1;
  }
  else if (Right){
    Right = 0;
  }


  //When Button 4 is pressed, Direction changes 1 <> -1
  if (Button4 && !LastState){
    Direction *= -1;
    LastState = 1;
  }
  else if (!Button4 && LastState){
    LastState = 0;
  }

  /*
  Button2 causes bot to go forward
  If a direction is held at the same time as B2, it will turn
  */
  //For forward movement
  if (Direction == 1){
    
    //The only time the left wheel should be going 
    //is when B2 is pressed and B1 is not
    if (Forward > 0 && !Left){
      ledcWrite (LMF, Forward);
    }
    else{
      ledcWrite (LMF, 0);
      ledcWrite (LMB, 0);
    }

    //Only moves right wheel when B2 and !B3
    if (Forward > 0 && !Right){
      ledcWrite(RMF, Forward);
    }
    else{
      ledcWrite (RMF, 0);
      ledcWrite (RMB, 0);
    }
  }


  //This is identical to Direction == 1 except
  //Right and Left are switched and Forward and Back are switched
  //Makes controls identical when switching directions
  else{
    if (Forward > 0 && !Left){
      ledcWrite (RMB, Forward);
    }
    else{
      ledcWrite (RMB, 0);
      ledcWrite (RMF, 0);
    }

    if (Forward > 0 && !Right){
      ledcWrite(LMB, Forward);
    }
    else{
      ledcWrite (LMB, 0);
      ledcWrite (LMF, 0);
    }
  }

  Serial.print("F - ");
  Serial.print(Forward);
  Serial.print("\tL - ");
  Serial.print(Left);
  Serial.print("\tR - ");
  Serial.println(Right);
  Serial.print("S - ");
  Serial.print(LastState);
  Serial.print("\tD - ");
  Serial.println(Direction);
  delay(100);
}