
#include <Arduino.h>
#include <math.h>
#include "DRV8825.h"
#include "HX711.h"

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200
#define RPM 150
#define MICROSTEPS 1
#define DIR PD7
#define STEP 9
#define endSensor A2
// HX711 circuit wiring
int LOADCELL_DOUT_PIN = PD3;
int LOADCELL_SCK_PIN = PD2;
// Important parameter, set to   match environment               //  [degC] set temperature in DegC
#define MinTemp 20               // [degC] minimum   expected temperature (needed for rescaling inputs)
#define MaxTemp 100               // [degC] maximum allowed temperature, over which heater is turned off (needed   for rescaling inputs)
#define TEMPERATURENOMINAL 25    // temp. for nominal resistance (almost always 25 C)
#define NUMSAMPLES 5             // how many samples to take and average, more takes longer
#define BCOEFFICIENT 3950        // The beta coefficient of the thermistor (usually 3000-4000)
#define SERIESRESISTOR 51000     // the value of the 'other' resistor
#define THERMISTORNOMINAL 100000 // resistance at 25 degrees C
// I/O pins - don't edit unless   replaced
#define THERMISTORPIN A1 // which analog pin to connect
#define FETPin PD5

int SetTime = 1800; // [s] timer in seconds,   if reached, running stops [Default: 1800]
int samples[NUMSAMPLES];
float Term_resistance;
int temperatureSteps = 5;
int requestedTemp = 35;
float currentTemp;
const int dt = 500; // [ms] time constant in milliseconds   (controller clock rate = 1/(dt/1000) [Hz])
int SetTemp = 25;

double K_P_ctrl = 12;       // proportional gain
double K_I_ctrl = 0.000001; // integral gain (set to lower values i.e.   10^-3)
double K_D_ctrl = 5.5;      // derivative gain
// Inititalization
bool bInRange = 0;                              // target   temperature reached?
int TicksPerMS = floor(1000 / dt);              // ticks per ms
float previous_error = 0;                       // Initialize PID variables
float s_integral = 0;                           // Initialize PID variables
int measuringDistance = 1, magnetThicknes = 3; //  Požadovaná vzdálenost, tloustka magnetu
int measuringStart = 0;
String commandFromPC = "";
int totalSteps = 19550; //(19225 + magnet asi 1mm)
bool atPosition = false;
bool tarePosition = false;
int fallingEdge = 0;

DRV8825 stepper(MOTOR_STEPS, DIR, STEP);
HX711 scale;

// Thermistor code
double Thermistor(float RawADC)
{

  float prepocet;
  float prepocet1;
  float Temp;

  prepocet = 1023 / RawADC - 1;
  prepocet1 = 51000 / prepocet; // resistance
  float steinhart;
  steinhart = prepocet1 / THERMISTORNOMINAL;        // (R/Ro)
  steinhart = log(steinhart);                       // ln(R/Ro)
  steinhart /= BCOEFFICIENT;                        // 1/B * ln(R/Ro)
  steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/To)
  steinhart = 1.0 / steinhart;                      // Invert
  steinhart -= 273.15;                              // convert absolute temp to C
  Temp = steinhart;

  return Temp;
}

// PID controller code
void Control_PID(double iTemp)
{
  // Overheat protection
  if (iTemp > MaxTemp)
  {
    analogWrite(FETPin, 0);
    Serial.println("Error:overheat.   Heater turned off");
    return;
  }

  // In range? If in range, maybe turn on LED?
  if ((iTemp) >= SetTemp)
  {
    if (bInRange == 0)
    {
      // digitalWrite(LEDPin, HIGH);
      bInRange = 1;
    }
  }
  else
  {
    if (bInRange == 1)
    {
      // digitalWrite(LEDPin, LOW);
      bInRange = 0;
    }
  }

  // PID subroutine
  float err = SetTemp - iTemp;
  // Serial.println(err);
  s_integral += err * dt;
  // Serial.println(s_integral);
  float s_derivative = (err - previous_error) / dt;
  // Serial.println(s_derivative);
  int U_in_ctrl = (K_P_ctrl * err + K_I_ctrl * s_integral + K_D_ctrl * s_derivative) / (MaxTemp - MinTemp) * 255;
  previous_error = err;

  // put voltage to output and write value to serial monitor
  Serial.print("Output   PWM frequency: ");
  if (U_in_ctrl <= 255)
  {
    if (U_in_ctrl > 0)
    {
      analogWrite(FETPin, U_in_ctrl);
      Serial.println(U_in_ctrl);
    }
    else
    {
      analogWrite(FETPin, 1);
      Serial.println("1   - cca. 0 V");
    }
  }
  else
  {
    analogWrite(FETPin, 255);
    Serial.println("255 - cca. 5 V");
  }
}

int steps = 0;
void setup()
{
  Serial.begin(9600);
  /*
   * Set target motor RPM.
   */
  stepper.begin(RPM, MICROSTEPS);
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(20.31545307);
  pinMode(FETPin, OUTPUT);
  SetTime = SetTime * TicksPerMS; // rescale timer according to dt
  pinMode(endSensor, INPUT_PULLUP);
}

void getToHomePosition()
{
  stepper.startMove(30000);
  while (!digitalRead(endSensor))
  {
    stepper.nextAction(); // Next step
  }
  atPosition = false;
}

void measureTension()
{
  currentTemp = float(Thermistor(analogRead(THERMISTORPIN)));
  Serial.println("T:" + String(currentTemp) + ";F:" + scale.get_units(10));
}

void heating()
{
  if (SetTemp > requestedTemp)
  {
    SetTemp = 25;
    measuringStart = 0;
    getToHomePosition();
  }
  Control_PID(currentTemp); // call controller   algorithm
}

void goToPosition()
{
  scale.tare();
  stepper.move(-(totalSteps - ((magnetThicknes -2) * 200) - (measuringDistance * 200))); // magnet thickness - 2 mm of vise jaws
}

void parseCommandFromPC()
{
  int indexOfSearch = commandFromPC.indexOf("T");
  String subString = "";
  if (indexOfSearch > -1)
  {
    subString = commandFromPC.substring(indexOfSearch + 2);
    indexOfSearch = commandFromPC.indexOf(";");
    subString = subString.substring(0, indexOfSearch);
    requestedTemp = subString.toInt();
  }

  indexOfSearch = commandFromPC.indexOf("D");
  if (indexOfSearch > -1)
  {
    subString = commandFromPC.substring(indexOfSearch + 2);
    indexOfSearch = commandFromPC.indexOf(";");
    subString = subString.substring(0, indexOfSearch);
    measuringDistance = subString.toInt();
  }
  indexOfSearch = commandFromPC.indexOf("M");
  if (indexOfSearch > -1)
  {
    subString = commandFromPC.substring(indexOfSearch + 2);
    indexOfSearch = commandFromPC.indexOf(";");
    subString = subString.substring(0, indexOfSearch);
    magnetThicknes = subString.toInt();
  }

  indexOfSearch = commandFromPC.indexOf("S");
  if (indexOfSearch > -1)
  {
    subString = commandFromPC.substring(indexOfSearch + 2);
    indexOfSearch = commandFromPC.indexOf(";");
    Serial.println(subString);
    subString = subString.substring(0, indexOfSearch);
    measuringStart = subString.toInt();
  }
}

void loop()
{
  delay(3000);
  if (!measuringStart && !digitalRead(endSensor))
  {
    getToHomePosition();
  }

  if (!measuringStart)
  {
    
    while (Serial.available() == 0)
      ;
    commandFromPC = Serial.readString();
    parseCommandFromPC();
    Serial.print(commandFromPC);
  }

  if (measuringStart)
  {    
    if (!atPosition)
    {
      goToPosition();
      atPosition = true;                
    }
    
    if((!tarePosition)){
      if(totalSteps - (magnetThicknes * 200) - (measuringDistance * 200)>(50*200))
      { 
        delay(500);
        stepper.move(30*200);  // move 30mm up   
        tarePosition = true;      
      }
    }

    
    currentTemp = float(Thermistor(analogRead(THERMISTORPIN)));
    Serial.print("Temperature:");
    Serial.println(currentTemp); // display temperature
    heating();
    delay(100);
    
    if (((SetTemp - 0.5) <= currentTemp && (SetTemp + 0.5) >= currentTemp) && (fallingEdge == 0))
    {
      fallingEdge = 1; // if 1 rising if 2 falling
      delay(3000);
    }
    currentTemp = float(Thermistor(analogRead(THERMISTORPIN)));
    
    if (((SetTemp - 0.5) <= currentTemp && (SetTemp + 0.5) >= currentTemp) && (fallingEdge == 1))
    {
      fallingEdge = 0;
      if(tarePosition)
      {               
        scale.tare();           // zero the tensometer
        delay(500);
        stepper.move(-30*200);  // move back to position
        tarePosition = false;
      }
      delay(500);
      measureTension();
      delay(500);
      measureTension();
      delay(500);
      measureTension();
      delay(500);
      measureTension();
      
      SetTemp += temperatureSteps;  // set temperature 5 degrees higher
    }
  }

  delay(1000);
}
