#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;

//SENSORS ARE NOT NEEDED
//BUT ONE DAY MAYBE

#include <FastLED.h>
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

uint32_t lastTime = 0;

#define M1A 19
#define M1B 18
#define M2A 17
#define M2B 16
#define WSLED 4

long randNumber;

//NOTE
//I DO NOT KNOW IF RIGHT AND LEFT IS ACCURATE
//THE THING MOVES RANDOMLY SO IT DOESNT MATTER
//IN THE END I THINK LOL

//MYSPEED HAS BEEN USELESS BECAUSE THIS THING IS
//SO HEAVY IT REQUIRES MAX SPEED JUST TO GET MOVING

void forward(uint8_t mySpeed){
  ledcWrite(2, mySpeed); //Set M2A to mySpeed
  ledcWrite(3, 0); //Set M2B to LOW
  ledcWrite(0, mySpeed);//Set M1A to mySpeed
  ledcWrite(1, 0); //Set M1B to LOW
}

void reverse(uint8_t mySpeed) {
  ledcWrite(3, mySpeed);     //full reverse
  ledcWrite(2, 0);           //no forward
  ledcWrite(1, mySpeed);     //full reverse
  ledcWrite(0, 0);           //no forward
}


void reverseLeft(uint8_t mySpeed){
  Serial.println("REVERSING");
  ledcWrite(3, mySpeed);     //full reverse
  ledcWrite(2, 0);           //no forward
  ledcWrite(1, 245); //reduced reverse
  ledcWrite(0, 0);           //no forward
}

void reverseRight(uint8_t mySpeed){
  ledcWrite(3, 245); //reduced reverse
  ledcWrite(2, 0);           //no forward
  ledcWrite(1, mySpeed);     //full reverse
  ledcWrite(0, 0);           //no forward 
}

void turnRight(uint8_t mySpeed) {
  ledcWrite(2, mySpeed);        //full speed
  ledcWrite(3, 0);              //no reverse
  ledcWrite(0, 245);    //reduced speed
  ledcWrite(1, 0);              //no reverse
}

void turnLeft(uint8_t mySpeed) {
  ledcWrite(2, 245);    //reduced speed
  ledcWrite(3, 0);              //no reverse
  ledcWrite(0, mySpeed);        //full speed
  ledcWrite(1, 0);              //no reverse
}

void fullStop() {
  ledcWrite(2, 0);    //no speed
  ledcWrite(3, 0);    //no reverse
  ledcWrite(0, 0);    //no speed
  ledcWrite(1, 0);    //no reverse
}

void fireCannon(uint8_t mySpeed) {
  reverse(255);
  delay(100);
  fullStop();
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  
  ledcSetup(0, 1000, 8); //sets up PWM control
  ledcAttachPin(M1A, 0);
  ledcSetup(1, 1000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 1000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 1000, 8);
  ledcAttachPin(M2B, 3);

  randomSeed(analogRead(0));
}

void loop() {
  int actionChoice = random(1,10); //the ifv chooses its actions
  if(actionChoice == 1) {
    forward(255);
    Serial.println("forward max");
  } else if (actionChoice == 2) { //actually stalls the robot due to excessive weight
    forward(235);
    Serial.println("forward med");
  } else if (actionChoice == 3) {
    forward(255);
    Serial.println("forward high");
  } else if (actionChoice == 4) {
    reverseLeft(255);
    Serial.println("reverse left");
  } else if (actionChoice == 5) {
    reverseRight(255);
    Serial.println("reverse right");
  } else if (actionChoice == 6) {
    int shotsFired = random(1,8);
    Serial.println("firing multi");
    for(int i = 1; i <= shotsFired; i++) { //fires cannon a random amount 1-7
      fullStop();
      fireCannon(255);
      delay(500);
    }
  } else if (actionChoice == 7) {
    Serial.println("firing single"); //fires single shot
    fullStop();
    delay(1200);
    fireCannon(255);
    delay(500);
  } else if (actionChoice == 8) {
    turnRight(255);
    Serial.println("turn right");
  } else if (actionChoice == 9) {
    turnLeft(255);
    Serial.println("turn left");
  } else if (actionChoice == 10) {
    Serial.println("firing single");
    fullStop();
    delay(1200);
    fireCannon(255);
    delay(500);
  } else if (actionChoice == 11) {
    int shotsFired = random(1,8);
    Serial.println("firing multi");
    for(int i = 1; i <= shotsFired; i++) { //just another chance to fire multiple rounds
      fullStop();
      fireCannon(255);
      delay(500);
    }
  }
  delay(3000); //makes actions actually last.
               //could do random(1000,3000); to make it have variance in its durations
}