//Todo
//Remove use of analog pins for easier swapping between Uno and Teensy due to 3.3 limit on Teensy
//For this reason it is a good idea to connect OUTPUT pins to other devices with 470Ω or 1k resistors, unless maximum current draw from the pins is required for a particular application.

//https://programmingelectronics.com/make-one-button-functionality-two-arduino/
//https://github.com/mathertel/OneButton
//http://www.mathertel.de/Arduino/OneButtonLibrary.aspx

//Libraries
#include <FreqCount.h>
#include <LiquidCrystal.h>
#include <OneButton.h>
#include <EEPROM.h>

//Floating Map Function
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//Pin Setup, 0 for Uno, 1 for Teensy 3.1
//Only works on Teensy 3.1 due to need for 5v
//THC A-D Input must be on pin 5 on Uno or pin 13 on Teensy 3.1

  //LCD Pins
  #define LCD_RS      12
  #define LCD_EN      11
  #define LCD_D4       6
  #define LCD_D5       4
  #define LCD_D6       8
  #define LCD_D7       7
  #define LCD_CHARS   20
  #define LCD_LINES    4

  //Encoder Pins
  int encoderPinA = 3;
  int encoderPinB = 2;

  //New OneButton
  OneButton button(13,true);



//LCD Pin Setup and Custom Character
LiquidCrystal lcd(LCD_RS, LCD_EN, LCD_D4, LCD_D5, LCD_D6, LCD_D7);
uint8_t testChar[8] = {0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff}; // Custom char


//Variable Setup
int16_t last, value, testval, tipval, TargetVal, EncoderValue;
bool flash; //Bool value for flashing text on screen
unsigned long startMillis, torchMillis, currentMillis, count;  //some global variables available anywhere in the program
long int frq;
float TorchVal;


int lastEncoded;
int TorchHysteresis; //Voltage offset from target before outputs activate




void setup() {

  FreqCount.begin(10);

  //Variable setup
    flash = false;
    lastEncoded = 0;
    EncoderValue = EEPROM.read(0); //Read starting value from EEPROM 0
    TargetVal = EncoderValue; //Set this equal to EncoderVal during setup
    TorchHysteresis = 4; //Voltage offset from target before outputs activate


  //Timer setup
    startMillis = millis();
    torchMillis = millis();


  //Button setup
    button.attachClick(ClickFunction); //Create Click function for button
    button.attachDoubleClick(DoubleClickFunction); //Create Double Click function for button
    button.attachLongPressStart(HoldFunction); //Create Hold function for button


  //Output setup
    pinMode(9, OUTPUT); //Torch Up output
    digitalWrite(9, LOW); //Turn output off
    pinMode(10, OUTPUT); //Torch Down output
    digitalWrite(10, LOW); //Turn output off


  //LCD Setup
    lcd.begin(LCD_CHARS, LCD_LINES);
    lcd.clear();
    lcd.createChar(0, testChar); //Create custom full block char and send to LCD


  //LCD startup message
    lcd.setCursor(0,0);
    lcd.print("CNC Plasma Torch");
    lcd.setCursor(0,1);
    lcd.print("Height Control");
    delay(500);
    lcd.clear();


  //LCD text setup
    lcd.setCursor(0, 0);
    lcd.print("Tip Voltage:");
    lcd.setCursor(0, 1);
    lcd.print("Tip Target:");
    lcd.setCursor(12, 1);
    lcd.print(TargetVal); //Write initial TargetVal, updated by button press
    lcd.setCursor(0,3);
    lcd.print("UP:");
    lcd.setCursor(6,3);
    lcd.print("Down:");
    lcd.setCursor(14,3);
    lcd.print("Off:");


  //Encoder Setup
    pinMode(encoderPinA, INPUT_PULLUP);
    pinMode(encoderPinB, INPUT_PULLUP);

    //get starting position
    int lastMSB = digitalRead(encoderPinA);
    int lastLSB = digitalRead(encoderPinB);

    //let start be lastEncoded so will index on first click
    lastEncoded = (lastMSB << 1) |lastLSB;

}//Setup

void loop() {

  currentMillis = millis(); //Grab current loop time, millis used to not delay code execution

  button.tick(); //Check button for input


  int MSB = digitalRead(encoderPinA); //MSB = most significant bit
  int LSB = digitalRead(encoderPinB); //LSB = least significant bit
  int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
  if(sum == 0b1101 || sum == 0b0010)EncoderValue ++;
  if(sum == 0b1110 || sum == 0b0001)EncoderValue --;


 lastEncoded = encoded; //store this value for next time

  if (FreqCount.available()) {
    TorchVal = mapfloat(FreqCount.read(), 1197, 9265, 0.0, 10.0);
    TorchVal = TorchVal*50;
  }

if (currentMillis - torchMillis >= 250) { //Delay printing of TorchVal to make it easier to read
  torchMillis = millis();
  lcd.setCursor(13,0);
  lcd.print("       ");

  lcd.setCursor(13,0);
  if (TorchVal <= 0){
    lcd.print("-----");
  }
  else{
    lcd.print(TorchVal,1);
  }
}

  if(EncoderValue != TargetVal){
    if(EncoderValue <= 100)EncoderValue = 100; //Don't let EncoderValue below 100
    if(EncoderValue >= 150)EncoderValue = 150; //Don't let EncoderValue above 150
    lcd.setCursor(12, 1);
    lcd.print(EncoderValue);

    //Begin code for flashing Tip Target text
    if (currentMillis - startMillis >= 500 && flash) { //Hide Tip Target text
      startMillis = millis();
      lcd.setCursor(0, 1);
      lcd.print("           ");
      flash = false;
    }
    else if(currentMillis - startMillis >= 300 && !flash) { //Show Tip Target text
      startMillis = millis();
      lcd.setCursor(0, 1);
      lcd.print("Tip Target:");
      flash = true;
    }
  }
  else{ //Make sure flashing text is returned to on in case we exit while off
    lcd.setCursor(0, 1);
    lcd.print("Tip Target:");
    lcd.setCursor(12, 1);
    lcd.print(EncoderValue);
  }


//Code for moving torch based on tip value compared to requested value, add outputs here.
  if(TorchVal < (TargetVal - TorchHysteresis)){ //Voltage too low, raise torch
    //Turn Down off, Up on
      digitalWrite(10, LOW);
      digitalWrite(9, HIGH);
    //Print Square
      lcd.setCursor(4,3);
      lcd.print((char)0);
    //Clear Other Squares
      lcd.setCursor(12,3);
      lcd.print(" ");
      lcd.setCursor(19,3);
      lcd.print(" ");
  } //End If
  else if(TorchVal > (TargetVal + TorchHysteresis)){ //Voltage too high, lower torch
    //Turn Up off, Down on
      digitalWrite(9, LOW);
      digitalWrite(10, HIGH);
    //Print Square
      lcd.setCursor(12,3);
      lcd.print((char)0);
    //Clear Other Squares
      lcd.setCursor(19,3);
      lcd.print(" ");
      lcd.setCursor(4,3);
      lcd.print(" ");
  } //End Else If
  else { //Voltage Stable - Outputs Off
    //Turn both outputs off
      digitalWrite(10, LOW);
      digitalWrite(9, LOW);
    //Print Square
      lcd.setCursor(19,3);
      lcd.print((char)0);
    //Clear Other Squares
      lcd.setCursor(4,3);
      lcd.print(" ");
      lcd.setCursor(12,3);
      lcd.print(" ");
  } //End Else


}// Loop


void ClickFunction() { //Runs on single click of encoder button
  TargetVal = EncoderValue; //Save current EncoderValue to TargetVal
}

void DoubleClickFunction() { //Runs on double click of encoder button
  EncoderValue = EEPROM.read(0); //Recall saved value from EEPROM 0
}

void HoldFunction() { //Runs on hold of encoder button
  EEPROM.write(0,0); //Clear EEPROM before write, maybe unnecessary
  EEPROM.write(0, TargetVal); //Write current TargetVal to EEPROM 0
}