#include <stdio.h>
#include <stdlib.h>

double u_now, u_prev; // u2 and u1

int time;

void setup() {
  Serial.begin(9600);
  double num[3] = {1.0, 2.0, 3.0};  // Example numerator coefficients
  double den[3] = {1.0, 4.0, 5.0};  // Example denominator coefficients
  u_prev = 0;
  u_now = 0;
  RK4Solver Motor;
  RK4Solver PID;
  RK4Solver_init(&Motor, num, den, 0.0, 0.0);  // Initialize solver with coefficients and initial states
  RK4Solver_init(&PID, num, den, 0.0, 0.0);  // Initialize solver with coefficients and initial states

}

void loop() {
  time = millis();
  if(time <= 1000)
    u_now = 1;
  else
    u_now = 0;

  RK4_2nOrder(RK4Solver *solver, double u_prev, double u_now, double t_prev, double t_now)
  u_prev = u_now;
}


typedef struct {
    double *num;  // Numerator coefficients (array of size 3)
    double *den;  // Denominator coefficients (array of size 3)
    double x1n;   // State variable 1
    double x2n;   // State variable 2
} RK4Solver;

// Constructor equivalent function
void RK4Solver_init(RK4Solver *solver, double num[3], double den[3], double x1_init, double x2_init) {
    solver->num = (double*)malloc(3 * sizeof(double));
    solver->den = (double*)malloc(3 * sizeof(double));
    for (int i = 0; i < 3; i++) {
        solver->num[i] = num[i];
        solver->den[i] = den[i];
    }
    solver->x1n = x1_init;
    solver->x2n = x2_init;
}

// RK4_2nOrder function equivalent
double RK4_2nOrder(RK4Solver *solver, double u1, double u2, double t_prev, double t_now) {
    // Extract coefficients
    double b0 = solver->num[0] / solver->den[0];
    double b1 = solver->num[1] / solver->den[0];
    double b2 = solver->num[2] / solver->den[0];
    double a1 = solver->den[1] / solver->den[0];
    double a2 = solver->den[2] / solver->den[0];

    // Calculate step size and input difference
    double DU = (u1 + u2) / 2;
    double h = t_now - t_prev;
    double half_h = h / 2;

    // Current states
    double x1 = solver->x1n;
    double x2 = solver->x2n;

    // Calculate the RK4 terms
    double K1X1 = x2;
    double K1X2 = -a2 * x1 - a1 * x2 + u1;

    solver->x1n = x1 + h/6 * (K1X1 +
        2 * (x2 + K1X2 * half_h) +
        2 * (x2 + (x2 + (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU)) * half_h) +
        (x2 + (-a2 * (x1 + (x2 + K1X2 * half_h) * half_h) - a1 * (x2 + (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU) * half_h) + DU) * h));

    solver->x2n = x2 + h/6 * (K1X2 +
        2 * (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU) +
        2 * (-a2 * (x1 + (x2 + K1X2 * half_h) * half_h) - a1 * (x2 + (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU) * half_h) + DU) +
        (-a2 * (x1 + (x2 + (x2 + (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU)) * half_h) * h) - a1 * (x2 + (-a2 * (x1 + (x2 + K1X2 * half_h) * half_h) - a1 * (x2 + (-a2 * (x1 + K1X1 * half_h) - a1 * (x2 + K1X2 * half_h) + DU) * half_h) + DU) * h) + u2));

    // Output equation
    double y = (b2 - a2 * b0) * solver->x1n + (b1 - a1 * b0) * solver->x2n + b0 * u2;
    return y;
}