/* Noch zu tun:
- Kommentare
- Endschalter
- 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 <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <OneButton.h>
#include <Adafruit_SPIDevice.h>
#include <AccelStepper.h>
#include <Servo.h>

/*
+++++++++++++++++++++++++++++++++++++++
Display (OLED 128x64)
fontsize 6x8 (default)
+++++++++++++++++++++++++++++++++++++++
*/

#define SCREEN_WIDTH 128 // OLED display width,  in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// declare an SSD1306 display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

/*
+++++++++++++++++++++++++++++++++++++++
Stepper
+++++++++++++++++++++++++++++++++++++++
*/
#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
+++++++++++++++++++++++++++++++++++++++
*/
#define servoPin 9
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

/*
+++++++++++++++++++++++++++++++++++++++
Encoder
+++++++++++++++++++++++++++++++++++++++
*/
#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 distance;
float dose;

OneButton button(inputSWEncoder,true);

/*
+++++++++++++++++++++++++++++++++++++++
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);
void moveServoToPosition(short);

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

Serial.begin(9600); // Serial display //löschbar
// initialize OLED display with address 0x3C for 128x64
if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
  Serial.println(F("SSD1306 allocation failed")); //löschbar
  while (true);
}
delay(2000);  // wait for initializing

previousStateCLK = digitalRead(inputCLKEncoder);
startDisplay();
stepper.setMaxSpeed(1000);
stepper.setAcceleration(500);
myservo.attach(servoPin);
myservo.write(m);  // servo shuts the mid
stepper.runToPosition();  // stepper in startposition
}

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

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

//Function starts the display and shows the topics
void startDisplay(){
  oled.clearDisplay();  // clear display
  oled.setTextSize(1);  // text size, minimum 1
  oled.setTextColor(WHITE);   // text color
  oled.setRotation(3);  // 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.drawLine(0,70,64,70,WHITE); // 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.writeFillRect(0,9,25,7,WHITE);
  oled.setTextColor(BLACK);
  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;
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCounterE > 0.4){
          rotatedCounterE = rotatedCounterE-0.1; 
        }
      getDistance(rotatedCounterE);
      blackValue(i);
      oled.print(rotatedCounterE,1);
      oled.display();
      }
    break;

    case 25:
      if (currentStateCLK != previousStateCLK){
        if (digitalRead(inputDTEncoder) != currentStateCLK && rotatedCountert < 999){
          rotatedCountert = rotatedCountert+1;
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCountert > 1){
          rotatedCountert = rotatedCountert-1; //
        }
      getDose(rotatedCounterE,rotatedCountert);
      blackValue(i);
      oled.print(rotatedCountert);
      oled.display();
      }
    break;

    case 54:
      if (currentStateCLK != previousStateCLK){
        if (digitalRead(inputDTEncoder) != currentStateCLK && rotatedCounters < 4){
          rotatedCounters = rotatedCounters+1;
        } else if (digitalRead(inputDTEncoder) == currentStateCLK && rotatedCounters > 1){
          rotatedCounters = rotatedCounters-1; //
        }
      setShutterpos(rotatedCounters);
      oled.display();
      }
    break;
  }
}

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

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

// Function if the encoder gets clicked. Chosen value is logged in and you can change the value below.
void validateValue(){
  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.writeFillRect(11,115,42,7,WHITE);
    oled.setTextColor(BLACK);
    oled.setCursor(11,115);
    oled.println("[START]"); //line 14
  }
  if (pressedCounter[i] == 115){
    oled.setCursor(0,pressedCounter[i]);
    oled.writeFillRect(0,pressedCounter[i],64,7,BLACK);
    oled.setTextColor(WHITE);
    oled.print("RUNNING..."); // irridiation is running  
    oled.display();
    moveRelative(distance); // move sample to calculated distance by the stepper
    while (stepper.isRunning()) {
      stepper.run();
      delay(10);
    }
    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
    oled.writeFillRect(0,pressedCounter[i],64,7,BLACK);
    oled.setCursor(17,pressedCounter[i]);
    oled.print("DONE"); // irridiation finished
    oled.display();
    for (int t=0; t<=2;t++){
      tone(13,440); // Sound on
      delay(800);
      noTone(13); // Sound off
      delay(800);
    }
    oled.writeFillRect(11,115,42,7,WHITE);
    oled.setTextColor(BLACK);
    oled.setCursor(11,115);
    oled.print("[AGAIN]"); // You can start again or change values!
  }
  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(){
  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.writeFillRect(0,pressedCounter[i],64,7,BLACK);
    oled.setTextColor(WHITE);
    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.writeFillRect(11,49,8,7,WHITE);
      oled.writeFillRect(47,49,8,7,BLACK);
      oled.setCursor(12,49);
    break;
    case 2:
      oled.writeFillRect(47,49,8,7,WHITE);
      oled.writeFillRect(11,49,8,7,BLACK);
      oled.writeFillRect(11,57,8,7,BLACK);
      oled.setCursor(48,49);
    break;
    case 3:
      oled.writeFillRect(11,57,8,7,WHITE);
      oled.writeFillRect(47,49,8,7,BLACK);
      oled.writeFillRect(47,57,8,7,BLACK);
      oled.setCursor(12,57);
    break;
    case 4:
      oled.writeFillRect(47,57,8,7,WHITE);
      oled.writeFillRect(11,57,8,7,BLACK);
      oled.setCursor(48,57);
    break;
  }
  oled.setTextColor(BLACK);
  oled.print("X");
}

// Function validates the chosen Shutterposition before the next value can be changed
void validateShutterpos(uint8_t rotatedCounters){
  switch (rotatedCounters){
    case 1:
      oled.writeFillRect(11,49,8,7,BLACK);
      oled.setCursor(12,49);
    break;
    case 2:
      oled.writeFillRect(47,49,8,7,BLACK);
      oled.setCursor(48,49);
    break;
    case 3:
      oled.writeFillRect(11,57,8,7,BLACK);
      oled.setCursor(12,57);
    break;
    case 4:
      oled.writeFillRect(47,57,8,7,BLACK);
      oled.setCursor(48,57);
    break;
  }
  oled.setTextColor(WHITE);
  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
  distance = (169.01-sqrt(pow(169.01,2)-4*5.5552*(1426.7-rotatedCounterE*1000)))/(2*5.552);
  oled.writeFillRect(0,82,25,8,BLACK); // overwrite previous value on display
  oled.setCursor(0,82);
  oled.setTextColor(WHITE);
  oled.print(distance);
}

// 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.writeFillRect(0,100,25,8,BLACK); // overwrite previous value on display
  oled.setCursor(0,100);
  oled.setTextColor(WHITE);
  oled.print(dose);
}

// Function to move the sample to the chosen value by the stepper.
void moveRelative(float distance) {
  targetSteps = map(distance, 2, 30, -STEPS_PER_REVOLUTION, STEPS_PER_REVOLUTION);
  stepper.move(targetSteps);
}

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