#include "Arduino.h"
#include <EEPROM.h>
#include <ButtonDebounce.h>
#include <LCD_I2C.h>

#include <Keypad.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2
#define  ROWS 4
#define COLS 3
#define FWD 3
#define BWD 4
#define throttlePin A0

char keys[ROWS][COLS] = {
  { '1', '2', '3' },
  { '4', '5', '6' },
  { '7', '8', '9' },
  { '*', '0', '#' }
};
uint8_t colPins[COLS] = { 5, 6, 7 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 8, 9, 10, 11 }; // Pins connected to R1, R2, R3, R4

char key;
char lastKey;
// int throttlePin;
int lastThrottle;
int newThrottle;
int LocoAddress = 3;
int LocoDirection = 1;
int lastLocoDir = 1;
int LocoSpeed = 0;
int counter = 0;
bool powerState;
bool debug = true;

ButtonDebounce powerBtn(2, 10);
LCD_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

void setup() {
  // put your setup code here, to run once:
  powerState = false;
  // throttlePin = A0;
  lastThrottle = 0;
  newThrottle = 0;
  pinMode(FWD, INPUT_PULLUP);
  pinMode(BWD, INPUT_PULLUP);
  Serial.begin (115200);

  getAddress();
  powerBtn.setCallback(changePowerState);

  lcd.begin();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Power Off");
}

void loop() {
  newThrottle = analogRead(throttlePin);        // reads the value of the potentiometer (value between 0 and 1023)
  newThrottle = map(newThrottle, 0, 1023, 0, 127);
  powerBtn.update();   // read the input power button

  if (powerState) {
    doKeypad();
    if (checkState()) {
      doDCC();
      doLCD();
    }
  }
}

void getDirection() {
  if (!digitalRead(FWD)) {
    LocoDirection = 1;
  }
  if (!digitalRead(BWD)) {
    LocoDirection = 0;
  }
}

bool checkState() {
  // check state
  bool stateChanged = false;
  if (newThrottle != lastThrottle) {
    LocoSpeed = newThrottle;
    lastThrottle = newThrottle;
    stateChanged =  true;
  }
  getDirection();
  if (lastLocoDir != LocoDirection) {
    lastLocoDir = LocoDirection;
    stateChanged = true;
  }
  return stateChanged;
}

void changePowerState(const int state) {
  if (state) {
    lcd.clear();
    if (powerState) {
      powerState = false;
      if (debug) Serial.println("Turn power Off");
      Serial.println("<0>");
      lcd.setCursor(0, 0);
      lcd.print("Power Off");
    } else {
      powerState = true;
      if (debug) Serial.println("Turn power On");
      Serial.println("<1>");
      getAddress();
      doLCD();
    }
  }
}

void doDCC() {
  //Serial.print("d = ");
  if (debug) Serial.println(LocoDirection);
  Serial.print("<t1 ");
  Serial.print(LocoAddress);//locoID);
  Serial.print(" ");
  Serial.print(LocoSpeed);
  Serial.print(" ");
  Serial.print(LocoDirection);
  Serial.write(">");
  Serial.println();
}

void setLocoAddress() {
  //Serial.print("<0>");// power off to DCC++ unit
  int total = 0;
  counter = 0;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Set Dcc Addr => ");
  do {
    key = keypad.getKey();
    if (key) {
      if(key == '*') break; // exit routine if right button pressed - ABORT new address
      counter++;
      int number =  key - 48;
      total = total * 10 + number;
      if (debug == 1) Serial.print("TOTAL= ");
      if (debug == 1) Serial.println(total);
      lcd.setCursor(0, 1);
      if (total == 0) {   // print multiple zeros for leading zero number
        for (int tempx = 1; tempx <= counter; tempx++) {
          lcd.print("0");
        }
      }
      else lcd.print(total);
      if (debug == 1) Serial.print("Counter = ");
      if (debug == 1) Serial.print(counter);
      if (debug == 1) Serial.print("  key= ");
      if (debug == 1) Serial.print(key);

      if (debug == 1) Serial.print("   val =");
      if (debug == 1) Serial.println(number);
    }
  }
  while (counter <= 3); //  collect exactly 4 digits
  //  while (  TrigVal2 == 1);
  LocoAddress = total;
  saveAddresses();
  lcd.clear();
  doLCD();
}

void getAddress() {
  int xxx = 0;
  if (debug) Serial.print("add0 = ");
  if (debug) Serial.print(EEPROM.read(0));
  if (debug) Serial.print(" add1 = ");
  if (debug) Serial.println(EEPROM.read(1));
  LocoAddress = EEPROM.read(0) * 256;
  LocoAddress = LocoAddress + EEPROM.read(1);
  if (LocoAddress >= 10000 || LocoAddress < 1) LocoAddress = 3;

  if (debug) Serial.println(" ");
  if (debug) Serial.print("loco = ");
  if (debug) Serial.print(LocoAddress);
  if (debug) Serial.println(" ");
}

void saveAddresses() {
  int xxx = 0;
  int xyz = 0;
  xxx = LocoAddress / 256;
  if (debug) Serial.println(" ");
  if (debug) Serial.print("loco = ");
  if (debug) Serial.print(LocoAddress);
  if (debug) Serial.print(" => msb ");
  if (debug) Serial.print(xxx);
  if (debug) Serial.print(" writing to ");
  if (debug) Serial.print(0);
  if (debug) Serial.print(" and ");
  if (debug) Serial.print(1);
  EEPROM.write(0, xxx);
  xxx = LocoAddress - (xxx * 256);
  if (debug) Serial.print(" lsb ");
  if (debug) Serial.print(xxx);
  EEPROM.write(1, xxx);

  if (debug) Serial.print("add0 = ");
  if (debug) Serial.print(EEPROM.read(0));
  if (debug) Serial.print(" add1 = ");
  if (debug) Serial.println(EEPROM.read(1));
}

void doLCD() {
  //lcd.setCursor(0, 0);
  //lcd.print("               ");
  lcd.setCursor(0, 0);
  lcd.print("S=");
  lcd.print(LocoSpeed, DEC);
  lcd.print("  ");
  lcd.setCursor(6, 0);
  if (LocoDirection == 1 ) {
    lcd.print(">>>");
  }
  else {
    lcd.print("<<<");
  }

  lcd.setCursor(10, 0);

  lcd.print("L=");
  if (LocoAddress <= 9) {
    lcd.print("0");  // add leading zero for single digit addresses
  }
  lcd.print(LocoAddress, DEC);
  // lcd.print(ActiveAddress + 1);
  // lcd.setCursor(5, 1); // start of 2nd line
  // String temp = "0000" + String(LocoFN0to4, BIN);  // pad with leading zeros
  // int tlen = temp.length() - 5;
  // lcd.print(temp.substring(tlen));
  // temp = "000" + String(LocoFN5to8, BIN);
  // tlen = temp.length() - 4;
  // lcd.setCursor(0, 1); // start of 2nd line
  // lcd.print(temp.substring(tlen));

}

void doKeypad() {
  key = keypad.getKey();
  if (key != NO_KEY)  {
    if (key != lastKey) {
      if(debug) Serial.println(key);
      if(key == '*') setLocoAddress();
      lastKey = key;
    }
  }
}