#include <Arduino.h>

#include <LiquidCrystal_I2C.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#include "PID2.h"
#include "SIM.h"
#include "motor.h"
#include "NN5.h"

//const float dt = 0.1;    // Time step
float fordulatszam = 0.0f;
//float fordulatszam_nn = 0.0f;

Rotax915iS motor(600.0);
Integrator throttle;
PIDD pid1;
NN controller(0.0001, 0.9);

static void IRAM_ATTR pid_task(void* arg)
{

  lcd.init();
  lcd.backlight();
  Serial.begin(115200);
  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    xTaskDelayUntil(&xLastWakeTime, 100);

    /*lcd.setCursor(0, 0);
      lcd.print( p_weights[0], 4);
      lcd.setCursor(0, 1);
      lcd.print( p_weights[1], 4);
      lcd.setCursor(0, 2);
      lcd.print( p_weights[2], 4);
      lcd.setCursor(0, 3);
      lcd.print( p_weights[3], 4);
      lcd.setCursor(10, 0);
      lcd.println( p_bias, 4);*/

    static float teher = 2.0; //2.625; //Nm

    if (digitalRead(4) == LOW)
      teher += random(-100, 100);

    if (digitalRead(17) == LOW)
      teher += 10;
    if (digitalRead(18) == LOW)
      teher -= 10;

    if (teher < 2.0)
      teher = 2.0;
    if (teher > 170)
      teher = 170;

    float governor;
    float governor_nn;

    if (digitalRead(27) == LOW) {
      controller.setMode(PID_TRAINING);
      governor = pid1.update(fordulatszam / 5500.0);
      governor_nn = controller.calc(1.0 - fordulatszam / 5500.0, governor);
    } else {
      controller.setMode(USE_TRAINED);
      governor = controller.calc(1.0 - fordulatszam / 5500.0, 0);
    }
    float thr = throttle.update(governor);

    if (thr < 0.39)
      thr = 0.39;

    fordulatszam = motor.rpm(thr * 100.0, teher);

    Serial.print( teher / 170, 4);
    Serial.print( "," );
    Serial.print( fordulatszam / 5500, 4 );
    Serial.print( "," );
    Serial.print( thr, 4);
    Serial.print( "," );
    Serial.print( governor, 4);
    Serial.print( "," );
    Serial.println( governor_nn, 4);
  }
}

void setup() {

  setCpuFrequencyMhz(240);




  // Print something
  lcd.setCursor(3, 0);
  lcd.print("Hello, world!");
  lcd.setCursor(2, 1);
  lcd.print("Wokwi Online IoT");
  lcd.setCursor(5, 2);
  lcd.print("Simulator");
  lcd.setCursor(7, 3);
  lcd.print("Enjoy!");

  pid1.set_params(6.5, 0, 1.23, 0);
  pid1.set_filters(0.0, 0.0, 0.0);
  throttle.set_time(1.6);

  pinMode(4, INPUT_PULLUP);
  pinMode(17, INPUT_PULLUP);
  pinMode(18, INPUT_PULLUP);
  pinMode(27, INPUT_PULLUP);

  xTaskCreatePinnedToCore(pid_task, "pid_task", 4096, NULL, 2, NULL, 1);

  delay(100);

  vTaskDelete(NULL);

}

void loop() {


  //delay( dt * 1000 ); /*wait the time-step*/
}

//0.2908	-0.7737	-0.6991	0.1911	0.9905

//-0.52532742	-0.60901686	-0.54931828	0.33482837	1.33564088

//-0.32718389	-0.86158632	0.85797037	0.16636327	0.16439068