/* Noch zu tun:
  - Kommentare
  - Encoder drehen nur ein Tick (delay?)
  - Endschalter (Nullpunkt und ein Schalter is drin)
  - Photodiode
  - Kontrolle Stepper (Startposition, Bewegung nach Neustart/AGAIN)
  - Shutterwerte anpassen an Realität
  - Verifizieren der Werte
*/

#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_I2CDevice.h>
#include <U8g2lib.h>
#include <Adafruit_GFX.h>
#include <OneButton.h>
#include <Adafruit_SPIDevice.h>
#include <AccelStepper.h>
#include <Servo.h>


/*
  +++++++++++++++++++++++++++++++++++++++
  Display (OLED 128x64)
  fontsize 6x8 (default)
  input voltage 3,3 V
  +++++++++++++++++++++++++++++++++++++++
*/

U8G2_SSD1306_128X64_NONAME_1_HW_I2C oled(U8G2_R0, U8X8_PIN_NONE);

/*
  +++++++++++++++++++++++++++++++++++++++
  Stepper
  input voltage 5 V
  +++++++++++++++++++++++++++++++++++++++
*/
#define motorPin1 2
#define motorPin2 3
#define motorPin3 4
#define motorPin4 5
#define STEPS_PER_REVOLUTION 64
AccelStepper stepper(AccelStepper::FULL4WIRE, motorPin1, motorPin2, motorPin3, motorPin4);
long targetSteps;

/*
  +++++++++++++++++++++++++++++++++++++++
  Servo
  input voltage 5 V
  +++++++++++++++++++++++++++++++++++++++
*/
#define servoPin 12
Servo myservo;
#define n 0 // nothing shut
#define r 90 // right shut, left irradiated
#define l 90 // left shut, right irradiated
#define m 360 // mid shut

/*
  +++++++++++++++++++++++++++++++++++++++
  Roller lever
  input voltage 5 V
  +++++++++++++++++++++++++++++++++++++++
*/
#define rollerlever 14

/*
  +++++++++++++++++++++++++++++++++++++++
  Encoder
  input voltage 5 V
  +++++++++++++++++++++++++++++++++++++++
*/
#define inputCLKEncoder 19
#define inputDTEncoder 18
#define inputSWEncoder 17

unsigned long now;
float rotatedCounterE = 0.4; // at beginning, between 0.4 and 1.4
uint16_t rotatedCountert = 0; // at beginning, between 0 and 999 s
uint8_t rotatedCounters = 3; // at beginning, between 1 and 4; 1 = left, 2 = right, 3 = mid, 4 = nothing
uint8_t currentStateCLK;
uint8_t previousStateCLK;

uint8_t pressedCounter[] = {9, 25, 54, 115}; //y-Coordinate of the OLED to write in
uint8_t i = 0; //counter to go through pressedCounter[]
float setdistance; // between 0 and 16 cm
float interndistance;
float dose;

OneButton button(inputSWEncoder, true);

/*
  +++++++++++++++++++++++++++++++++++++++
  speaker
  input voltage 5 V
  +++++++++++++++++++++++++++++++++++++++
*/
#define speaker 13

/*
  +++++++++++++++++++++++++++++++++++++++
  Functiondeclaration
  +++++++++++++++++++++++++++++++++++++++
*/
void startDisplay();
void setValue();
void blackValue(uint8_t);
void whiteValue(uint8_t);
void goBackValue();
void validateValue();
void setShutterpos(uint8_t);
void validateShutterpos(uint8_t);
void getDistance(float);
void getDose(float, uint16_t);
void moveRelative(float, float);
void moveServoToPosition(short);

/*
  +++++++++++++++++++++++++++++++++++++++
  Setup
  +++++++++++++++++++++++++++++++++++++++
*/
void setup() {
  pinMode(rollerlever, INPUT);
  pinMode (inputCLKEncoder, INPUT);
  pinMode (inputDTEncoder, INPUT);
  button.attachClick(validateValue);
  button.attachDoubleClick(goBackValue);
  button.setDebounceTicks(20);

  Serial.begin(9600); // Serial display //löschbar
  delay(2000);  // wait for initializing
  previousStateCLK = digitalRead(inputCLKEncoder);
  startDisplay();
  stepper.setMaxSpeed(100);
  stepper.setAcceleration(50);
  myservo.attach(servoPin);
  myservo.write(m);  // servo shuts the mid
  //while (digitalRead(rollerlever) == LOW) { // Sobald Endschalter verbaut, go to startposition
  //  stepper.move(-1);
  //  stepper.run();
  //  interndistance = 0;
  //}
  //stepper.runToPosition();  // stepper in startposition????
  interndistance = 0; //löschen sobald Endschalter verbaut
  Serial.println("setup completed");
}

/*
  +++++++++++++++++++++++++++++++++++++++
  Loop
  +++++++++++++++++++++++++++++++++++++++
*/
void loop() {
  now = millis();
  button.tick();
  setValue();
}

/*
  +++++++++++++++++++++++++++++++++++++++
  Functions
  +++++++++++++++++++++++++++++++++++++++
*/

//Function starts the display and shows the topics
void startDisplay() {
  oled.clearDisplay();  // clear display
  oled.setFont(u8g2_font_5x7_tr);  // text size, minimum 1
  oled.setDisplayRotation(U8G2_R3);  // rotate text (270°)
  oled.setCursor(0, 0); // position to display
  oled.print("E =");  // text to display, line 1
  oled.setCursor(27, 9); // set the cursor to next position to be written in
  oled.println("mW/cm2"); // line 2
  oled.println("t ="); // line 3
  oled.print(rotatedCountert);
  oled.setCursor(27, 25);
  oled.println("s"); // line 4
  oled.println("irradiatedside:"); // line 5 + 6
  oled.println("l[ ], r[ ]"); // line 7
  oled.println("m[X], n[ ]"); // line 8
  oled.setDrawColor(1);
  oled.drawLine(0, 70, 64, 70); // line to seperate input and output values
  oled.setCursor(0, 73);
  oled.println("Distance ="); // line 9
  getDistance(rotatedCounterE);
  oled.setCursor(27, 82);
  oled.println("cm"); // line 10
  oled.println("Dose ="); // line 11
  getDose(rotatedCounterE, rotatedCountert);
  oled.setCursor(27, 100);
  oled.print("mJ/cm2"); // line 12
  oled.setCursor(11, 115);
  oled.print("[START]"); //line 13
  oled.setCursor(0, 9);
  oled.setDrawColor(1);
  oled.drawBox(0, 9, 25, 7);
  oled.setDrawColor(0);
  oled.print(rotatedCounterE, 1);
  oled.display(); // show on OLED
}

// Function to change the value in the marked line.
void setValue() {
  currentStateCLK = digitalRead(inputCLKEncoder);
  switch (pressedCounter[i]) {

    case 9:
      if (currentStateCLK != previousStateCLK) {
        if (digitalRead(inputDTEncoder) != currentStateCLK && rotatedCounterE < 1.4) {
          rotatedCounterE = rotatedCounterE + 0.1;
          Serial.println("+"); //löschbar
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCounterE > 0.4) {
          rotatedCounterE = rotatedCounterE - 0.1;
          Serial.println("-"); //löschbar
        }
        getDistance(rotatedCounterE);
        blackValue(i);
        oled.print(rotatedCounterE, 1);
        oled.display();
        delay(20); //eventuell zum Entprellen
      }
      break;

    case 25:
      if (currentStateCLK != previousStateCLK) {
        if (digitalRead(inputDTEncoder) != currentStateCLK && rotatedCountert < 999) {
          rotatedCountert = rotatedCountert + 1;
          Serial.println("+"); //löschbar
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCountert > 1) {
          rotatedCountert = rotatedCountert - 1;
          Serial.println("-"); //löschbar
        }
        getDose(rotatedCounterE, rotatedCountert);
        blackValue(i);
        oled.print(rotatedCountert);
        oled.display();
        delay(20); //eventuell zum Entprellen
      }
      break;

    case 54:
      if (currentStateCLK != previousStateCLK) {
        if (digitalRead(inputDTEncoder) != currentStateCLK && rotatedCounters < 4) {
          rotatedCounters = rotatedCounters + 1;
          Serial.println("+"); //löschbar
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCounters > 1) {
          rotatedCounters = rotatedCounters - 1;
          Serial.println("-"); //löschbar
        }
        setShutterpos(rotatedCounters);
        oled.display();
        delay(20); //eventuell zum Entprellen
      }
      break;
  }
}

// Function to write a new black value in a white box
void blackValue(uint8_t i) {
  oled.setCursor(0, pressedCounter[i]);
  oled.setDrawColor(1);
  oled.drawBox(0, pressedCounter[i], 25, 7); // overwrite previous value
  oled.setDrawColor(0);
}

// Function to write a new white value in a black box
void whiteValue(uint8_t i) {
  oled.setCursor(0, pressedCounter[i]);
  oled.setDrawColor(0);
  oled.drawBox(0, pressedCounter[i], 25, 7); // overwrite previous value
  oled.setDrawColor(1);
}

// Function if the encoder gets clicked. Chosen value is logged in and you can change the value below.
void validateValue() {
  Serial.println("weiter"); //löschbar
  if (pressedCounter[i] == 9 || pressedCounter[i] == 25) {
    whiteValue(i); // previous value logged
    switch (pressedCounter[i]) {
      case 9:
        oled.println(rotatedCounterE, 1);
        blackValue(i + 1);
        oled.println(rotatedCountert);
        break;
      case 25:
        oled.println(rotatedCountert);
        setShutterpos(rotatedCounters);
        break;
    }
  }
  if (pressedCounter[i] == 54) {
    validateShutterpos(rotatedCounters);
    oled.setDrawColor(0);
    oled.setCursor(11, 115);
    oled.println("[START]"); //line 14
  }
  if (pressedCounter[i] == 115) {
    oled.setCursor(0, pressedCounter[i]);
    oled.setDrawColor(0);
    oled.drawBox(0, pressedCounter[i], 64, 7);
    oled.setDrawColor(1);
    oled.print("RUNNING..."); // irridiation is running
    oled.display();
    moveRelative(setdistance, interndistance); // move sample to calculated distance by the stepper
    if (setdistance <= 16) { // Nochmalige Begrenzung durch das Programm (an dem Zeitpunkt ist der Abstand eigentlich auch nicht möglich)
      while (stepper.isRunning()) {
        stepper.run();
        Serial.println("Go"); //löschbar
        delay(10);
      }
      interndistance = setdistance; // wenn wir immer von der momentanen Position rechnen wollen
      switch (rotatedCounters) { // move shutter into chosen position by the servo
        case 1:
          moveServoToPosition(r);
          break;
        case 2:
          moveServoToPosition(l);
          break;
        case 3:
          moveServoToPosition(n);
          break;
        case 4:
          moveServoToPosition(m);
          break;
      }
      delay(rotatedCountert * 1000); // irridiation is running!
      moveServoToPosition(m); //shut the shutter again
      moveRelative(-setdistance, interndistance); // move back to startposition
      while (stepper.isRunning()) {
        stepper.run();
        Serial.println("Go"); //löschbar
        delay(10);
      }
      //while (digitalRead(rollerlever) == LOW) { // Sobald Endschalter verbaut, while Schleife davor löschen, go to startposition
      //  stepper.move(-1);
      //  stepper.run();
      //  Serial.println("Go");
      //  interndistance = 0;
      //}
      oled.setDrawColor(0);
      oled.drawBox(0, pressedCounter[i], 64, 7);
      oled.setCursor(17, pressedCounter[i]);
      oled.print("DONE"); // irridiation finished
      oled.display();
      for (int t = 0; t <= 2; t++) {
        tone(speaker, 440); // Sound on
        delay(800);
        noTone(speaker); // Sound off
        delay(800);
      }
      oled.setDrawColor(1);
      oled.drawBox(11, 115, 42, 7);
      oled.setDrawColor(0);
      oled.setCursor(11, 115);
      oled.print("[AGAIN]"); // Start again or change values!
    }
    if (setdistance > 16 || setdistance < 0) { // Falls die Distanz nicht möglich ist
      oled.setDrawColor(1);
      oled.drawBox(0, 115, 64, 7);
      oled.setDrawColor(0);
      oled.setCursor(0, 115);
      oled.print("[CHANGE E]"); // Distance not possible --> roller level, change E!
    }
  }
  oled.display();
  if (i < 3) {
    i++;
  }
}

// Function if the encoder gets double clicked. Chosen value is logged in and you can change the value above.
void goBackValue() {
  Serial.println("zurück"); //löschbar
  switch (pressedCounter[i]) {
    case 25:
      whiteValue(i);
      oled.println(rotatedCountert);
      blackValue(i - 1);
      oled.print(rotatedCounterE, 1);
      break;
    case 54:
      validateShutterpos(rotatedCounters);
      blackValue(i - 1);
      oled.print(rotatedCountert);
      break;
    case 115:
      oled.setDrawColor(0);
      oled.drawBox(0, pressedCounter[i], 64, 7);
      oled.setDrawColor(1);
      oled.setCursor(11, 115);
      oled.println("[START]");
      setShutterpos(rotatedCounters);
      break;
  }
  oled.display();
  if (i > 0) {
    i--;
  }
}

// Function sets the old Shutterposition to the new Shutterposition
void setShutterpos(uint8_t rotatedCounters) {
  switch (rotatedCounters) {
    case 1:
      oled.setDrawColor(1);
      oled.drawBox(11, 49, 8, 7);
      oled.setDrawColor(0);
      oled.drawBox(47, 49, 8, 7);
      oled.setCursor(12, 49);
      break;
    case 2:
      oled.setDrawColor(1);
      oled.drawBox(47, 49, 8, 7);
      oled.setDrawColor(0);
      oled.drawBox(11, 49, 8, 7);
      oled.drawBox(11, 57, 8, 7);
      oled.setCursor(48, 49);
      break;
    case 3:
      oled.setDrawColor(1);
      oled.drawBox(11, 57, 8, 7);
      oled.setDrawColor(0);
      oled.drawBox(47, 49, 8, 7);
      oled.drawBox(47, 57, 8, 7);
      oled.setCursor(12, 57);
      break;
    case 4:
      oled.setDrawColor(1);
      oled.drawBox(47, 57, 8, 7);
      oled.setDrawColor(0);
      oled.drawBox(11, 57, 8, 7);
      oled.setCursor(48, 57);
      break;
  }
  oled.setDrawColor(0);
  oled.print("X");
}

// Function validates the chosen Shutterposition before the next value can be changed
void validateShutterpos(uint8_t rotatedCounters) {
  oled.setDrawColor(0);
  switch (rotatedCounters) {
    case 1:
      oled.drawBox(11, 49, 8, 7);
      oled.setCursor(12, 49);
      break;
    case 2:
      oled.drawBox(47, 49, 8, 7);
      oled.setCursor(48, 49);
      break;
    case 3:
      oled.drawBox(11, 57, 8, 7);
      oled.setCursor(12, 57);
      break;
    case 4:
      oled.drawBox(47, 57, 8, 7);
      oled.setCursor(48, 57);
      break;
  }
  oled.setDrawColor(1);
  oled.print("X");
}

// Function calculates the distance based on E and prints the new value
void getDistance(float rotatedCounterE) { // highest possible  E = 1.4 mW/cm2, distance 0.16 cm
  setdistance = (169.01 - sqrt(pow(169.01, 2) - 4 * 5.5552 * (1426.7 - rotatedCounterE * 1000))) / (2 * 5.552);
  oled.setDrawColor(0);
  oled.drawBox(0, 82, 25, 8); // overwrite previous value on display
  oled.setCursor(0, 82);
  oled.setDrawColor(1);
  oled.print(setdistance);
}

// Function calculates the distance based on E and t and prints the new value
void getDose(float rotatedCounterE, uint16_t rotatedCountert) {
  dose = rotatedCounterE * rotatedCountert;
  oled.setDrawColor(0);
  oled.drawBox(0, 100, 25, 8); // overwrite previous value on display
  oled.setCursor(0, 100);
  oled.setDrawColor(1);
  oled.print(dose);
}

// Function to move the sample to the chosen value by the stepper.
void moveRelative(float setdistance, float interndistance) {
  //targetSteps = map(distance, 2, 30, -STEPS_PER_REVOLUTION, STEPS_PER_REVOLUTION); Stepper soll ja durchdrehen
  targetSteps = (setdistance - interndistance) / 0.0125; // needed steps to reach the distance
  stepper.move(targetSteps);
}

// Function to move the shutter into chosen position by the servo.
void moveServoToPosition(short position) {
  myservo.write(position);
}