#include "StepperController.h"
#include "StepperXController.h"
#include "Time.h"
#include <Arduino_FreeRTOS.h>

#define JoystickPin 63

#define grabPin 19

Time time;

void setup(){
    // Start serial communication at 115200 baud
    Serial.begin(115200);
    
    // Set the pin mode for the defined pins
    pinMode(grabPin, INPUT_PULLUP);

    initSteppers();
    initXStepper();
}

bool isGrabbing = false;
float startGrabbingTime = 0;
const float moveToDropZoneTime = 1750; // How long to move right (relative to me)

const int joyCriticalPoint = 10;

void loop(){
  // Updates deltatime
  time.update();

  stepperXLoop();

  // Joystick. TODO: Make this only run when needed. Too lazy now.
  int joystickValueRaw = analogRead(JoystickPin);
  int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
  bool left = joystickValue > 100-joyCriticalPoint;
  bool right = joystickValue < joyCriticalPoint;

  // Serial.print("Left: ");
  // Serial.print(left);
  // Serial.print("    Right: ");
  // Serial.println(right);

  if (isGrabbing && millis() > startGrabbingTime + moveToDropZoneTime) {
    isGrabbing = false;
    processJoystickInput(false, false); // Quickly stop motor
    delay(1000);
    moveStepper(StepperAxis::Head, 10000, 100); // open hand
  }

  processJoystickInput(left, right);

  if (digitalRead(grabPin) == LOW){
      beginGrabSequence();
      //initGrabber();
  }

  // //Serial.print(millis());

  // Serial.print("Left state: ");
  // Serial.print(left);
  // Serial.print("  |  ");
  // Serial.print("Right state: ");
  // Serial.println(right);


  delay(10);  // delay in milliseconds
}

void initGrabber(){
  moveStepper(StepperAxis::Head, 100000, 1000);
}

void beginGrabSequence(){
    moveStepper(StepperAxis::Head, 10000, 100); // open hand
    moveStepper(StepperAxis::Y, 10000, -1100);
    moveStepper(StepperAxis::Head, 1000, -100, 100); // close hand
    moveStepper(StepperAxis::Y, 10000, 1100);

    isGrabbing = true;
    startGrabbingTime = millis();
}