/********************************************************
   QuickPID simulated 6dof drone Example
   Reading simulated analog input 0 to control analog PWM output 3
 ********************************************************/

// This is an incomplete adaptation of a heater simulation

// Simulation https://wokwi.com/projects/361764563106419713
//
//  Based on
// Simulation at https://wokwi.com/projects/359818835102393345
//  PID_v1 sim at https://wokwi.com/projects/359088752027305985
//  Wokwi https://wokwi.com/projects/357374218559137793
//  Wokwi https://wokwi.com/projects/356437164264235009

//#include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library
#define HACK_QPID_PE_MULT 0
#include "QuickPID.h" // https://github.com/Dlloydev/Arduino-PID-Library
// For 1D heat transfer model:
#include <BasicLinearAlgebra.h> // https://github.com/tomstewart89/BasicLinearAlgebra

#include <sTune.h> // https://github.com/Dlloydev/sTune

//Define Variables we'll be connecting to
float Setpoint, Input, Output;

//Specify the links and initial tuning parameters

//float Kp = 20, Ki = .01, Kd = 10; // works reasonably with sim heater block fo 220deg
//float Kp = 25.5, Ki = 0.001, Kd = 0; // +/-10°proportional band
//float Kp = 255, Ki = 0.001, Kd = 0; // works reasonably with sim heater block
//float Kp = 255, Ki = 0.0, Kd = 0; // +/-1° proportional band works reasonably with sim heater block
//float Kp = 10000, Ki = 0.0, Kd = 0.0; // bang-bang
float Kp = 25, Ki = 5.0, Kd = 0.0; // P-only
//float Kp = 25, Ki = 0.0, Kd = 0.0; // P-only
//float Kp = 2, Ki = 0.0, Kd = 0.0; // P-only
//float Kp = 2, Ki = 5, Kd = 1; // commonly used defaults

//PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, P_ON_E, DIRECT);

//Specify PID links
QuickPID myPID(&Input, &Output, &Setpoint);


// expose state vector to other routines
const int n_ele = 2;
static BLA::Matrix<n_ele> x; //initial
const int ele_sensor = 0;  // 0..(n_ele-1)

const int PWM_PIN = 3;  // UNO PWM pin for Output
const int FAN_PIN = 5;  // UNO PWM pin for Fan Output

const int INPUT_PIN = -1; // Analog pin for Input (set <0 for simulation)
const int AUTOMATIC_PIN = 8; // Pin for controlling manual/auto mode, NO
const int OVERRIDE_PIN = 12; // Pin for integral override, NO
const int PLUS_PIN = 4; // Pin for integral override, NO
const int MINUS_PIN = 7; // Pin for integral override, NO
const int STUNE_PIN = 11; // Pin for initiating sTune
const int SHIFT_PIN = 2;

#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);

enum MODES {MANUAL_MODE = 0, AUTO_MODE = 1, TUNE_MODE = 2} mode = MANUAL_MODE;
const char * modes[] = {"Manual", "Auto  ", "Tuning"};
float incValue; // for editing params
uint16_t control_time = 50000U; // us on/off cycle for softpwm

sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.direct5T, tuner.printALL);
/*                                         ZN_PID           directIP     serialOFF
                                           DampedOsc_PID    direct5T     printALL
                                           NoOvershoot_PID  reverseIP    printSUMMARY
                                           CohenCoon_PID    reverse5T    printDEBUG
                                           Mixed_PID
                                           ZN_PI
                                           DampedOsc_PI
                                           NoOvershoot_PI
                                           CohenCoon_PI
                                           Mixed_PI
*/


//  end of globals



void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  //apply PID gains
  myPID.SetTunings(Kp, Ki, Kd);
  myPID.SetOutputLimits(-255, 255); // 
  pinMode(OVERRIDE_PIN, INPUT_PULLUP);
  pinMode(AUTOMATIC_PIN, INPUT_PULLUP);
  pinMode(MINUS_PIN, INPUT_PULLUP);
  pinMode(PLUS_PIN, INPUT_PULLUP);
  pinMode(SHIFT_PIN, INPUT_PULLUP);

  pinMode(PWM_PIN, OUTPUT);
  pinMode(FAN_PIN, OUTPUT);
  pinMode(STUNE_PIN, INPUT_PULLUP);

  Setpoint = 0;
  incValue = 1;

  //turn the PID on
  if (digitalRead(AUTOMATIC_PIN) == HIGH) {
    myPID.SetMode(myPID.Control::automatic);
    mode = AUTO_MODE;
  } else {
    myPID.SetMode(myPID.Control::manual);
    mode = MANUAL_MODE;
  }
  myPID.SetMode(digitalRead(AUTOMATIC_PIN) == HIGH ? myPID.Control::automatic : myPID.Control::manual);
  if (INPUT_PIN > 0) { // Actual pin or simulated ?
    Input = analogRead(INPUT_PIN);
  } else {
    Input = simArm(0.0); // simulate heating
  }
  lcd.init();
  lcd.backlight();

  Serial.println("Setpoint Input Output Integral");
}

void loop()
{
  // gather Input from INPUT_PIN or simulated block

  float fanWatts = Output * 10.0 / 255; // 10W fan
  //float heaterWatts = (digitalRead(PWM_PIN)==HIGH) * 5.0 ; // 5W heater

  if (INPUT_PIN > 0 ) { // Actual pin or simulated heater?
    Input = analogRead(INPUT_PIN);
  } else {
    float blockTemp = simArm(fanWatts); // simulate heating
    Input = blockTemp;   // read input from simulated heater block
  }

  if (myPID.Compute() || myPID.GetMode() == (uint8_t)myPID.Control::automatic);
  {
    //Output = (int)Output; // Recognize that the output as used is integer
    analogWrite(PWM_PIN, Output >= 0 ? Output : 0); // Heater
    analogWrite(FAN_PIN, Output < 0 ? -Output * 255 / 4.0 : 0); // Fan
  }
  // duty cycle or analogWrite above
  //softPwm(PWM_PIN,Output >= 0 ? Output : 0,control_time);

  doButtons();
  static uint32_t lastButton = 0;
  if (myPID.GetMode() == (uint8_t)myPID.Control::manual && millis() - lastButton > 250) {
    if (digitalRead(STUNE_PIN) == LOW) { // initiate sTune
      mode = 2; // stune
      lcd.clear();
      lcd.setCursor(0, 2);
      lcd.print("sTune: see serial");
      delay(1000);
      sTuneRunner();
      mode = MANUAL_MODE;

    }
  }

  report();
  reportLCD();

}

int softPwm(byte pin, float val, uint32_t control_time_us) {
  static uint32_t last = 0;
  uint32_t now = micros();
  if (now - last >= control_time_us) {
    if (val > 0)digitalWrite(pin, HIGH);
    last = now;
  }
  if (now - last >= (val * control_time_us) / 255) {
    digitalWrite(pin, LOW);
  }
  return now - last < (val * control_time_us) / 255;

}

void doButtons(void) {
  uint32_t now = millis();
  static uint32_t shiftMs = 0, minusMs = 0, plusMs = 0;
  static byte lastShift = HIGH, lastMinus = HIGH, lastPlus = HIGH;
  byte shift = digitalRead(SHIFT_PIN);
  byte minus = digitalRead(MINUS_PIN);
  byte plus = digitalRead(PLUS_PIN);

  if (digitalRead(OVERRIDE_PIN) == LOW) mySetIntegral(&myPID, 0); // integral override
  if ((digitalRead(AUTOMATIC_PIN) == HIGH !=  mode == AUTO_MODE) && mode != TUNE_MODE) {
    switch (digitalRead(AUTOMATIC_PIN)) {
      case HIGH:
        mode = AUTO_MODE;
        myPID.SetMode(myPID.Control::automatic);
        break;
      case LOW:
        mode = MANUAL_MODE;
        myPID.SetMode(myPID.Control::manual);
        break;
    }
  }
  if (plus == LOW && now - plusMs > 250) {
    if (mode == MANUAL_MODE) {
      Output += incValue;
    } else if (mode == AUTO_MODE) {
      Setpoint += incValue;
    }
    plusMs = now;
  }

  if (minus == LOW && now - minusMs > 250) {
    if (mode == MANUAL_MODE) {
      Output -= incValue;
    } else if (mode == AUTO_MODE) {
      Setpoint -= incValue;
    }
    minusMs = now;
  }

  if (shift == LOW && now - shiftMs > 250) {
    incValue *= 10;
    if (incValue > 100) {
      incValue = 0.01;
    }
    shiftMs = now;
  }


}

void report(void)
{
  static uint32_t last = 0;
  const int interval = 250;
  if (millis() - last > interval) {
    last += interval;
    //    Serial.print(millis()/1000.0);
    Serial.print("SP:"); Serial.print(Setpoint);
    Serial.print(" PV:");
    Serial.print(Input);
    Serial.print(" CV:");
    Serial.print(Output);
    Serial.print(" Int:");
    Serial.print(myPID.outputSum);
    Serial.print(' ');
    for (int i = 0; i < n_ele; ++i) {
      Serial.print("x_i");
      Serial.print(i);
      Serial.print(':');
      Serial.print(x(i), 3);
      Serial.print(' ');
    }

    Serial.println();
  }
}

void reportLCD(void)
{
  static uint32_t last = 0;
  const int interval = 250;
  if (millis() - last > interval) {
    last += interval;
    //    Serial.print(millis()/1000.0);
    // lcd.clear();

    lcd.setCursor(0, 0);
    lcd.print("PV:");
    lcd.print(Input, 3);
    lcd.print(" CV:");
    lcd.print(Output, 2);
    lcd.print(" ");
    lcd.setCursor(0, 1);
    lcd.print("SP:");
    lcd.print(Setpoint, 2);
    lcd.print("  ");
    lcd.print(modes[mode]);
    lcd.print("  ");
    lcd.setCursor(0, 3);
    lcd.print("Int:");
    lcd.print(myPID.outputSum, 4);
    lcd.print("  +");
    lcd.print(incValue, incValue >= 1 ? 0 : incValue >= 0.1 ? 1 : 2);
    lcd.print(" ");
  }
}



float simArm(float Q) { // Energy input in W (or J/s)
  // simulate a pivoting arm with a fan
  // 
  // State-space model, with x(theta,thetaDot)
  //
  //                                  ^fan^ +watts thrust
  //   ---------------------------------- < arm length L, weight W
  //   ^pivot
  //
  // fan: dp *q *u = P // P = watts, dp =deltaPascals, q = m3/sec, u = 0-1 eff 
  // https://www.engineeringtoolbox.com/fans-efficiency-power-consumption-d_197.html
  //     F = (P*r/K)^2/3 K=0.3636 per http://www.starlino.com/power2thrust.html
  //
  // the arm will roll around the pivot, based on fan thrust
  // fan thrust will act to raise the arm
  // gravity will pull the arm back down

  using namespace BLA;

  const int n_force = 2;
  
  // setup for state space model
  // expose x{roll,roll_speed} vector to other routines
  // move these to global if you want other routines to see them:
  // const int n_ele = 5;
  // static BLA::Matrix<n_ele> x; //initial
  static BLA::Matrix<n_force> u; // Forcings
  static BLA::Matrix<n_ele, 2> G;  // map from forcings to elements, dT=f(W)
  static BLA::Matrix<n_ele, n_ele> Fa ; // transition matrix
  static BLA::Matrix<n_ele> err; // noise disturbance 

  float rod_len = 0.40; //m
  float rod_mass = 30 ; // g
  float fan_mass = 50;
  float rod_dia = 1;
  float Iarm = rod_len*rod_len/4*rod_mass+ rod_len*rod_len*fan_mass;
   
  float K = 0.3636; // fan thrust efficiency
  float propRad = 0.10; // prop radius meters 

  static uint32_t last = 0;
  uint32_t interval = 10; // ms
  float dt = interval / 1000.0;
  static bool oneShot = true;
  if (millis() - last >= interval) {
    last += interval;

    // TODO -- populate Fa with dynamics
    // x: angle, angle_dot
    if (oneShot == true) {
      oneShot = false;
      x(0) = 0; // pos
      x(1) = 0; // velocity
      // Fa is dX/dt
      Fa(0,0) = 0; // 
      Fa(0,1) = 1; // +velocity -> +position
      Fa(1,0) = 0;
      Fa(1,1) = 0;
      // G maps inputs to 
      G(0, 0) = .001; // convert force to position
      G(0, 1) = 0; // convert force to velocity 
      G(1, 0) = 0; // convert position to position
      G(1, 1) = 0; // convert  to position

      for (int i = 0; i < n_ele; ++i) {
        u(0) = 0;
        u(n_force - 1) = 0;
        Serial << "Fa:" << Fa << '\n';
        Serial << "G:" << G << '\n';
        Serial.print("Oneshot");
      }
    } // end oneShot initialiation
    
    // w^{..} = (F-mg cos(w))*r/I
    u(0) = 100*pow(abs(Q)*propRad/K,0.667)/Iarm*(Q>0?1.0:-1) - 0*9.81*(rod_mass*rod_len/2 + fan_mass*rod_len)*cos(x(0)) ;  //  
    u(1) = 0;  // opposing/righting force
    // noise
    err(0) = (random(10)-5)/100.0;
    err(1) = 0*(random(10)-5)/100.0;

    // update state x += (dx/dt)*dt
    x += (Fa * x + G * u + err) * dt ;
    if(isnan(x(0))) x(0) = 0;
    if(isnan(x(1))) x(1) = 0;

  }
  return x(0)*180/PI; // angle from dangling down in degrees
}


void  mySetIntegral(QuickPID * ptrPID, float value ) {
  // per
  uint8_t saveMode = ptrPID->GetMode();
  ptrPID->SetMode(myPID.Control::manual);
  Output = value;
  ptrPID->SetMode(saveMode);
}


void sTuneRunner() { // blocking
  float inputSpan = 255, outputSpan = 255;
  float outputStart = Output;
  float outputStep = Output + 2;
  float testTimeSec = 300;
  float settleTimeSec = 10;
  float samples = 300;
  float tempLimit = 255;
  tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples);
  tuner.SetEmergencyStop(tempLimit);
  int armed = true;
  while (armed) {
    float outputSpan = 255;
    int relayPin = PWM_PIN;
    float heaterWatts = Output * 5.0 / 255; // 5W heater

    Input = simArm(heaterWatts); // simulate heating

    // float sTune::softPwm(const uint8_t relayPin, float input, float output, float setpoint, uint32_t windowSize, uint8_t debounce)
    tuner.softPwm(relayPin, Input, Output, 0, outputSpan, 1);
    int junk = tuner.Run();
    switch (junk) { // does the step
      case tuner.sample: // active once per sample during test
        // Serial.print(".");
        // void sTune::plotter(float input, float output, float setpoint, float outputScale, uint8_t everyNth) {

        //tuner.plotter(Input, Output, Setpoint, 1, 3); // ... outScale, every nth
        reportLCD();
        //report();
        break;

      case tuner.tunings: // active just once when sTune is done
        Serial.println("sTune Tunings");
        // Update values from sTun results
        tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune
        myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings
        lcd.setCursor(0, 2);
        lcd.print("Kp:");
        lcd.print(Kp, 2);
        lcd.print(" Ki:");
        lcd.print(Ki, 3);

        Output = 0; // turn off
        tuner.SetTuningMethod(tuner.TuningMethod::DampedOsc_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::NoOvershoot_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::CohenCoon_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::Mixed_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::ZN_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::DampedOsc_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::NoOvershoot_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::CohenCoon_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::Mixed_PI);
        tuner.printTunings();

        //        tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune
        // myPID.SetOutputLimits(0, outputSpan);
        // myPID.SetSampleTimeUs(outputSpan * 1000 - 1);
        // myPID.SetMode(myPID.Control::automatic); // the PID is turned on
        // myPID.SetProportionalMode(myPID.pMode::pOnMeas);
        // myPID.SetAntiWindupMode(myPID.iAwMode::iAwClamp);
        // myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings
        armed = false;
        break;

      case tuner.runPid: // active once per sample after tunings
        Serial.println("sTune Run");
        armed = false; //end function
        break;
      default:
        // Serial.print("default tuner.???");
        ;
    }
  }
}

float rnorm(float mean, float stdev) {
  // https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
  static byte state = 0;
  static float F1,F2;
  switch (state) {
    case 0:
      F1 = sqrt(-2 * log(random()));
      F2 = 2 * PI * random();
      state = 1;
      return mean + stdev * F1 * cos(F2);
      break;
    case 1:
      return mean + stdev * F1 * sin(F2);
      state = 0;
      break;
  }
}