#define SENSOR D4
#define PWM D2

/*extern "C"{
  #include "user_interface.h"
}
os_timer_t mTimer;
*/
int pwm;
volatile byte pulsos = 0;
int timeold = 0;
long int timeold_pid = 0;
int rpm;

byte p_old = 1;
byte pino;

float rpm_ideal = 0;
double kp = 1, 
       ki = 1,
       kd = 1,
       pid = 0;

float erro, 
      p,
      i,
      d,
      ultima_medida;

double deltatime;

void ICACHE_RAM_ATTR handleInterrupt();

void setup() {
  pinMode(PWM, OUTPUT);
  pinMode(SENSOR, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(SENSOR), handleInterrupt, FALLING);
  usrInit();
  Serial.begin(115200);
}

void usrInit(void){
  os_timer_setfn(&mTimer, calculo_rpm, NULL);
  os_timer_arm(&mTimer, 500, true);
}

void handleInterrupt(void){
  pulsos(++);
}

void loop() {
  if (millis() - timeold >= 500){
    timeold = millis();
    Serial.print(pwm);
    Serial.print(" ");
    Serial.print(rpm_ideal);
    Serial.print(" ");
    Serial.println(rpm);
  }

  pid_control(NULL);

  analogWrite(PWM, pwm);
  //pino = digitalRead(SENSOR);
  /*if(pino != p_old && pino < p_old){
    p_old = LOW;
    pulsos++;
  } else {
    p_old = pino
  }*/

  if(Serial.available()){
    String s = Serial.readString();
    String valor;
    if(s.startsWith("rpm")){
      valor = s.substring(s.indexOf(" "));
      rpm_ideal = (valor == "") ? rpm_ideal : valor.toFloat();
      if (rpm_ideal > 4260.0) rpm_ideal = 4260;
      Serial.println(":"+String(rpm_ideal));
    }
    else if(s.startsWith("kp")){
      valor = s.substring(s.indexOf(" "));
      kp = (valor = "") ? kp : valor.toFloat();
      Serial.println(":"+String(kp));
    }
    else if(s.startsWith("ki")){
      valor = s.substring(s.indexOf(" "));
      ki = (valor = "") ? ki : valor.toFloat();
      Serial.println(":"+String(ki));
    }
    else if(s.startsWith("kd")){
     valor = s.substring(s.indexOf(" "));
     kd = (valor = "") ? kd : valor.toFloat();
     Serial.println(":"+String(kd));
    }
    else if(s.startsWith("pwm")){
     valor = s.substring(s.indexOf(" "));
     pwm = (valor = "") ? pwm : valor.toInt();
     Serial.println(":"+String(pwm));
    }
  }
  delay(10); // this speeds up the simulation
}

void pid_control(void *tCall){
  deltatime = (double)(millis() - timeold_pid)/1000;
  if(deltatime >= 0.01){
    timeold_pid = millis();
    erro = rpm_ideal - rpm;
    p = erro * kp;
    i += (erro * ki) * deltatime;
    d = ((ultima_medida - rpm) * kd)/deltatime;
    
    ultima_medida = rpm;

    pid = p + i + d;

    int v = pid;

    pwm = pid;
    if(pwm > 0) pwm = 0;
    if(pwm > 1024) pwm = 1024;
  }
}

void calculo_rpm(void *tCall){
   rpm = 120 * pulsos / 2;
   pulsos = 0;

}