#include "Arduino.h"

#include <Wire.h>
#include <SPI.h>  
#include <Wire.h>  
#include <Adafruit_GFX.h>  
#include <Adafruit_SSD1306.h>  
#include <TimerOne.h>


#define screen_width 128 // OLED display width, in pixels  
#define screen_height 64 // OLED display height, in pixels  
#define OLED_RESET 4  
Adafruit_SSD1306 display(screen_width, screen_height);  

namespace
{
constexpr int arcVoltageInput = A0;
constexpr int pulseIntervalInput = A2;
constexpr int setPointInput = A3;
constexpr int interruptPin = 2;
constexpr int arcOkInput = 4;
constexpr int dirPin = 5;
constexpr int highLedPin = 8;
constexpr int grblDirPin = 7;
constexpr int lowLedPin = 10;
constexpr int stepPin = 9;
constexpr int arcOkLedPin = 12;
constexpr int engageThcSwitch = 3;

constexpr unsigned deadband = 5;
int arcVoltage;
int setPoint;
long pulseInterval = 800;
bool arcOk;
bool cw = true;
bool arcVHigh;
bool arcVLow;
bool engageThc = HIGH;
}

void readInputs()
{
  arcVoltage = analogRead(arcVoltageInput);
  setPoint = map(analogRead(setPointInput), 474, 1024, 778, 614);//Will need conversion to arcVoltage range.
  pulseInterval = map(analogRead(pulseIntervalInput), 0, 1024, 0, 10000);//In microseconds.
  engageThc = !digitalRead(engageThcSwitch);
  arcOk = !digitalRead(arcOkInput);//Switches THC on only when arcOk signal from plasma machine.
}


void setup() {
  pinMode(arcOkInput, INPUT_PULLUP);
  pinMode(dirPin, OUTPUT);
  pinMode(grblDirPin, INPUT);
  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(stepPin, OUTPUT);
  pinMode(engageThcSwitch, INPUT_PULLUP);
  pinMode(highLedPin, OUTPUT);
  pinMode(lowLedPin, OUTPUT);
  pinMode(arcOkLedPin, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(interruptPin), passThrough, FALLING);
  Timer1.initialize();
  //Timer1.pwm(stepPin, 126);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 

  Serial.begin(115200);
  Serial.println("Stand alone THC starting");
  display.clearDisplay();  
  display.setRotation(2);
  display.setTextSize(1.5);    
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);  
  display.drawRect(0, 0, 128, 64, WHITE);
  display.setCursor(19, 10);  
  display.print("SD THC STARTING");  
  display.setCursor(19, 25);  
  display.print("ENZ TRONIC LABS"); 
  display.setCursor(35, 40);  
  display.print("PLASMA CUT"); 
  display.display();  
  delay(5000);
  display.clearDisplay(); 
  display.setTextSize(0.5);
  display.setCursor(3, 25);
  display.print("SetV"); 
  display.setCursor(67, 5);
  display.print("ArcV"); 
  display.setCursor(67, 25);
  display.print("Spd"); 
  display.drawRect(0, 0, 128, 20, WHITE);
  display.drawRect(0, 0, 128, 40, WHITE);
  display.drawRect(0, 0, 64, 40, WHITE);
  display.drawRect(0, 0, 128, 64, WHITE);
  display.display();  
 // delay(10000);
}

void loop() {
  readInputs();
  updateLcd();
  arcOk = !digitalRead(arcOkInput);//Switches THC on only when arcOk signal from plasma machine.
  arcVHigh = arcVoltage > setPoint + deadband;
  arcVLow = arcVoltage < setPoint - deadband;

  if (engageThc) {
    digitalWrite(arcOkLedPin, HIGH);
    if (arcOk) {
      if (arcVHigh) {
        digitalWrite(dirPin, HIGH);
        digitalWrite(highLedPin, HIGH);
        digitalWrite(lowLedPin, LOW);
        Timer1.pwm(stepPin, 512, pulseInterval);
      }
      else if (arcVLow) {
        digitalWrite(dirPin, LOW);
        digitalWrite(lowLedPin, HIGH);
        digitalWrite(highLedPin, LOW);
        Timer1.pwm(stepPin, 512, pulseInterval);
      }
      else {
        //analogWrite(stepPin, 255);
        digitalWrite(lowLedPin, LOW);
        digitalWrite(highLedPin, LOW);
        Timer1.pwm(stepPin, 1024, pulseInterval);
      }
    }

    else {      

      stop();
    }
  }
  else {
    stop();
    digitalWrite(arcOkLedPin, LOW);
  }
}
void passThrough()
{
  digitalWrite(stepPin, LOW);
  digitalWrite(stepPin, HIGH);
}

void stop()
{
  Timer1.stop();
  Timer1.disablePwm(stepPin);
  digitalWrite(dirPin, digitalRead(grblDirPin));
        digitalWrite(lowLedPin, LOW);
        digitalWrite(highLedPin, LOW);  
}
void updateLcd()
{
  long previousMillis;
  if (previousMillis + 500 <= millis()) {
    if (engageThc){
    
      if (arcOk) {
      display.setTextSize(1);    
      display.setCursor(3, 5);  
      display.print("Active ");
      display.print(" ");
      display.display();    
      
         if (arcVHigh) {
        display.setTextSize(1.75);    
        display.setCursor(3, 45);  
        display.print("THC DOWN ");
        display.print("         ");
        display.cp437(true);
        display.setTextSize(2);
        display.setCursor(80, 45);
        display.write(0x19);
        display.write("  ");
        display.display();  

        }
        else if (arcVLow) {
        display.setTextSize(1.75);     
        display.setCursor(3, 45);  
        display.print("THC UP ");
        display.print("       ");
        display.cp437(true);
        display.setTextSize(2);
        display.setCursor(80, 45);
        display.write(0x18);
        display.write("  ");
        display.display(); 
        }

      else {
        display.setTextSize(1.75);    
        display.setCursor(3, 45);  
        display.print("THC OK ");
        display.print("       ");
        display.setTextSize(2);
        display.setCursor(80, 45);
        display.write("  ");
        display.display(); 
       }
    }
    else {
      display.setTextSize(1);    
      display.setCursor(3, 5);  
      display.print("inactive ");
      display.print(" ");
      display.display(); 
      display.setTextSize(1.75);    
      display.setCursor(3, 45);  
      display.print("         ");
      display.setTextSize(2);
      display.setCursor(80, 45);
      display.write("   ");
      display.display(); 
    }
    }
    else {
      display.setTextSize(1);    
      display.setCursor(3, 5);  
      display.print("Disable");
      display.print(" ");
      display.display();
      display.setTextSize(1.75);    
      display.setCursor(3, 45);  
      display.print("         ");
      display.setTextSize(2);
      display.setCursor(80, 45);
      display.write("   ");
      display.display(); 
       }
    int V = ((50 * (float(setPoint) / 10)) / 205) * 10;
    int Va = ((50 * (float(arcVoltage) / 10)) / 205) * 10;
    int SPD = pulseInterval/100;
    display.setTextSize(1);    
    display.setCursor(34, 25);  
    display.print(V);
    display.print(" ");
    display.setCursor(98, 5);  
    display.print(Va);
    display.print(" ");
    display.setCursor(98, 25);
    display.print(SPD);
    display.print("%");
    display.print(" ");
    display.display(); 

    previousMillis = millis();
  }
}