#include "FastAccelStepper.h"
#include <FastPID.h>

#define dirPinStepper    5
#define enablePinStepper 13
#define stepPinStepper   6
#define StepInput   3
#define DirInput   4
#define THC_ON   7
#define THC_ON_LED   13

int potpin1 = 1;  // analog pin used to connect the potentiometer
int potpin2 = 2;  // analog pin used to connect the potentiometer
int ARC_PV;    // variable to read the value from the analog pin
int ARC_SP;    // variable to read the value from the analog pin
int val = 0;
int dir;

float Kp=2, Ki=1, Kd=0.1, Hz=100;
int output_bits = 8;
bool output_signed = false;
int16_t output;
int16_t Test;
FastPID ARC_PID(Kp, Ki, Kd, Hz, output_bits, output_signed);

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

void setup() {
  Serial.begin(9600);
  pinMode(8, OUTPUT);
  pinMode(THC_ON, INPUT_PULLUP);
  pinMode(THC_ON_LED, OUTPUT);
  pinMode(StepInput, INPUT_PULLUP);
  pinMode(DirInput, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(StepInput), Pass_Step, RISING);

   engine.init();
   stepper = engine.stepperConnectToPin(stepPinStepper);
   if (stepper) {
      stepper->setDirectionPin(dirPinStepper);
      stepper->setEnablePin(enablePinStepper);
      stepper->setAutoEnable(true);

      stepper->setSpeedInHz(5000);       // 500 steps/s
      stepper->setAcceleration(5000);    // 100 steps/s²
      //stepper->move(1000);
   }
   ARC_PID.setOutputRange(-400, 400);
}

void loop() {
  val = digitalRead(THC_ON);
  if (val == HIGH) {
    digitalWrite(THC_ON_LED, HIGH);
    ARC_PV = analogRead(potpin1);            // reads the value of the potentiometer (value between 0 and 1023)
    ARC_PV = map(ARC_PV, 0, 1023, 0, 200);     // scale it to use it with the servo (value between 0 and 180)
    ARC_SP = analogRead(potpin2);            // reads the value of the potentiometer (value between 0 and 1023)
    ARC_SP = map(ARC_SP, 0, 1023, 0, 200);     // scale it to use it with the servo (value between 0 and 180)

    Serial.print("Setpoint: ");
    if ((ARC_PV > ARC_SP) and (output > -400)){
      output = ARC_PID.step(ARC_SP, ARC_PV);
    }
    if ((ARC_PV < ARC_SP) and (output < 400)){
      output = ARC_PID.step(ARC_SP, ARC_PV);
    } 
    Serial.println(output);
  }
  else {
    digitalWrite(THC_ON_LED, LOW);
  }
  stepper->moveTo(output);
  if (stepper->isRunning()){
    digitalWrite(8, HIGH);
  }
  else {
    digitalWrite(8, LOW);
  }
  //delay(1000);
  //stepper->moveTo(50);
}
void Pass_Step(){
  dir = digitalRead(DirInput);
  if (dir == LOW) {
    output++;
  }
  else {
    output--;
  }
}