#include <Arduino.h>
#include <PID_v1.h>
#include <MsTimer2.h> //Timer2 for velocity measurement
#include <LiquidCrystal.h>
#include <Wire.h>
#include <Servo.h> //Timer1 for ESC control
#include <EEPROM.h>

double Setpoint = 0, Input, Output;
float Kp = 2, Ki = 1, Kd = 0.5;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
unsigned int direct = 1000, status1 = 0, status2 = 0, button = 0, mode = 0;
volatile unsigned long previousMillis = 0, previousMicros = 0, dmillis = 0, dmicros = 0, backlight_time = 0, backlight_timeout = 0;
volatile unsigned int enc = 0, bool1 = 0, bool2 = 0;
const double inverval = 100; // unit ms
double count = 0;
const double line = 10;      // grating line
const unsigned int maxspeed = 24000;
const byte rs = 4, en = A0, d4 = 13, d5 = 7, d6 = 8, d7 = 9;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
Servo esc;
const byte grating = 3, sck = 2, dt = 12, button_encoder = A1, switch_control = A2, analog_in = A3, esc_out = 10, pwm = 5, pwm_out = 6, backlight = 11;
void count1();
void count2();
void rpm();
void encoder1();
void encoder2();
void backlight_on();

void count1()
{
  dmicros = micros() - previousMicros;
  if (dmicros > (unsigned long)(60UL * 1000UL * 1000UL / (maxspeed * line) / 2)) // debounce
  {
    if (!digitalRead(grating))
    {
      count += 1;
      attachInterrupt(digitalPinToInterrupt(grating), count2, RISING);
    }
  }
}

void count2()
{
  if (digitalRead(grating))
  {
    attachInterrupt(digitalPinToInterrupt(grating), count1, FALLING);
    previousMicros = micros();
  }
}

void rpm()
{
  detachInterrupt(digitalPinToInterrupt(grating));         // prevent interrupt during reading long type value
  Input = (count / line) / (inverval / 1000 / 60); // rpm
  count = 0;
  attachInterrupt(digitalPinToInterrupt(grating), count1, FALLING);
}

void encoder1()
{
  dmillis = millis() - previousMillis;
  if (dmillis > 10) // debounce
  {
    if (!digitalRead(sck))
    {
      if (digitalRead(dt))
      {
        enc = 1;
        bool1 = 1;
      }
      else
      {
        enc = 2;
        bool1 = 2;
      }
      attachInterrupt(digitalPinToInterrupt(sck), encoder2, RISING);
      backlight_on();
    }
  }
}

void encoder2()
{
  if (digitalRead(sck))
  {
    if (bool1 == 1)
    {
      if (digitalRead(dt))
      {
        return;
      }
    }
    if (bool1 == 2)
    {
      if (!digitalRead(dt))
      {
        return;
      }
    }
    attachInterrupt(digitalPinToInterrupt(sck), encoder1, FALLING);
    previousMillis = millis();
  }
}

void backlight_on()
{
  digitalWrite(backlight, HIGH);
  backlight_time = millis();
}

void setup()
{
  pinMode(button_encoder, INPUT_PULLUP);
  pinMode(switch_control, INPUT_PULLUP);
  pinMode(grating, INPUT_PULLUP);
  pinMode(sck, INPUT_PULLUP);
  pinMode(dt, INPUT_PULLUP);
  pinMode(analog_in, INPUT);
  pinMode(esc_out, OUTPUT);
  pinMode(pwm, OUTPUT);
  esc.attach(esc_out); // ESC control
  lcd.begin(16, 2);
  backlight_on();
  pid.SetOutputLimits(1000, 2000); // ESC input 1000-2000 uS
  pid.SetMode(AUTOMATIC);
  Kp = EEPROM.read(0);
  Ki = EEPROM.read(1);
  Kd = EEPROM.read(2);
  mode = EEPROM.read(3);
  MsTimer2::set(inverval, rpm);
  attachInterrupt(digitalPinToInterrupt(grating), count1, FALLING);
  attachInterrupt(digitalPinToInterrupt(sck), encoder1, FALLING);
}

void loop()
{
  if (digitalRead(switch_control) == 0) // directly control
  {
    if (bool2 != 1)
    {
      status1 = 0;
      direct = 1000;
      bool2 = 1;
      backlight_on();
    }
    if (digitalRead(button_encoder) == 0)
    {
      if (button == 0)
      {
        status1++;
        button = 1;
        backlight_on();
        if (status1 > 2)
        {
          status1 = 0;
        }
      }
    }
    else
    {
      button = 0;
    }
    if (status1 == 1)
    {
      if (enc == 1)
      {
        enc = 0;
        if (direct < 2000)
        {
          direct = direct + 5;
        }
        else
        {
          direct = 2000;
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (direct > 1000)
        {
          direct = direct - 5;
        }
        else
        {
          direct = 1000;
        }
      }
    }
    if (status1 == 2)
    {
      if (enc == 1)
      {
        enc = 0;
        if (mode == 0)
        {
          mode = 1;
          EEPROM.write(3, mode);
          direct = 1000;
        }
        else
        {
          mode = 0;
          EEPROM.write(3, mode);
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (mode == 0)
        {
          mode = 1;
          EEPROM.write(3, mode);
          direct = 1000;
        }
        else
        {
          mode = 0;
          EEPROM.write(3, mode);
        }
      }
    }
    if (mode == 0)
    {
      direct = map(analogRead(analog_in), 0, 1023, 1000, 2000);
    }
    esc.writeMicroseconds(direct);
    analogWrite(pwm, map(direct, 1000, 2000, 0, 255));
    analogWrite(pwm_out, map(direct, 1000, 2000, 0, 255));
    lcd.setCursor(0, 0);
    if (status1 == 1)
    {
      lcd.print("PWM:>");
    }
    else
    {
      lcd.print("PWM: ");
    }
    lcd.print(direct);
    lcd.print("                ");
    lcd.setCursor(0, 1);
    lcd.print("RPM: ");
    lcd.print(round(Input));
    lcd.print("     ");
    lcd.setCursor(14, 1);
    if (status1 == 2)
    {
      lcd.print(">");
    }
    else
    {
      lcd.print(" ");
    }
    if (mode == 0)
    {
      lcd.print("A");
    }
    else
    {
      lcd.print("M");
    }
  }

  else // PID control
  {
    if (bool2 != 2)
    {
      status2 = 0;
      Setpoint = 0;
      bool2 = 2;
      backlight_on();
    }
    if (digitalRead(button_encoder) == 0)
    {
      if (button == 0)
      {
        status2++;
        button = 1;
        backlight_on();
        if (status2 > 5)
        {
          status2 = 0;
        }
      }
    }
    else
    {
      button = 0;
    }

    if (status2 == 1)
    {
      if (enc == 1)
      {
        enc = 0;
        if (Setpoint < maxspeed)
        {
          Setpoint = Setpoint + 100;
        }
        else
        {
          Setpoint = maxspeed;
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (Setpoint > 0)
        {
          Setpoint = Setpoint - 100;
        }
        else
        {
          Setpoint = 0;
        }
      }
    }

    if (status2 == 2)
    {
      if (enc == 1)
      {
        enc = 0;
        if (Kp < 9.9)
        {
          Kp = Kp + 0.1;
          EEPROM.write(0, Kp);
        }
        else
        {
          Kp = 9.9;
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (Kp > 0)
        {
          Kp = Kp - 0.1;
          EEPROM.write(0, Kp);
        }
        else
        {
          Kp = 0;
        }
      }
    }

    if (status2 == 3)
    {
      if (enc == 1)
      {
        enc = 0;
        if (Ki < 9.9)
        {
          Ki = Ki + 0.1;
          EEPROM.write(1, Ki);
        }
        else
        {
          Ki = 9.9;
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (Ki > 0)
        {
          Ki = Ki - 0.1;
          EEPROM.write(1, Ki);
        }
        else
        {
          Ki = 0;
        }
      }
    }

    if (status2 == 4)
    {
      if (enc == 1)
      {
        enc = 0;
        if (Kd < 9.9)
        {
          Kd = Kd + 0.1;
          EEPROM.write(2, Kd);
        }
        else
        {
          Kd = 9.9;
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (Kd > 0)
        {
          Kd = Kd - 0.1;
          EEPROM.write(2, Kd);
        }
        else
        {
          Kd = 0;
        }
      }
    }
    if (status2 == 5)
    {
      if (enc == 1)
      {
        enc = 0;
        if (mode == 0)
        {
          mode = 1;
          EEPROM.write(3, mode);
          Setpoint = 0;
        }
        else
        {
          mode = 0;
          EEPROM.write(3, mode);
        }
      }
      if (enc == 2)
      {
        enc = 0;
        if (mode == 0)
        {
          mode = 1;
          EEPROM.write(3, mode);
          Setpoint = 0;
        }
        else
        {
          mode = 0;
          EEPROM.write(3, mode);
        }
      }
    }
    enc = 0;
    if (mode == 0)
    {
      Setpoint = map(analogRead(analog_in), 0, 1023, 0, maxspeed);
    }
    pid.SetTunings(Kp, Ki, Kd);
    pid.Compute();
    esc.writeMicroseconds(Output);
    analogWrite(pwm, map(Output, 1000, 2000, 0, 255));
    analogWrite(pwm_out, map(Output, 1000, 2000, 0, 255));

    lcd.setCursor(0, 0);
    if (status2 == 1)
    {
      lcd.print("RPM:>");
    }
    else
    {
      lcd.print("RPM: ");
    }
    lcd.print(round(Setpoint));
    lcd.print("/");
    lcd.print(round(Input));
    lcd.print("                ");
    lcd.setCursor(0, 1);
    if (status2 == 2)
    {
      lcd.print("P>");
    }
    else
    {
      lcd.print("P");
    }
    lcd.print(Kp, 1);
    if (status2 == 3)
    {
      lcd.print(" I>");
    }
    else
    {
      lcd.print(" I");
    }
    lcd.print(Ki, 1);
    if (status2 == 4)
    {
      lcd.print(" D>");
    }
    else
    {
      lcd.print(" D");
    }
    lcd.print(Kd, 1);
    if (status2 == 5)
    {
      lcd.print(">");
    }
    else
    {
      lcd.print(" ");
    }
    if (mode == 0)
    {
      lcd.print("A");
    }
    else
    {
      lcd.print("M");
    }
    lcd.print("                ");
  }
  backlight_timeout = millis() - backlight_time;
  if (backlight_timeout > 60000)
  {
    digitalWrite(backlight, LOW);
  }
}