#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include <AccelStepper.h>
#include <math.h>

#define TFT_DC 9
#define TFT_CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);


// Motor Connections (constant current, step/direction bipolar motor driver)
const int dirPin = 5;
const int stepPin = 6;

AccelStepper myStepper(AccelStepper::DRIVER, stepPin, dirPin);

/************ MENU **************/

int activeMenu = 0;
int activeItem = 1;

const int modeMainMenu = 0;
const int modeSubMenu = 1;
const int modeValMenu = 2;
const int modeSelectVal = 3;
int menuMode = modeMainMenu;

const int modePanDistance = 0;
const int modePanSpeed = 1;
const int modeRotDistance = 2;
const int modeRotSpeed = 3;
const int modeCamDistance = 4;
const int modeTrackDistance = 5;
const int modeTrackSpeed = 6;
int modeValSet;

// Settings for movement
int panDistance;
int panSpeed;
int rotateDistance;
int rotateSpeed;
int camDistance;
int trackDistance;
int trackSpeed;
int currentVal;

int maxPanDistance = 4000;
int maxSpeed = 1000;
int maxRotate = 800;
int maxObjDistance = 1200; // millimeters - about 4'


const int mainMenu = 0;
const int panMenu = 1;
const int rotateMenu = 2;
const int panRotateMenu = 3;
const int trackMenu = 4;

String arrMainMenu[5] = {"CAMERA SLIDER", "Pan", "Rotate", "Pan and Rotate", "Track Object"}; // Menu 0
String arrPanMenu[5] = {"PAN", "Pan Distance", "Pan Speed", "GO", "BACK"}; // Menu 1
String arrRotateMenu[5] = {"ROTATE", "Rot. Distance", "Rot. Speed", "GO", "BACK"}; // Menu 2
String arrPanRotateMenu[7] = {"PAN ROTATE", "Pan Distance", "Pan Speed", "Rot. Distance", "Rot. Speed", "GO", "BACK"}; // Menu 3
String arrTrackMenu[6] = {"TRACK OBJECT", "Dist to Cam", "Dist to Track", "Pan Speed", "GO", "BACK"};  // Menu 4

// At top so all functions can use it
void printToScreen(int pX, int pY, String txt, bool inverted) {
  tft.setCursor(pX, pY);
  if (inverted == true) {
    tft.setTextColor(ILI9341_YELLOW);
  }
  tft.println(txt);
  tft.setTextColor(ILI9341_WHITE);
}

void drawMenu(String menu[], int itemCount, int curMenu) {
  tft.fillScreen(ILI9341_BLACK);
  printToScreen(0, 2, menu[0], false);
  int yPos = 16;
  // For menus that scroll
  int startItem = (activeItem > 4) ? activeItem - 3 : 1;
  for (int i = startItem; i <= itemCount; i++) {
    if (activeItem == i) {
      printToScreen(12, yPos, menu[i], true);
    } else {
      printToScreen(12, yPos, menu[i], false);
    }
    int tmpVal = getValFromMenuAndIndex(curMenu, i);
    if (tmpVal != 0) {
      printToScreen(200, yPos, String(tmpVal), false);
    }
    yPos += 12;
  }
}

// Menu array, itemCount, mode
void drawMainMenu() {
  drawMenu(arrMainMenu, 2, mainMenu);
}
void drawPanMenu() {
  drawMenu(arrPanMenu, 4, panMenu);
}
void drawRotateMenu() {
  drawMenu(arrRotateMenu, 4, rotateMenu);
}
void drawPanRotateMenu() {
  drawMenu(arrPanRotateMenu, 7, panRotateMenu);
}
void drawTrackMenu() {
  drawMenu(arrTrackMenu, 6, trackMenu);
  return;
}

void drawActiveMenu() {
  switch (activeMenu) {
    case mainMenu:
      menuMode = modeMainMenu;
      drawMainMenu();
      break;
    case panMenu:
      menuMode = modeSubMenu;
      drawPanMenu();
      break;
    case rotateMenu:
      menuMode = modeSubMenu;
      drawRotateMenu();
      break;
    case panRotateMenu:
      menuMode = modeSubMenu;
      drawPanRotateMenu();
      break;
    case trackMenu:
      menuMode = modeSubMenu;
      drawTrackMenu();
      break;
  }
}

// Clumsy but no len() property we can access
// Number of value items in a menu
int getMaxItemsForMenu() {
  int maxItems;
  switch (activeMenu) {
    case mainMenu:
      maxItems = 2; //4 when adding back pan/rotate and track
      break;
    case panRotateMenu:
      maxItems = 6;
      break;
    case trackMenu:
      maxItems = 5;
     break;
    default: // panMenu, rotateMenu
      maxItems = 4;
  }
  return maxItems;
}
int getMaxValForSetting(int mode) {
  switch(mode) {
    case modePanDistance:
      return maxPanDistance;
      break;
    case modeRotDistance:
      return maxRotate;
    case modePanSpeed:
    case modeRotSpeed:
      return maxSpeed;
      break;
    case modeCamDistance:
    case modeTrackDistance:
      return maxObjDistance;
      break;
  }
}

void updateVal(int rotationCounter) {
  tft.fillScreen(ILI9341_BLACK);
  printToScreen(0, 0, getValType(), false);
  printToScreen(0, 20, String(rotationCounter), false);
}

/*********** ROTARY ENCODER ***********/

// Rotary encoder pins
#define PIN_A 3     // DT (CLK)
#define PIN_B 2    // SW (DT)
#define PUSH_BTN 4  // CLK (SW)

// A turn counter for the rotary encoder (negative = anti-clockwise)
int rotationCounter = 1;

// Flag from interrupt routine (moved=true)
volatile bool rotaryEncoder = false;

// Interrupt routine just sets a flag when rotation is detected
void rotary()
{
    rotaryEncoder = true;
}

// Rotary encoder has moved (interrupt tells us) but what happened?
// See https://www.pinteric.com/rotary.html
int8_t checkRotaryEncoder()
{
    // Reset the flag that brought us here (from ISR)
    rotaryEncoder = false;

    static uint8_t lrmem = 3;
    static int lrsum = 0;
    static int8_t TRANS[] = {0, -1, 1, 14, 1, 0, 14, -1, -1, 14, 0, 1, 14, 1, -1, 0};

    // Read BOTH pin states to deterimine validity of rotation (ie not just switch bounce)
    int8_t l = digitalRead(PIN_A);
    int8_t r = digitalRead(PIN_B);

    // Move previous value 2 bits to the left and add in our new values
    lrmem = ((lrmem & 0x03) << 2) + 2 * l + r;

    // Convert the bit pattern to a movement indicator (14 = impossible, ie switch bounce)
    lrsum += TRANS[lrmem];

    /* encoder not in the neutral (detent) state */
    if (lrsum % 4 != 0)
    {
        return 0;
    }

    /* encoder in the neutral state - clockwise rotation*/
    if (lrsum == 4)
    {
        lrsum = 0;
        return 1;
    }

    /* encoder in the neutral state - anti-clockwise rotation*/
    if (lrsum == -4)
    {
        lrsum = 0;
        return -1;
    }

    // An impossible rotation has been detected - ignore the movement
    lrsum = 0;
    return 0;
}

// DEBOUNCE PUSHBUTTON
long lastDebounceTime = 0;
long debounceDelay = 200;

// ----- END ROTARY ENCODER 

bool goStep = false;

void setup()
{
  Serial.begin(115200);
  tft.begin();

  myStepper.setMaxSpeed(1000.0);

  // The module already has pullup resistors on board
  pinMode(PIN_A, INPUT);
  pinMode(PIN_B, INPUT);

  // But not for the push switch
  pinMode(PUSH_BTN, INPUT_PULLUP);

  // We need to monitor both pins, rising and falling for all states
  attachInterrupt(digitalPinToInterrupt(PIN_A), rotary, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_B), rotary, CHANGE);
  
  // Initialize LCD screen
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  
  // Setup splash screen
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(0, 1);
  tft.println("CAMERA SLIDER");
  tft.setCursor(0, 3);
  tft.println("* Linear movement,");
  tft.println("* Rotation, and");
  tft.println("* Object Tracking");
  delay(1000);
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(0, 0);
  Serial.println("Initial Setup complete");

  // So we can print values that are set in menus
  resetVars();
  drawActiveMenu();
}


void loop()
{
    // Has rotary encoder moved?
    if (rotaryEncoder)
    {
      // Get the movement (if valid)
      int8_t rotationValue = checkRotaryEncoder();
      // If valid movement, do something
      if (rotationValue != 0)
      {
        encoderRotate(rotationValue);
      }
    }

    if (digitalRead(PUSH_BTN) == LOW)
    {
      if ( (millis() - lastDebounceTime) > debounceDelay) {
        Serial.print("X");
        Serial.println(rotationCounter);
        pushbutton();
        lastDebounceTime = millis(); //set the current time
      }
    }

    if (goStep == true) {
      runStepper();
    }
}

void encoderRotate(int rotationValue) {
  int maxVal;
  // Switch between drawing menu items and value selection
  switch (menuMode) {
    case modeMainMenu:
    case modeSubMenu:
      maxVal = getMaxItemsForMenu();
      rotationCounter += rotationValue;
      if (rotationCounter > maxVal) rotationCounter = maxVal;
      if (rotationCounter <= 1) rotationCounter = 1;
      activeItem = rotationCounter;
        
      Serial.println(rotationCounter);
      drawActiveMenu(); //update the selection on the LCD
      break;
    default:
      // Seting a value, go between max and min values
      menuMode = modeSelectVal;

      int minVal = 0;
      int startVal = 0;
      int increment = 1;

      maxVal = getMaxValForSetting(modeValSet);
      if (modeValSet == modeRotDistance) {
        minVal = -800;
      }

      // Increment
      if (modeValSet == modePanDistance) increment = 10;

      rotationCounter += startVal;
      rotationCounter += rotationValue * increment;
      if (rotationCounter > maxVal) rotationCounter = maxVal;
      if (rotationCounter <= minVal) rotationCounter = minVal;
      
      currentVal = rotationCounter;
      updateVal(rotationCounter);
      break;
  }
}

void pushbutton()
{
  tft.fillScreen(ILI9341_BLACK);
  if (activeMenu == 0) {
    activeMenu = activeItem;
    activeItem = 1;
    drawActiveMenu();
  } else if (menuMode == modeSubMenu) {
    modeValSet = -1;
    handleSubMenuSelection();
  } else if (menuMode == modeValMenu) {
    // modeValMenu - We're in a submenu, setting a value
    printToScreen(0, 0, getValType(), false);
    printToScreen(0, 30, "0", false);
  } else {
    // Set var with currentVal then clear & return to submenu
    setVal(currentVal);
    printToScreen(0, 30, "Value:" + String(currentVal), false);
    delay(1000);
    menuMode == modeSubMenu;
    activeItem = 1;
    drawActiveMenu();
  }
  rotationCounter = 0;
  rotaryEncoder = false;
}

void setVal(int curVal) {
  switch (modeValSet) {
    case modePanDistance:
      panDistance = curVal;
      break;
    case modePanSpeed:
      panSpeed = curVal;
      break;
    case modeRotDistance:
      rotateDistance = curVal;
      break;
    case modeRotSpeed:
      rotateSpeed = curVal;
      break;
    case modeCamDistance:
      camDistance = curVal;
      break;
    case modeTrackDistance:
      trackDistance = curVal;
      break;
    case modeTrackSpeed:
      trackSpeed = curVal;
      break;
  }
}

String getValType() {
  String tmpValType = "";
  switch (modeValSet) {
    case modePanDistance:
      tmpValType = "Pan Distance";
      break;
    case modePanSpeed:
      tmpValType = "Pan Speed";
      break;
    case modeRotDistance:
      tmpValType = "Rotate Distance";
      break;
    case modeRotSpeed:
      tmpValType = "Rotate Speed";
      break;
    case modeCamDistance:
      tmpValType = "Distance to Camera";
      break;
    case modeTrackDistance:
      tmpValType = "Distance to Track";
      break;
    case modeTrackSpeed:
      tmpValType = "Tracking Speed";
      break;
  }
  return tmpValType;
}

void resetVars() {
  panDistance = 0;
  panSpeed = 0;
  rotateDistance = 0;
  rotateSpeed = 0;
  camDistance = 0;
  trackDistance = 0;
  trackSpeed = 0;
}

void handleSubMenuSelection() {
  int backItem = getMaxItemsForMenu();
  int goItem = getMaxItemsForMenu() -1;
 
  if (activeItem == backItem) {
    // Same for all menus
    activeItem = activeMenu;
    activeMenu = 0;
    resetVars();
    drawActiveMenu();
  } else if (activeItem == goItem) {
    // Same for all menus
    Serial.println("GO BUTTON PRESSED");
    goStep = true;
  } else {
    // Normal menu items dir, speed...
    menuMode = modeValMenu;
    drawValuesMenu(activeMenu, activeItem);
  }
}

void drawValuesMenu(int curMenu, int item) {
  switch(curMenu) {
      case panMenu:
        if (item == 1) {
          modeValSet = modePanDistance;
          printToScreen(0, 4, getValType(), false);
        } else if (item == 2) {
          modeValSet = modePanSpeed;
          printToScreen(0, 4, getValType(), false);
        }
        break;
      case rotateMenu:
        if (item == 1) {
          modeValSet = modeRotDistance;
          printToScreen(0, 4, getValType(), false);
        } else if (item == 2) {
          modeValSet = modeRotSpeed;
          printToScreen(0, 4, getValType(), false);
        }
        break;
      case panRotateMenu:
        // Pan Dist
        Serial.println("PanRotate Menu Settings");
        break;
      case trackMenu:
        Serial.println("trackMenu Settings");
    }
}

int getValFromMenuAndIndex(int curMenu, int index) {
  switch (curMenu) {
    case panMenu:
      switch (index) {
        case 1:
          return panDistance;
          break;
        case 2:
          return panSpeed;
          break;
        default:
          return 0;
      }
      break;
    case rotateMenu:
      switch (index) {
        case 1:
          return rotateDistance;
          break;
        case 2:
          return rotateSpeed;
          break;
        default:
          return 0;
      }
      break;
    case panRotateMenu:
      switch (index) {
        case 1:
          return rotateDistance;
          break;
        case 2:
          return rotateSpeed;
          break;
        case 3:
          return panDistance;
          break;
        case 4:
          return panSpeed;
          break;
        default:
          return 0;
      }
      break;
    case trackMenu:
      switch (index) {
        case 1:
          return camDistance;
          break;
        case 2:
          return trackDistance;
          break;
        case 3:
          return trackSpeed;
          break;
        default:
          return 0;
      }
    default:
      return 0;
  }
}

void runStepper() {
  if (activeMenu == panMenu) {
    myStepper.setSpeed(panSpeed);
    myStepper.moveTo(panDistance);
    myStepper.runToPosition();
  }
}