#include "math.h"
#include <PID_v1.h>

//HEATING
#define THERMISOR_PIN A0
const byte HEATER_PIN = 6;
const byte VCC = 5;
const int B_TEMP_CONST = 4220;
const float T0_K = 298.15;
double tempReading, DIFF, rThermistor, temp, limit;

//STIRRING
double target, readRPM;
double writeRPM = 24;

PID motorPID(&readRPM, &writeRPM, &target, 0.04,0.014,0.012, DIRECT); //last known good 0.017 0.025 0.01

int things[100];

volatile long unsigned last = 0;
volatile long unsigned now = 1;
static unsigned long last_interrupt = 0;
volatile bool doit = false;

float RPM = 0;
float RPMsmooth = 0;
float RPMlast = 0;
float expCoeff = 0.05;

float phTarget, tempTarget, rpmTarget;

void setup() {
  Serial.begin(9600);
  pinMode(THERMISOR_PIN, INPUT);
  //pinMode(8, OUTPUT);
  //digitalWrite(8, HIGH);
  Serial.begin(9600);
  pinMode(3, OUTPUT);
  pinMode(2, INPUT); //interrupt
  analogWrite(3, 27);
  pinMode(8, OUTPUT); //heating element control
  attachInterrupt(digitalPinToInterrupt(2), thing, RISING);
  target = 500;
  motorPID.SetMode(AUTOMATIC);

}

void loop() {
  //use 5v on arduino

  //Serial.println(RTH);


  if(Serial.available() > 0)
  {
    if(Serial.readStringUntil('#') == "Confirm")
    {
      phTarget = Serial.readStringUntil(',').toFloat();
      tempTarget = Serial.readStringUntil(',').toFloat();
      rpmTarget = Serial.readStringUntil('\n').toFloat();
    }
    //Serial.println(target);
  }


  float RPM = readControlStirring(rpmTarget);

  float temp_DEG = readControlTemperature(tempTarget);

  float PH = 7;


  Serial.print(RPM); Serial.print(",");
  Serial.print(temp_DEG); Serial.print(",");
  Serial.print(PH); Serial.println(",#");


}



double readControlTemperature(float inputTemp){


  double vThermistor = readThermistor();

  double temp_DEG = calculateTemp(vThermistor);

  delay(10);

  //Serial.println(temp_DEG);
  delay(100);
  int limit = 1.25;
  if (temp_DEG <= inputTemp - limit ){
    //analogWrite(HEATER_PIN, 255);
    digitalWrite(8, HIGH);

    //Serial.println("ON");
  }
  else {
    //analogWrite(HEATER_PIN, 0);
    //Serial.println("OFF");
    digitalWrite(8, LOW);
  }

  return temp_DEG;

}


float readControlStirring(float Target){

  target = Target;

  if(micros()-now > 1000000) { last = now - 60000000000; }
  RPM = 30000000.0/float((now - last));
  RPMsmooth = expCoeff * RPM + (1-expCoeff)*RPMlast;
  readRPM = RPMsmooth;
  //Serial.print(RPMsmooth); Serial.print(" ");
  //Serial.println(Target);

  if (RPMsmooth < 2800)
  {
  motorPID.Compute();
  }
  analogWrite(3, writeRPM);

  RPMlast = RPMsmooth;
  //if (doit == true) {Serial.println(millis()); doit = false;}

  return RPMsmooth;

}





double readThermistor(){

  double vThermistor = analogRead(THERMISOR_PIN);

  return vThermistor;

}


double calculateTemp(double vThermistor){

  double vResistor = VCC * (vThermistor/1023);

  double rThermistor = (9780*vResistor)/(VCC-vResistor);

  double temp_K = 1 / ((log(rThermistor / 10000) / B_TEMP_CONST ) + (1/T0_K));

  double temp_DEG = temp_K-273.15;

  return (temp_DEG - 1);

}

void thing()
{
  unsigned long interrupt_now = millis();
  if(interrupt_now - last_interrupt > 4)
  {
    //doit = true;
    last = now;
    now = micros();
  }
  last_interrupt = interrupt_now;
}