#include "Igrf13Model.h"

Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {}

void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
                               const double altitude, timeval timeOfMagMeasurement,
                               double* magFieldModelInertial) {
  double magFieldModel[3] = {0, 0, 0};
  double phi = longitude, theta = gcLatitude;  // geocentric
  /* Here is the co-latitude needed*/
  theta -= 90. * M_PI / 180.;
  theta *= (-1);

  double rE = 6371200.0;  // radius earth [m]
  /* Predefine recursive associated Legendre polynomials */
  double P11 = 1;
  double P10 = P11;    // P10 = P(n-1,m-0)
  double dP11 = 0;     // derivative
  double dP10 = dP11;  // derivative

  double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;

  for (int m = 0; m <= igrfOrder; m++) {
    for (int n = 1; n <= igrfOrder; n++) {
      if (m <= n) {
        /* Calculation of Legendre Polynoms (normalised) */
        if (n == m) {
          P2 = sin(theta) * P11;
          dP2 = sin(theta) * dP11 + cos(theta) * P11;
          P11 = P2;
          P10 = P11;
          P20 = 0;
          dP11 = dP2;
          dP10 = dP11;
          dP20 = 0;
        } else if (n == 1) {
          P2 = cos(theta) * P10;
          dP2 = cos(theta) * dP10 - sin(theta) * P10;
          P20 = P10;
          P10 = P2;
          dP20 = dP10;
          dP10 = dP2;
        } else {
          K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3));
          P2 = cos(theta) * P10 - K * P20;
          dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
          P20 = P10;
          P10 = P2;
          dP20 = dP10;
          dP10 = dP2;
        }
        /* gradient of scalar potential towards radius */
        magFieldModel[0] +=
            pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
            ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
        /* gradient of scalar potential towards theta */
        magFieldModel[1] +=
            pow(rE / (altitude + rE), (n + 2)) *
            ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
        /* gradient of scalar potential towards phi */
        magFieldModel[2] +=
            pow(rE / (altitude + rE), (n + 2)) *
            ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
      }
    }
  }

  magFieldModel[1] *= -1;
  magFieldModel[2] *= (-1 / sin(theta));

  double JD2000 = 0;
  Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
  double UT1 = JD2000 / 36525.;

  double gst =
      280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
  gst = std::fmod(gst, 360.);
  gst *= M_PI / 180.;
  double lst = gst + longitude;  // local sidereal time [rad]

  magFieldModelInertial[0] =
      (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
      magFieldModel[2] * sin(lst);
  magFieldModelInertial[1] =
      (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
      magFieldModel[2] * cos(lst);
  magFieldModelInertial[2] =
      magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);

  // convert nT to uT
  VectorOperations<double>::mulScalar(magFieldModelInertial, 1e-3, magFieldModelInertial, 3);
}

void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
  double JD2000Igrf = (2458850.0 - 2451545);  // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
  double JD2000 = 0;
  Clock::convertTimevalToJD2000(timeOfMagMeasurement, &JD2000);
  double days = ceil(JD2000 - JD2000Igrf);
  for (int i = 0; i <= igrfOrder; i++) {
    for (int j = 0; j <= (igrfOrder - 1); j++) {
      updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365);
      updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365);
    }
  }
}

void Igrf13Model::schmidtNormalization() {
  double kronDelta = 0;
  schmidtFactors[0][0] = 1;
  for (int n = 1; n <= igrfOrder; n++) {
    if (n == 1) {
      schmidtFactors[0][n - 1] = 1;
    } else {
      schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
    }
    for (int m = 1; m <= igrfOrder; m++) {
      if (m == 1) {
        kronDelta = 1;
      } else {
        kronDelta = 0;
      }
      schmidtFactors[m][n - 1] =
          schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
    }
  }

  for (int i = 0; i <= igrfOrder; i++) {
    for (int j = 0; j <= (igrfOrder - 1); j++) {
      coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
      coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];

      svG[i][j] = schmidtFactors[i][j] * svG[i][j];
      svH[i][j] = schmidtFactors[i][j] * svH[i][j];
    }
  }
}