#include "TimerOne.h"
#include "U8glib.h"

// Drive the motor
const byte IN1 = 10;
const byte IN2 = 11;
const byte ENABLE = 6;
const byte ENCO_B = 3;
const byte ENCO_A = 2;

double output_level = 0;

// PID
double err = 0, diff_err = 0, accumulate_err = 0;
const double Kp = 2, Ki = 0, Kd = 6;
const unsigned long DO_PID_CONTROL_IN_US = 1000;

// Settings for the level
long goal_step = 0;
long current_step = 0;

const int DISTANCE_BETWEEN_LEVELS = 50;
const int NUMBER_OF_LEVELS = 5;
const long MAX_GOAL_STEP = DISTANCE_BETWEEN_LEVELS * NUMBER_OF_LEVELS;

// OLED Args and initialization
U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE);

const byte OLED_WIDTH = 128;
const byte OLED_HEIGHT = 32;
const byte PROGESS_BAR_HEIGHT = 8;

void onEncoAChange() {
  if (digitalRead(ENCO_A) == digitalRead(ENCO_B)) {
    current_step++;
  } else {
    current_step--;
  }

  if ((current_step - goal_step) % DISTANCE_BETWEEN_LEVELS >
          DISTANCE_BETWEEN_LEVELS / 2 &&
      goal_step < MAX_GOAL_STEP) {
    goal_step += DISTANCE_BETWEEN_LEVELS;
    accumulate_err = 0;
  }
  if ((goal_step - current_step) % DISTANCE_BETWEEN_LEVELS >
          DISTANCE_BETWEEN_LEVELS / 2 &&
      goal_step > 0) {
    goal_step -= DISTANCE_BETWEEN_LEVELS;
    accumulate_err = 0;
  }
}

void doPIDControl() {
  diff_err = goal_step - current_step - err;
  err = goal_step - current_step;
  accumulate_err += err;

  output_level = Kp * err + Ki * accumulate_err + Kd * diff_err;

  if (output_level > 255) {
    output_level = 255;
  } else if (output_level < -255) {
    output_level = -255;
  }

  if (output_level < 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENABLE, -output_level);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENABLE, output_level);
  }
}

void drawOLED() {
  u8g.firstPage();
  do {
    u8g.drawFrame(0, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2, OLED_WIDTH,
                  PROGESS_BAR_HEIGHT);
    u8g.drawBox(2, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2 + 2,
                (OLED_WIDTH - 4) *
                    (current_step < 0
                         ? 0
                         : (current_step > MAX_GOAL_STEP ? MAX_GOAL_STEP
                                                         : current_step)) /
                    MAX_GOAL_STEP,
                PROGESS_BAR_HEIGHT - 4);
  } while (u8g.nextPage());
}

void setup() {
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENABLE, OUTPUT);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  pinMode(ENCO_B, INPUT);
  pinMode(ENCO_A, INPUT);
  Timer1.initialize();
  Timer1.setPeriod(DO_PID_CONTROL_IN_US);
  Timer1.attachInterrupt(doPIDControl);
  attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
  Serial.begin(115200);
}

void loop() {
  // Serial.print("Goal: ");
  Serial.print('$');
  Serial.print(goal_step);
  // Serial.print("  Current: ");
  Serial.print(", ");
  Serial.print(current_step);
  Serial.print(";");
  drawOLED();
}