#include <Wire.h>
#include <U8g2lib.h>
#include <AccelStepper.h>
#include <EZButton.h>

// Stepper motor driver
#define STEP_PIN  2  //step
#define DIR_PIN   5  //direction
#define EN_PIN 6    //stepper driver enable pin

// Define a stepper and the pins it will use  6400 steps per round with TCM2208 and NEMA 17 Creality 42-34
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN); // (Type of driver: with 2 pins, STEP, DIR - for TMC2208 Aliexpres driver)
int motorMaxSpeed = 120;    //50  maximum steps per second (about 3rps / at 16 microsteps)
int motorAccel = 10;  //steps/second/second to accelerate

//Display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
bool needDisplayUpdate = false;

//Encoder with switch
#define ENCODER_CLK 3   // CLK pin 
#define ENCODER_DT 8    // DT pin 

//encoder
volatile int encoderPos = 0;
int lastEncoderPos = 0;
unsigned long lastInterruptTime = 0;
unsigned long debounceDelay = 40; // debounce time 20 default

//button
#define BTN_1_PIN 9
#define BTN_1 0
void ReadButtons(bool *states, int num) {
  //Read  button states
  states[BTN_1] = !digitalRead(BTN_1_PIN); }

EZButton _ezb(1, ReadButtons, 1000, 500, 5);

//relay 2 module
int relay_2 = 4;
int relay_1 = 7;

//DCC orientation timer
unsigned long previousPolarityMillis;
unsigned long previousLedMillis;
int poziciaMotora;

int encoderPolarity;
int RedLed1 = 10; 
int RedLed2 = 11;
int GreenLed1 = A2;
int GreenLed2 = A1;

#define homeSwitch A0
byte homePrevState;
volatile boolean homingFlag = false;

ISR (PCINT8_vect)
{
  flag();
}

////////////////////////////////////////////////////////////////////////////////////

void setup() {

  // encoder 
  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);
 
  // button 
  pinMode(BTN_1_PIN, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);

   //subscribe to needed events for button
  _ezb.Subscribe(BTN_1, Btn1Pressed, PRESSED);
  _ezb.Subscribe(BTN_1, Btn1Released, RELEASED);
  _ezb.Subscribe(BTN_1, Btn1Hold, HOLD);
  _ezb.Subscribe(BTN_1, Btn1HoldReleased, HOLD_RELEASED);

  // motor driver;
  pinMode (EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);
  stepper.setMaxSpeed(motorMaxSpeed);
  stepper.setAcceleration(motorAccel);
  //stepper.moveTo(0);

  //relay set
  pinMode(relay_1, OUTPUT);
  pinMode(relay_2, OUTPUT);
  digitalWrite(relay_1, LOW);
  digitalWrite(relay_2, LOW);

  //display
  u8g2.begin();

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_fub49_tn); 
  u8g2.setCursor(22 , 57);
  u8g2.println("00");
  u8g2.sendBuffer();

  pinMode (homeSwitch, INPUT_PULLUP);

  while (digitalRead(homeSwitch)) {
    stepper.setMaxSpeed(1500);
    stepper.setAcceleration(2000);
    stepper.runToNewPosition(5);
    stepper.setCurrentPosition(0);
     }
  while (!digitalRead(homeSwitch)) {
    stepper.setMaxSpeed(1500);
    stepper.setAcceleration(2000);
    stepper.runToNewPosition(5);
    stepper.setCurrentPosition(0);
     }
     stepper.setCurrentPosition(0);
  
  //PCIFR  |= bit (PCIF1);   // clear any outstanding interrupts
  PCICR  |= bit (PCIE1);   // enable pin change interrupts for D8 to D13
  PCMSK1 |= bit (PCINT8);  // want pin A0
 
 attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), handleEncoder, CHANGE);

  pinMode(RedLed1, OUTPUT);
  pinMode(RedLed2, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(GreenLed1, OUTPUT);
  pinMode(GreenLed2, OUTPUT);

  stepper.setMaxSpeed(motorMaxSpeed);
  stepper.setAcceleration(motorAccel);
 }

void flag() {
  if (digitalRead(homeSwitch) != HIGH) {
    stepper.stop ();
  }
homingFlag = false;
}
 
//////////////////////////////////////////////////////////////////////////////////

void loop() {

PCMSK1 = 0b00000000;

  if (homingFlag == false) {
    digitalWrite(12, HIGH);
    digitalWrite(RedLed1, HIGH);
    digitalWrite(RedLed2, HIGH);
    digitalWrite(GreenLed1, HIGH);
    digitalWrite(GreenLed2, HIGH);
    delay(500);
  
    stepper.runToNewPosition(-1434);//-1433
    homingFlag = true;
    stepper.setCurrentPosition(0);
  }

_ezb.Loop();

  if (encoderPos != lastEncoderPos) {
    lastEncoderPos = encoderPos;
    needDisplayUpdate = true;
  }

 if(needDisplayUpdate) {
    if(encoderPos <= 9) {
    u8g2.clearBuffer();
    u8g2.setCursor(40, 57);
    u8g2.setFont(u8g2_font_fub49_tn);
    u8g2.println(encoderPos);
    u8g2.sendBuffer();
    }
    else {
    u8g2.clearBuffer();
    u8g2.setCursor(22, 57);
    u8g2.setFont(u8g2_font_fub49_tn);
    u8g2.println(encoderPos);
    u8g2.sendBuffer();
    }
		needDisplayUpdate = false;
 }

//correct DCC polarity to position
unsigned long currentPolarityMillis = millis();
if(currentPolarityMillis - previousPolarityMillis >= 1000 ) { 
previousPolarityMillis = currentPolarityMillis;
checkPolarity();
  }

//poziciaMotora = stepper.currentPosition()/320;
//Serial.println(stepper.currentPosition()/320);

unsigned long currentLedMillis = millis();
if(currentLedMillis - previousLedMillis >= 977 ) { 
previousLedMillis = currentLedMillis;
checkLed();
  }



}

/////////////////////////////////functions////////////////////////////////

void handleEncoder() {
  unsigned long currentMillis = millis();

  if (currentMillis - lastInterruptTime > debounceDelay) {
    int CLKstate = digitalRead(ENCODER_CLK);
    int DTstate = digitalRead(ENCODER_DT);

    if (CLKstate == DTstate) {
      encoderPos--;
      if (encoderPos <= 1 ) {
        encoderPos = 1;
        }
    } else {
      encoderPos++;
      if (encoderPos >= 20 ) {
        encoderPos = 20;
        }
    }

    lastInterruptTime = currentMillis;
  }
}

///////////////////////////////////////button/////////////////////////////////////

void Btn1Pressed() {
  digitalWrite(LED_BUILTIN, HIGH);
    long kamIdeMotor = ( 320 * encoderPos );
    digitalWrite(GreenLed1, LOW);
    digitalWrite(GreenLed2, LOW);
    digitalWrite(RedLed1, HIGH);
    digitalWrite(RedLed2, HIGH);
    stepper.moveTo(kamIdeMotor);
    stepper.setMaxSpeed(motorMaxSpeed);
    stepper.setAcceleration(motorAccel);
    stepper.runToPosition();
    encoderPolarity = encoderPos;
    checkLed();
delay(10);
}

void Btn1Released() {
  digitalWrite(LED_BUILTIN, LOW);
}

bool state = true;
void Btn1Hold() {
  
  state = !state;
  digitalWrite(LED_BUILTIN, state);
    u8g2.clearBuffer();
    u8g2.setCursor(15, 45);
    u8g2.setFont(u8g2_font_fub30_tf);
    u8g2.println(stepper.currentPosition());
    u8g2.sendBuffer();
}

void Btn1HoldReleased() {
  for (int i = 0; i < 6; i++) {
    digitalWrite(EN_PIN, HIGH);
    state = !state;
    digitalWrite(LED_BUILTIN, state);
    
    delay(5);
  }
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(EN_PIN, LOW);
}

////////////////////////////////////////polarity////////////////////////////////

//change polarity if necessary
void checkPolarity() {
if (encoderPolarity >= 6 && encoderPolarity < 14)   {
//Serial.println("Normal Polarity");
digitalWrite(relay_1, LOW);

digitalWrite(relay_2, LOW);
    }
else if (encoderPolarity >= 16 && encoderPolarity < 21)   {
//Serial.println("Normal Polarity");
digitalWrite(relay_1, LOW);
digitalWrite(relay_2, LOW);
    }
    else {
//Serial.println("Reversed Polarity");
digitalWrite(relay_1, HIGH);
digitalWrite(relay_2, HIGH);
    }
}

///////////////////////////////////// LED status ////////////////////////////////
void checkLed() {

  if(encoderPolarity == 4 ) {
  greenLedBoth();
}
else if(encoderPolarity == 14 ) {
  greenLedBoth();
}
else if(encoderPolarity == 15 ) {
  greenLed1();
}
else if(encoderPolarity == 6 ) {
  greenLedBoth();
}
else if(encoderPolarity == 16 ) {
  greenLedBoth();
}
else if(encoderPolarity == 17 ) {
  greenLed2();
}
else if(encoderPolarity == 7 ) {
  greenLed1();
}
else if(encoderPolarity == 5 ) {
  greenLed2();
}
else {
  ledRedBoth();
}

}


void ledRedBoth() {
  digitalWrite(RedLed1, HIGH);
  digitalWrite(RedLed2, HIGH);
  digitalWrite(GreenLed1, LOW);
  digitalWrite(GreenLed2, LOW);
}

void greenLed1() {
  digitalWrite(RedLed1, LOW);
  digitalWrite(RedLed2, HIGH);
  digitalWrite(GreenLed1, HIGH);
  digitalWrite(GreenLed2, LOW);
}
void greenLed2() {
  digitalWrite(RedLed1, HIGH);
  digitalWrite(RedLed2, LOW);
  digitalWrite(GreenLed1, LOW);
  digitalWrite(GreenLed2, HIGH);
}
void greenLedBoth() {
  digitalWrite(GreenLed2, HIGH);
  digitalWrite(GreenLed1, HIGH);
  digitalWrite(RedLed1, LOW);
  digitalWrite(RedLed2, LOW);
}