#include <Wire.h>
#include <LiquidCrystal_I2C.h>
//#include "bounce.h"
LiquidCrystal_I2C lcd(0x27, 16, 2);   //adres toewijzen LCD

// BtnStates
#define BTN_SETUP 0
#define BTN_READ 1
#define BTN_START_TIMER 2
#define BTN_WACHT 3
#define BTN_INGEDRUKT 4
#define BTN_DUIDELIJK_SIGNAAL 5
#define INTERVAL 15 // aantal millis nodig om de bounce weg te werken

// MachineStates
#define IDLE 0
#define BUZY 1
#define RESET 2

//pinnen declareren
#define PIN_AANUIT 2 // knop om machine aan te zetten
#define PIN_RESET 3   //7 // doet de veermotor terug draaien tot begin positie
#define PIN_M1_STEPPULSE 4 // regelt de snelheid van de draaimotor
#define PIN_M1_DIRPULSE 5  //richting motor 1
#define PIN_M2_STEPPULSE 7 // regelt de snelheid van de draadgeleiding
#define PIN_M2_DIRPULSE 6   // bepaalt de richting, deze moet terug draaien na het afknippen van de veer

#define PIN_SET_PLUS_BTN 13       // input drukknop
#define PIN_SET_MIN_BTN 8       // input drukknop
#define PIN_ENTER_BTN 12  // input drukknop

// Vaste 'variabelen' van de veermachine
#define SpoedDrijfStang 1
#define StepsPerRevolutie 3200

// De prototypes van de functies moeten hier staan:
void draai_Motor(int nummer);
void check_AanUit();
void check_ResetKnop();
void draaiMotor(byte PINPulse, int DT, bool Richting, byte PINDir);
void printTabel();
void berekenStpWaarden(float StepsM1, float StepsM2);    //hier staat de berekening van de start en stopwindingen en de steps in.

// Helpervariabelen voor het resetten of terugdraaien van de motors. dit om terug in begin positie van de spindel te staan en om de veer te kunnen verwijderen
byte Afgewerkt = 0;
uint32_t AfgewerkteM2Steps = 0;  //unsigned int 32 bit. is tot 4 miljoen etc...
uint32_t AfgewerkteM1Steps = 0;
uint32_t TellerSteps = 0;
//float M1Steps = 0;
byte status = IDLE;
bool WaardenNietAanwezig = true;
static int EnterBtnState = 0;
static int SetPlusBtnState = 0;
static int SetMinBtnState = 0;

// VeerVariabelen in te geven door Operator
float DraadDikte = 0;
int WinSta = 0;
int WinSto = 0;
float SpoedVeer = 0;
float LengteVeer = 0;


// Bereken alles:
// +-----------+-----------+----------+-----------+
// |           | Veerdeel  | Veerdeel | Veerdeel  |
// |           |   Start   |  Midden  |   Stop    |
// +-----------+-----------+----------+-----------+
// | Windingen | (gegeven) |  WinMid  | (gegeven) |
// +-----------+-----------+----------+-----------+
// |   Spoed   |   SpSta   |   SpMid  |  SpSto    |
// +-----------+-----------+----------+-----------+
// | Steps M1  | StpM1Sta  | StpM1Mid | StpM1Sto  |
// +-----------+-----------+----------+-----------+
// | Steps M2  | StpM2Sta  | StpM2Mid | StpM2Sto  |
// +-----------+-----------+----------+-----------+

// Afgeleide variabelen nodig om de machine te laten werken

float LengteWinSta = 0;
float LengteWinSto = 0 ;
float lengteWinMid = 0;
float WinMid = 0;

uint32_t StpM1Sta = 0 ;
uint32_t StpM1Mid = 0;
uint32_t StpM1Sto = 0;

float SpSta = 0;
float SpMid = 0;
float SpSto = 0 ;

uint32_t StpM2Sta = 0;
uint32_t StpM2Mid = 0;
uint32_t StpM2Sto = 0;

float Aanpassing;
uint32_t iAanpassing;

void pin_init() {
  pinMode(PIN_AANUIT, INPUT_PULLUP);
  pinMode(PIN_M1_STEPPULSE, OUTPUT);
  pinMode(PIN_M1_DIRPULSE, OUTPUT);
  pinMode(PIN_M2_STEPPULSE, OUTPUT);
  pinMode(PIN_M2_DIRPULSE, OUTPUT);
  pinMode(PIN_RESET, INPUT_PULLUP);
  pinMode(PIN_SET_PLUS_BTN, INPUT_PULLUP);
  pinMode(PIN_SET_MIN_BTN, INPUT_PULLUP);
  pinMode(PIN_ENTER_BTN, INPUT_PULLUP);
}


// Bepaalt begininstelling zoals draairichting motoren
void motor_init() {
  digitalWrite(PIN_M2_DIRPULSE, HIGH);
  digitalWrite(PIN_M1_DIRPULSE, HIGH
  );
}

// initialize the lcd
void lcd_init() {
  lcd.init();
  lcd.backlight();
}

void print_lcd_screen01() {
  lcd.setCursor(0, 0);
  lcd.print("Vlakslijpmachine:");
  lcd.setCursor(0, 1);
  lcd.print("Geef waarden in");
  delay(1000);
  lcd.clear();
}

void check_VeerWaardenAanwezig() {
  if (WaardenNietAanwezig) {
    DraadDikte = vraagWaardeViaKnoppenEnDisplay(">draaddikte", 0.1);
    WinSta = vraagWaardeViaKnoppenEnDisplay(">startwindingen",  1.0);
    Serial.println(WinSta);
    WinSto = vraagWaardeViaKnoppenEnDisplay(">stopwindingen",  1.0);
    SpoedVeer = vraagWaardeViaKnoppenEnDisplay(">Spoed Veer",  1.0);
    LengteVeer = vraagWaardeViaKnoppenEnDisplay(">Lengte Veer",  1.0);
    WaardenNietAanwezig = false;
    BEREKEN_WAARDEN_WINDINGEN();
    printTabel();
  }
}

void setup() {
  Serial.begin(9600);
  pin_init();
  motor_init();
  lcd_init();
  print_lcd_screen01();
}

void loop() {

  switch (status) {
    case IDLE:
      check_VeerWaardenAanwezig();
      check_AanUit();
      check_ResetKnop();
      break;
    case BUZY:
      PRINTLCD();
      berekenStpWaarden(StpM1Sta, StpM2Sta);
      maakVeer2(StpM1Sta, StpM2Sta);
      AfgewerkteM2Steps = StpM2Sta;
      AfgewerkteM1Steps = StpM1Sta;

      berekenStpWaarden(StpM1Mid, StpM2Mid);
      maakVeer2(StpM1Mid, StpM2Mid);
      AfgewerkteM2Steps = AfgewerkteM2Steps + StpM2Mid;
      AfgewerkteM1Steps = AfgewerkteM1Steps + StpM1Mid;

      berekenStpWaarden(StpM1Sto, StpM2Sto);
      maakVeer2(StpM1Sto, StpM2Sto);
      AfgewerkteM2Steps = AfgewerkteM2Steps + StpM2Sto;
      AfgewerkteM1Steps = AfgewerkteM1Steps + StpM1Sto;
      Afgewerkt = HIGH;

      status = IDLE;
      break;
    case RESET:
      //lcd.clear();
      RESET_constantMotor(PIN_M2_STEPPULSE, AfgewerkteM2Steps);
      RESET_constantMotor(PIN_M1_STEPPULSE, AfgewerkteM1Steps);
      AfgewerkteM2Steps = 0;
      Afgewerkt = LOW;
      // de veervariabelen moeten opnieuw ingegeven worden.
      //WaardenNietAanwezig = true;
      break;
    default:
      break;
  }
}

float  BEREKEN_WAARDEN_WINDINGEN() {
  // met deze afgeleide variabelen worden de windingen en steps berekent
  LengteWinSta = DraadDikte * WinSta;
  LengteWinSto = DraadDikte * WinSto;
  lengteWinMid = LengteVeer - (LengteWinSta + LengteWinSto);
  WinMid = lengteWinMid / SpoedVeer;

  StpM1Sta = WinSta * StepsPerRevolutie;
  StpM1Mid = WinMid * StepsPerRevolutie;
  StpM1Sto = WinSto * StepsPerRevolutie;

  SpSta = DraadDikte;
  SpMid = SpoedVeer;
  SpSto = DraadDikte;

  StpM2Sta = (DraadDikte / SpoedDrijfStang) * StepsPerRevolutie * WinSta;
  StpM2Mid = (SpoedVeer / SpoedDrijfStang) * StepsPerRevolutie * WinMid;
  StpM2Sto = (DraadDikte / SpoedDrijfStang) * StepsPerRevolutie * WinSto;

}




float vraagWaardeViaKnoppenEnDisplay(String label, float stapGrootte) {
  static bool enterPressed;
  static float returnValue;
  enterPressed = false;
  returnValue = 0;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(label);
  //Serial.println(enterPressed);
  //Serial.println(returnValue);
  // de waarde kan onmiddellijk aangepast worden,
  // als ze ingegeven is en dus verschillend is van 0
  // mag de functie terugkeren met een returnValue
  // maar eerst moet de gebruiker op enter duwen
  while (enterPressed == false || returnValue == 0) {
    check_BtnPressed(&EnterBtnState, PIN_ENTER_BTN);
    check_BtnPressed(&SetPlusBtnState, PIN_SET_PLUS_BTN);
    check_BtnPressed(&SetMinBtnState, PIN_SET_MIN_BTN);

    if (EnterBtnState == BTN_DUIDELIJK_SIGNAAL) {
      enterPressed = true;
      EnterBtnState = BTN_SETUP;
    }

    if (SetPlusBtnState == BTN_DUIDELIJK_SIGNAAL) {
      returnValue = returnValue + stapGrootte;
      SetPlusBtnState =  BTN_SETUP;
      enterPressed = false;
    }

    if (SetMinBtnState == BTN_DUIDELIJK_SIGNAAL) {
      if (returnValue - stapGrootte >= 0) {
        returnValue = returnValue - stapGrootte;
        SetMinBtnState =  BTN_SETUP;
      }
      enterPressed = false;
    }
    lcd.setCursor(0, 1);
    lcd.print(returnValue);


  }
  return returnValue;
}

void  PRINTLCD() {
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("DD");
  lcd.setCursor(2, 0);
  lcd.print(DraadDikte);

  lcd.setCursor(7, 0);
  lcd.print("ST");
  lcd.setCursor(9, 0);
  lcd.print(WinSta);

  lcd.setCursor(11, 0);
  lcd.print("SP");
  lcd.setCursor(13, 0);
  lcd.print(WinSto);

  lcd.setCursor(0, 1);
  lcd.print("SPV");
  lcd.setCursor(3, 1);
  lcd.print(SpoedVeer);

  lcd.setCursor(8, 1);
  lcd.print("LV");
  lcd.setCursor(11, 1);
  lcd.print(LengteVeer);


}



void check_AanUit() {
  if (digitalRead(PIN_AANUIT) == LOW) {
    //Serial.println(digitalRead(PIN_AANUIT));
    status = BUZY;
  }
}

void check_ResetKnop() {
  if ((digitalRead(PIN_RESET) == LOW) && Afgewerkt == HIGH) {
    status = RESET;
    Serial.print("rest steps");
    Serial.println(AfgewerkteM2Steps);
  }
}

void berekenStpWaarden(float StepsM1, float StepsM2) {
  if (StepsM1 > StepsM2) {
    // spoed van de veer is groter dan de spoed van de drijfveer (1mm)
    Aanpassing = round((StepsM1 / StepsM2) * 10);
  } else {
    Aanpassing = round((StepsM2 / StepsM1) * 10);
  }
  iAanpassing = static_cast<int>(Aanpassing);
}

void maakVeer2(float StepsM1, float StepsM2) {
  printTabel();
  if (StepsM1 > StepsM2) {
    for (uint32_t iM1 = 0; iM1 * iAanpassing < StepsM1; iM1++) {
      for (uint32_t jM1 = 0; jM1 < 10; jM1++) {
        draaiMotor(PIN_M1_STEPPULSE, 20, HIGH, PIN_M1_DIRPULSE);
        draaiMotor(PIN_M2_STEPPULSE, 20, HIGH, PIN_M2_DIRPULSE);
      }
      for (uint32_t jM1 = 0; jM1 < iAanpassing - 10; jM1++) {
        draaiMotor(PIN_M1_STEPPULSE, 20, HIGH, PIN_M1_DIRPULSE);
      }
    }
  } else {
    for (uint32_t iM1 = 0; iM1 * iAanpassing < StepsM2; iM1++) {
      for (uint32_t jM1 = 0; jM1 < 10; jM1++) {
        draaiMotor(PIN_M1_STEPPULSE, 20, HIGH, PIN_M1_DIRPULSE);
        draaiMotor(PIN_M2_STEPPULSE, 20, HIGH, PIN_M2_DIRPULSE);
      }
      for (uint32_t jM1 = 0; jM1 < iAanpassing - 10; jM1++) {
        draaiMotor(PIN_M2_STEPPULSE, 20, HIGH, PIN_M1_DIRPULSE);
      }
    }
  }
}

void draaiMotor(byte PINPulse, int DT, bool Richting, byte PINDir) {
  digitalWrite(PINDir, Richting);
  delayMicroseconds(DT);
  digitalWrite(PINPulse, HIGH);
  delayMicroseconds(DT);
  digitalWrite(PINPulse, LOW);
}


void RESET_constantMotor(byte PIN, uint32_t pulse) {
  Serial.println("reset1");
  Serial.println(pulse);
  digitalWrite(PIN_M2_DIRPULSE, LOW);
  digitalWrite(PIN_M1_DIRPULSE, LOW);
  delay(10);
  while (pulse > 0) {
    delayMicroseconds(100);
    digitalWrite(PIN, HIGH);
    delayMicroseconds(100);
    digitalWrite(PIN, LOW);
    pulse--;
  }
  digitalWrite(PIN_M2_DIRPULSE, HIGH);
  status = IDLE;
}
void printTabel() {
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.println("|           | Veerdeel  | Veerdeel | Veerdeel  |");
  Serial.println("|           |   Start   |  Midden  |   Stop    |");
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.print("| Windingen |     ");
  Serial.print(WinSta);
  Serial.print("  -  ");
  Serial.print(WinMid);
  Serial.print("  -  ");
  Serial.println(WinSto);
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.print("|   Spoed   |     ");
  Serial.print(SpSta);
  Serial.print("  -  ");
  Serial.print(SpMid);
  Serial.print("  -  ");
  Serial.println(SpSto);
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.print("| Steps M1  |     ");
  Serial.print(StpM1Sta);
  Serial.print("  -  ");
  Serial.print(StpM1Mid);
  Serial.print("  -  ");
  Serial.println(StpM1Sto);
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.print("| Steps M2  |     ");
  Serial.print(StpM2Sta);
  Serial.print("  -  ");
  Serial.print(StpM2Mid);
  Serial.print("  -  ");
  Serial.println(StpM2Sto);
  Serial.println("+-----------+-----------+----------+-----------+");
  Serial.print("| Aanpasng  |     ");
  Serial.println(iAanpassing);
  Serial.println("+-----------+-----------+----------+-----------+");
}


void check_BtnPressed(int *BtnState,byte pin) {
  static unsigned long timer = 0;

  switch (*BtnState) {
    case BTN_SETUP:
      (*BtnState) = BTN_READ;
      break;
    case BTN_READ:
      if (digitalRead(pin) == LOW) {
        (*BtnState) = BTN_START_TIMER;
      }
      break;
    case BTN_START_TIMER:
      timer = millis();
      (*BtnState) = BTN_WACHT;
      break;
    case BTN_WACHT:
      // deze indrukfase van de knop is niet duidelijk dus
      // de knop wordt gereset
      if (digitalRead(pin) == HIGH )
        (*BtnState) = BTN_SETUP;
      // knop werd duidelijk ingedrukt gehouden
      // duidelijk omdat hij gedurende de duur van het
      // INTERVAL geen HIGH signaal heeft door gegeven.
      // Het 'bouncen' is dus voorbij
      if (millis() - timer > INTERVAL)
        (*BtnState) = BTN_INGEDRUKT;
      break;
    case BTN_INGEDRUKT:
      if (digitalRead(pin) == HIGH)
        (*BtnState) = BTN_DUIDELIJK_SIGNAAL;
      break;
    case BTN_DUIDELIJK_SIGNAAL:
      (*BtnState) = BTN_SETUP;
      break;
  }
}