// -----
// InterruptRotator.ino - Example for the RotaryEncoder library.
// This class is implemented for use with the Arduino environment.
//
// Copyright (c) by Matthias Hertel, http://www.mathertel.de
// This work is licensed under a BSD 3-Clause License. See http://www.mathertel.de/License.aspx
// More information on: http://www.mathertel.de/Arduino
// -----
// 18.01.2014 created by Matthias Hertel
// 04.02.2021 conditions and settings added for ESP8266
// 03.07.2022 avoid ESP8266 compiler warnings.
// 03.07.2022 encoder instance not static.
// -----

// This example checks the state of the rotary encoder using interrupts and in the loop() function.
// The current position and direction is printed on output when changed.

// Hardware setup:
// Attach a rotary encoder with output pins to
// * 2 and 3 on Arduino UNO. (supported by attachInterrupt)
// * A2 and A3 can be used when directly using the ISR interrupts, see comments below.
// * D5 and D6 on ESP8266 board (e.g. NodeMCU).
// Swap the pins when direction is detected wrong.
// The common contact should be attached to ground.
//
// Hints for using attachinterrupt see https://www.arduino.cc/reference/en/language/functions/external-interrupts/attachinterrupt/

#include <Arduino.h>
#include <RotaryEncoder.h>

#include "utility.h"

// Example for Arduino UNO with input signals on pin A4 and A5
#define PIN_IN1 A4
#define PIN_IN2 A5

// Include utility after pin definitions
#define A_in0 A0
#define A_in1 A1
#define A_in2 A2
#define A_in3 A3

#define PIN_FAHRT 6
#define PIN_LANGSAM 7

#define PIN_Nullanschlag 2
#define LANGSAM_WINKEL 100 //0.1°

#define LOGGING_MS 2000 //ms
#define TIMEOUT_NACH_REF_UPDATE 1000 //ms

#define LOG_POS_UPDATE true

// A pointer to the dynamic created rotary encoder instance.
// This will be done in setup()
RotaryEncoder *encoder = nullptr;

// This interrupt routine will be called on any change of one of the input signals
void checkPosition()
{
  encoder->tick(); // just call tick() to check the state.
}


void setup()
{
  Serial.begin(115200);
  Serial.println("Startup Rotary Encoder");

  pinMode(PIN_Nullanschlag, INPUT_PULLUP);
  pinMode(PIN_FAHRT, OUTPUT);
  pinMode(PIN_LANGSAM, OUTPUT);

  // setup the rotary encoder functionality

  // use FOUR3 mode when PIN_IN1, PIN_IN2 signals are always HIGH in latch position.
  // encoder = new RotaryEncoder(PIN_IN1, PIN_IN2, RotaryEncoder::LatchMode::FOUR3);

  // use FOUR0 mode when PIN_IN1, PIN_IN2 signals are always LOW in latch position.
  // encoder = new RotaryEncoder(PIN_IN1, PIN_IN2, RotaryEncoder::LatchMode::FOUR0);

  // use TWO03 mode when PIN_IN1, PIN_IN2 signals are both LOW or HIGH in latch position.
  encoder = new RotaryEncoder(PIN_IN1, PIN_IN2, RotaryEncoder::LatchMode::FOUR3);

  // register interrupt routine
  attachInterrupt(digitalPinToInterrupt(PIN_IN1), checkPosition, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_IN2), checkPosition, CHANGE);
} // setup()


// Read the current position of the encoder and print out when changed.
void loop(){
  unsigned long currentMillis = millis();
  static unsigned long LoggingMillis = 0;
  static unsigned long posUpdateMillis = 0;
  static int32_t refPos = 0;
  static uint8_t prev_Nullanschlag_state = 0;
  static int state = 0; //0 = uncalibrated, 1= waiting, 2=forward, 3=backward, 4=pos reached
  static int pos = 0;

  
  if(digitalRead(PIN_Nullanschlag) == LOW){
    //Debounce input
    if(prev_Nullanschlag_state == digitalRead(PIN_Nullanschlag) == LOW){
      if(state == 0){
        Serial.println("Nullanschlag");
      }
      encoder->setPosition(0);
      state = 1;
    }
    prev_Nullanschlag_state = digitalRead(PIN_Nullanschlag) == LOW;
  }

  //check input position from TA
  bool invalid_pos = false;
  int new_refPos = 0;//get_reference_position(A_in0,A_in1,A_in2, A_in3);
  if(new_refPos > 3600){
    invalid_pos = true;
    new_refPos = 3600;
  }

  if(refPos != new_refPos){
    posUpdateMillis = currentMillis;
    refPos = new_refPos;      
    Serial.print("ref pos: ");
    Serial.println(refPos);

    if(state != 0){ //only if already callibrated
      state = 1;
    }
  }


  encoder->tick(); // just call tick() to check the state.

  int newPos = encoder->getPosition();
  if (pos != newPos) {
    int dir = (int)(encoder->getDirection());
    if(LOG_POS_UPDATE){
      Serial.print("pos:");
      Serial.print(newPos);
      Serial.print(" dir:");
      Serial.println(dir);
    }
    pos = newPos;

    //TODO does not work
    if( dir < 0){
      state = 3; //rückwärtsfahrt
    }else if(state == 3){
      state = 2;
    }
  }

  //starte zu fahren nach timeout von position update
  if(state == 1 && (currentMillis > posUpdateMillis + TIMEOUT_NACH_REF_UPDATE)){
    state = 2;
  }

  //check ref position falls am forwärts fahren
  if(state == 2 && pos > refPos){
    state = 4;
  }


  //set outputs
  if(state == 2){
    digitalWrite(PIN_FAHRT, HIGH);
  }else{
    digitalWrite(PIN_FAHRT, LOW);
  }

  //langsam fahrt erreicht, und nur wenn fahrt aktiv
  if((state == 2 || state == 3) && pos < LANGSAM_WINKEL){
    digitalWrite(PIN_LANGSAM, HIGH);
  }else{
    digitalWrite(PIN_LANGSAM, LOW);
  }

  //logging
  if(currentMillis > LoggingMillis + LOGGING_MS){
    LoggingMillis = currentMillis;
    Serial.print("state: ");
    Serial.print(state);
    Serial.print(", pos: ");
    Serial.print(pos);
    Serial.print(", ref: ");
    Serial.print(refPos);

    if(state == 4){
      Serial.print(", refpos reached");
    }
    Serial.println();

    if(invalid_pos){
      Serial.println("invalid reference, limited to 360.0°");
    }
  }  
} // loop ()