#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

// Connections to A4988 for Mot2
#define Mot2_DIR    0 // Direction
#define Mot2_STEP   1
#define Mot2_MS2    2
#define Mot2_MS3    3
#define Mot2_MS1    4
#define Mot2_Enable 5
#define Unused_06   6
#define VERTICAL_HOMING   7
#define Rotary_CLK  9
#define Rotary_DT   14
#define Rotary_BTN  15
// Connections to A4988 for Mot1
#define Mot1_DIR    18 // Direction
#define Mot1_STEP   19 // Step
#define Mot1_MS2    22
#define Mot1_MS1    23
#define PWM_Servo   30


// STATE Machine

#define STATE_IDLE          0
#define STATE_SCAN          1
#define STATE_MOVING        2
#define STATE_SHOOT         3
#define STATE_HOMING        4
#define STATE_WAIT_PIC_CONF 5

//STATE variables
int state = 0; // Faire des #define pour donner les noms des states
//Homing statuses and origin of the command
bool homingNotDone = true;
bool homingFromMenu;
//Next target position for the motors
int indexTheta = 0;
int positionTheta = 0; //Position in steps
int indexZ = 0;
int positionZ = 0;
int minAngle = 30;
int horizontalAngle = 55;
int maxAngle = 90;
int cameraAngle = horizontalAngle;
int counter; // Current picture index
// Resolution in the two axes
int verticalPicturesNumber = 2 ;
int horizontalPicturesNumber = 2;
// Steps interval between to photo takes;
int verticalInterval;
int horizontalInterval;
int maxPictures;
String espMessage;
// OLED Screen controls

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library.
// On a SAMD21:       PA16(SDA), PA17(SCL)
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)

#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


//Motor parameters
// for the a classical NEMA17 stepper
const int STEPS_PER_REV = 200; // Steps per revolution in the motor
const int MOTOR_STEPS_DELAY = 1000; //Delay in microseconds between HIGH and LOW
// HORIZONTAL MOTOR
const int GEAR_MOTOR = 20; // teeth number on the motor gear
const int GEAR_PULLEY = 190;// teeth number on the turning plate gear
const float GEAR_REDUCTION = GEAR_MOTOR/GEAR_PULLEY; // Gear reduction ratio

const int HORIZONTAL_STEPS_PER_REV = STEPS_PER_REV * GEAR_PULLEY /GEAR_MOTOR;

// VERTICAL MOTOR
const int PITCH = 2; // 2 mm per turn
const int MAX_HEIGHT = 130; // 130 mm Travel distance between bottow and top
const int VERTICAL_STEPS_PER_TRAVEL = STEPS_PER_REV * MAX_HEIGHT/PITCH;

//Servomotor
Servo myservo;  // create servo object to control a servo

//Motor AccelStepper

AccelStepper stepperHorizontal(AccelStepper::DRIVER, Mot1_STEP, Mot1_DIR);
AccelStepper stepperVertical(AccelStepper::DRIVER, Mot2_STEP, Mot2_DIR);


int currentStateCLK;
int previousStateCLK;
String serialMessage;

//Utiliser U8g2 pour le menu OLED
void setup() {

  // Connections to A4988 for Mot1
  //Motor 1
  pinMode(Mot1_DIR,OUTPUT);
  pinMode(Mot1_STEP,OUTPUT);
  pinMode(Mot1_MS1,OUTPUT);
  pinMode(Mot1_MS2,OUTPUT);
  stepperHorizontal.setMaxSpeed(200);
  stepperHorizontal.setAcceleration(10);
  stepperHorizontal.setCurrentPosition(0);
  //Motor 2
  pinMode(Mot2_DIR,OUTPUT);
  pinMode(Mot2_STEP,OUTPUT);
  pinMode(Mot2_MS1,OUTPUT);
  pinMode(Mot2_MS2,OUTPUT);
  pinMode(Mot2_MS3,OUTPUT);
//  if(microsteppingV){
//    digitalWrite(Mot2_MS1,HIGH);
//    digitalWrite(Mot2_MS2,HIGH);
//    digitalWrite(Mot2_MS3,HIGH);
//  } else{
//    digitalWrite(Mot2_MS1,LOW);
//    digitalWrite(Mot2_MS2,LOW);
//    digitalWrite(Mot2_MS3,LOW);
//  }
  pinMode(Mot2_Enable,OUTPUT);
  pinMode(VERTICAL_HOMING,INPUT_PULLUP);

  stepperVertical.setAcceleration(10);
  stepperVertical.setMaxSpeed(500);
  //Rotary Encoder
  pinMode(Rotary_BTN,INPUT_PULLUP );
  pinMode(Rotary_CLK,INPUT);
  pinMode(Rotary_DT,INPUT);
  //Servo motor
  pinMode(PWM_Servo,OUTPUT);
  myservo.attach(PWM_Servo);
  // Control LED
  pinMode(Unused_06,OUTPUT);
  delay(1000);
  SerialUSB.begin(115200);
  Serial1.begin(115200);
  // OLED Screen
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    SerialUSB.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  // Clear the buffer
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println(F("Initialisation"));
  display.display();

  // Rotary Encoder
  previousStateCLK = digitalRead(Rotary_CLK);
  myservo.write(cameraAngle);
}

void loop() {
  switch (state) {
    case STATE_IDLE:
    serialMessage = "";
    if (SerialUSB.available() > 0) {
      // read the incoming byte:
      serialMessage = SerialUSB.readString();
    }
    if(serialMessage=="STATE_SCAN"){
      SerialUSB.println("Scanning order received");
      state = STATE_SCAN;
    }
    else if(serialMessage=="STATE_HOMING"){
      state = STATE_HOMING;
      homingFromMenu = true;
    }
    else {
      displayString("state: IDLE",-1);
//      displayString("Waiting",-1);
      }

    if (Serial1.available() > 0) {
    // read the incoming byte:
    espMessage = Serial1.readString();
    SerialUSB.println(espMessage);
    }
    //Implement menu /button reading
//    if (homingNotDone){
//      homingFromMenu = true;
//      state = STATE_HOMING;
//    } else if (start_scan){
//      counter = 0;
//      state = STATE_SCAN;
//    }
      break;
    case STATE_HOMING:
    displayString("state: HOMING",-1);
    Serial1.print("LED");
    displayString("Homing in process",-1);
    delay(1000);
   if (homingNotDone){
     verticalMotorHoming();
     stepperVertical.setCurrentPosition(0);
     SerialUSB.print("value homingFromMenu :");
     SerialUSB.println(homingFromMenu);
     homingNotDone = false;
     if (homingFromMenu){
       state = STATE_IDLE;
       SerialUSB.println("back to state IDLE");
       homingFromMenu = false;
     } else {
       counter = 0;
       maxPictures = verticalPicturesNumber*horizontalPicturesNumber;
       state = STATE_SCAN;
     }
   }
   return;
    break;
    case STATE_SCAN:
    displayString("state: SCAN",-1);
    delay(2000);
    displayProgress(counter);
    //
    if (homingNotDone){
      state = STATE_HOMING;
      return;
    }
    positionFromPhotoCounter(counter);
    displayProgress(counter);
    stepperHorizontal.moveTo(positionTheta);
    stepperVertical.moveTo(positionZ);
    SerialUSB.print("counter: ");
    SerialUSB.println(counter);
    SerialUSB.print("indexTheta: ");
    SerialUSB.println(indexTheta);
    SerialUSB.print("indexZ: ");
    SerialUSB.println(indexZ);
    state = STATE_MOVING;
    break;

    case STATE_MOVING:
    myservo.write(cameraAngle);
//    displayString("state: MOVING",-1);
//    displayString("Moving to",stepperVertical.distanceToGo());
    // USE AccelStepper https://www.airspayce.com/mikem/arduino/AccelStepper/
    if (stepperVertical.isRunning() || stepperHorizontal.isRunning())
    {
      stepperVertical.run();
      stepperHorizontal.run();
    } else{
      state = STATE_SHOOT;
    }
    break;

    case STATE_SHOOT:
//    displayString("state: SHOOT",-1);
//    delay(2000);
    Serial1.print("SHOOT");
    state = STATE_WAIT_PIC_CONF;
    break;

    case STATE_WAIT_PIC_CONF:
//    displayString("state: STATE_WAIT_PIC_CONF",-1);
    
    if (Serial1.available() > 0) {
      //  // read the incoming byte:
      String espMessage = Serial1.readString();
      SerialUSB.println("espMessage");
      SerialUSB.println(espMessage);
      if (espMessage.substring(0, 2) =="ok"){
        
        if (counter == maxPictures){
          state = STATE_IDLE;
        } 
        else {
        state = STATE_SCAN;
        counter++;
        }
      } else if (espMessage.substring(0, 3) =="nok"){
        SerialUSB.println("Photo failed, retrying");
        Serial1.print("SHOOT");
      }
    } else {
      return;
    }
    break;
  }
}


void positionFromPhotoCounter(int counter) {
  // HORIZONTAL_STEPS_PER_REV;
  horizontalInterval = HORIZONTAL_STEPS_PER_REV/horizontalPicturesNumber;
  verticalInterval = VERTICAL_STEPS_PER_TRAVEL/verticalPicturesNumber;

  //Index calculation
  indexTheta = counter/verticalPicturesNumber;
  indexZ = counter%(verticalPicturesNumber);
  if (indexTheta%2 !=0){
    indexZ = (verticalPicturesNumber-1) - indexZ;
  }
  positionTheta = indexTheta * horizontalInterval;
  positionZ = indexZ * verticalInterval;
  cameraAngle = map(indexZ, 0, verticalPicturesNumber, horizontalAngle, maxAngle)
  ;
}

void verticalMotorHoming() {
  // Spin vertical motor
  digitalWrite(Mot2_DIR,LOW);
  while(digitalRead(VERTICAL_HOMING)==LOW) {
    digitalWrite(Mot2_STEP,HIGH);
    delayMicroseconds(1000);
    digitalWrite(Mot2_STEP,LOW);
    delayMicroseconds(500);
  }
  SerialUSB.println("Vertical move homing finished");
}

//Blink LED on pin 6
void blinkLED(void) {
  digitalWrite(Unused_06,HIGH);
  delay(500);
  digitalWrite(Unused_06,LOW);
  delay(500);
}

//_______________________TO BE CODED_____________________________________



//_______________________TO BE REVIEWED_____________________________________

void resumeDisplaySerial(void) {
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println(F("Listening to Serial"));
  display.display();
}
void displaySerial(String serialMessage) {
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("I received:"));
  display.setCursor(0,10);             // Start at top-left corner
  display.setTextSize(1);
  display.println(serialMessage);
  display.display();
}

void displayString(String message,int step_idx) {
  if(message !="state: IDLE" && message !="Waiting"){
    SerialUSB.println(message);
  }
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);             // Start at top-left corner
  display.println(message);
  if (step_idx!=-1)
  {
    display.println(step_idx);
  }
  display.display();
}

void displayProgress(int counter) {
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);             // Start at top-left corner
  display.print("Scan in progress");
  display.setCursor(0,10);
  display.print(counter+1);
  display.print("/");
  display.print(horizontalPicturesNumber*verticalPicturesNumber);
  display.setCursor(0,20);
  display.print("H:");
  display.print(indexTheta+1);
  display.print("/");
  display.print(horizontalPicturesNumber);
  display.setCursor(60,20);
  display.print("V:");
  display.print(indexZ);
  display.print("/");
  display.print(verticalPicturesNumber);
  display.display();
}