#include <Encoder.h>
#include <Adafruit_SSD1306.h>

//Encoder pins
#define E_CLK 7
#define E_DT 8
#define E_SW 4

//Switch pins
#define SW_L 6
#define SW_R 5
#define SW_R_LI 3
#define SW_L_LI 2

//Motor pins
#define M_ENBL A2
#define M_STEP A1
#define M_DIR A0

//Display settings
#define D_HEIGHT 32
#define D_WIDTH 128
#define D_I2C_ADDR 0x3C

Adafruit_SSD1306 display(D_WIDTH, D_HEIGHT, &Wire, -1);

boolean motorActive = false;

int setSpeed = 1;

Encoder speed(E_DT,E_CLK);

//Add time values for swith debouncing
unsigned long E_SWtime = 0, SW_Ltime = 0, SW_Rtime = 0, SW_R_LItime = 0, SW_L_LItime = 0, D8time = 0;
unsigned long screenRefresh = 0;

//int encoderLastState = 0;

//int encoderState = 0;

void rightLimit() {
  Serial.println("Right Limit");
  digitalWrite(M_ENBL, HIGH);
  delay(100);
  digitalWrite(M_DIR, LOW);
  delay(100);
  digitalWrite(M_ENBL, LOW);
}

void leftLimit() {
  Serial.println("Left Limit");
  digitalWrite(M_ENBL, HIGH);
  delay(100);
  digitalWrite(M_DIR, HIGH);
  delay(100);
  digitalWrite(M_ENBL, LOW);
}

void motorSW() {
  Serial.println("Motor Toggle");
  digitalWrite(M_ENBL, !digitalRead(M_ENBL));
  motorActive = !motorActive;
}

void setup() {
  Serial.begin(115200);

  //Set up Encoder
  pinMode(E_SW, INPUT_PULLUP); //Switch
  pinMode(E_DT, INPUT_PULLUP);
  pinMode(E_CLK, INPUT_PULLUP);
  //encoderLastState = digitalRead(E_CLK);

  //Set up Switches
  pinMode(SW_L, INPUT);
  pinMode(SW_R, INPUT);
  pinMode(SW_R_LI, INPUT);
  pinMode(SW_L_LI, INPUT);

  //Set up display
  display.begin(SSD1306_SWITCHCAPVCC, D_I2C_ADDR);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);

  //Set up Motor
  pinMode(M_ENBL, OUTPUT);
  pinMode(M_STEP, OUTPUT);
  pinMode(M_DIR, OUTPUT);
  digitalWrite(M_ENBL, HIGH);
  digitalWrite(M_DIR, HIGH);
}

void loop() {
  //Read speed setting from Encoder
  long encoder = speed.read();

  if (setSpeed != encoder) {
    if (encoder <= 0) {
      setSpeed = 1;
    } else {
      setSpeed = encoder;
    }
  }
  
  if (SW_R_LI && millis()-SW_R_LItime > 50) {
    rightLimit();
    SW_R_LItime = millis(); 
  }

  if (SW_L_LI && millis()-SW_L_LItime > 50) {
    leftLimit();
    SW_L_LItime = millis(); 
  }

  if (!E_SW && millis()-E_SWtime > 50) {
    motorSW();
    E_SWtime = millis(); 
  }

  if (SW_L && millis()-SW_Ltime > 50 && !motorActive) {
    Serial.println("right switch");
    digitalWrite(M_DIR, HIGH);
    SW_Ltime = millis();
  }

  if (SW_R && millis()-SW_Rtime > 50 && !motorActive) {
    Serial.println("left switch");
    digitalWrite(M_DIR, LOW);
    SW_Rtime = millis();
  }


  Serial.println(setSpeed);

  display.clearDisplay();

  if (digitalRead(M_DIR) == HIGH) {
    display.println("right");
  } else {
    display.println("left");
  }

  if (millis()-screenRefresh > 1000) {
    display.println("speed: "+(String)setSpeed);
    display.display();
    screenRefresh = millis();
  }

  if(motorActive) {
    digitalWrite(M_STEP, HIGH);
    delay(20/setSpeed);
    digitalWrite(M_STEP, LOW);
    delay(20/setSpeed);
  }
}