import 'dart:math';

/// ========== RK4 for BASHFORTH-ADAMS FORMULATION  ==========
double bashforthAdamsRK4(
  List<List<double>> afRK4Table,
  double fApexCurvature,
  double fShapeFactor,
  double fSign,
) {
  double A = fApexCurvature;
  double A2 = 2.0 / A;
  double B = fShapeFactor;
  // double KX0 = 1.0;
  // double KT0 = 1.0;
  double inv6 = 1.0 / 6.0;
  double fTotalDiffSquared = 0.0;
  int n = 1;

  while (n < afRK4Table.length) {
    double x = afRK4Table[n - 1][3]; // predicted x
    double y = afRK4Table[n - 1][1]; // actual y
    double t = afRK4Table[n - 1][4]; // predicted angle
    double dy = afRK4Table[n][2]; // actual y diff
    double AG = 1.0; // AG: Added
    double ydy = y + 0.5 * dy;
    double KX1 = dy * (1.0 / tan(t));
    double KT1 =
        dy * (1.0 / sin(t)) * (A2 - (B * y) - (sin(t) / (AG * x))); // A -> AG
    double KX2 = dy * (1.0 / tan(t + 0.5 * KT1));
    double STKT1 = sin(t + 0.5 * KT1);
    double KT2 =
        dy *
        (1.0 / STKT1) *
        (A2 - (B * ydy) - (STKT1 / (AG * (x + 0.5 * KX1)))); // A -> AG
    double KX3 = dy * (1.0 / tan(t + 0.5 * KT2));
    double STKT2 = sin(t + 0.5 * KT2);
    double KT3 =
        dy *
        (1.0 / STKT2) *
        (A2 - (B * ydy) - (STKT2 / (AG * (x + 0.5 * KX2)))); // A -> AG
    double KX4 = dy * (1.0 / tan(t + KT3));
    double STKT3 = sin(t + KT3);
    double KT4 =
        dy *
        (1.0 / STKT3) *
        (A2 - (B * (y + dy)) - (STKT3 / (AG * (x + KX3)))); // A -> AG
    double KX = (KX1 + 2.0 * (KX2 + KX3) + KX4) * inv6;
    double KT = (KT1 + 2.0 * (KT2 + KT3) + KT4) * inv6;

    afRK4Table[n][3] = x + KX;
    afRK4Table[n][4] = t + KT;
    fTotalDiffSquared += pow(afRK4Table[n][0] - (fSign * afRK4Table[n][3]), 2);
    n += 1;
  }

  return fTotalDiffSquared;
}

List<List<double>> makeRK4Table(List<double> edgePoints) {
  List<double> initialValues = List.filled(13, 0.0);

  List<List<double>> rk4Table = [];
  rk4Table.add(initialValues);
  double yPrev = 0.0;

  for (var point in edgePoints) {
    List<double> entryValues = List.filled(13, 0.0);
    double x = point[0];
    double y = point[1];
    entryValues[0] = x;
    entryValues[1] = y;
    entryValues[2] = y - yPrev;
    yPrev = y;
    rk4Table.add(entryValues);
  }

  return rk4Table;
}