// libmagdev.c
// (C) 2009 Philip Endecott
// See http://chezphil.org/libmagdev/
//
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE.

#include "libmagdev.h"

#include <math.h>


// Include the model parameters that are extracted from the data file by an awk script:
struct model {
  float epoch;
  int max1;        // This is the order of the model.
  int max2;
  int max3;
  float yrmin;
  float yrmax;
  float altmin;
  float altmax;
  float* params;  // There are max1*(max1+2) parameters.  It looks lik a diagonal matrix?
  float* diffs;   // These are differential terms.  They are only present for the last model.
};
#include "params.c"
#include "diffs.c"
#include "models.c"

int n_models = sizeof(models)/sizeof(models[0]);


static const struct model* find_model_for_year(float yr)
{
  // The models are in ascending order, so we find the first one whose end date
  // is after the required date.  If the required date is before the first model
  // we return NULL.
  if (models[0].yrmin > yr) {
    return NULL;
  }
  for (int m=0; m<n_models; ++m) {
    struct model* mod = &models[m];
    if (mod->yrmax > yr) {
      return mod;
    }
  }
  // If we're beyond the end of the last model we just use the last model.
  return &models[n_models-1];
}


static void interpolate_models(struct model* interp_mod,
                               const struct model* mod1, const struct model* mod2,
                               float yr)
{
  // Given two models mod1 and mod2, compute an intermediate model for the
  // specified date.  We do this simply by linearly interpolating each parameter.
  // The two models may not have the same number of parameters.  Make the resulting
  // model as good as the best input, using zeros for the other parameters.

  interp_mod->max1 = (mod1->max1 > mod2->max1) ? mod1->max1 : mod2->max1;

  float factor = (yr - mod1->yrmin) / (mod2->yrmin - mod1->yrmin);

  int mod1_nparams = mod1->max1 * (mod1->max1+2);
  int mod2_nparams = mod2->max1 * (mod2->max1+2);
  int min_nparams = (mod1_nparams<mod2_nparams) ? mod1_nparams : mod2_nparams;

  int i=0;
  for (; i<min_nparams; ++i) {
    interp_mod->params[i] = mod1->params[i] + factor * (mod2->params[i] - mod1->params[i]);
  }
  for (; i<mod1_nparams; ++i) {
    interp_mod->params[i] = (1-factor) * mod1->params[i];
  }
  for (; i<mod2_nparams; ++i) {
    interp_mod->params[i] = factor * mod2->params[i];
  }
}


static void extrapolate_model(struct model* extrap_mod, const struct model* mod, float yr)
{
  // Given a model mod, compute an extrapolated model for the specified date.
  // We do this simply by linearly extrapolating each parameter using the differential terms.
  // We rely on there being zero terms beyond the end of the useful terms.

  extrap_mod->max1 = mod->max1;

  float factor = yr - mod->yrmin;

  int nparams = mod->max1 * (mod->max1+2);
  for (int i=0; i<nparams; ++i) {
    extrap_mod->params[i] = mod->params[i] + factor * mod->diffs[i];
  }
}


static float degtorad(float deg) {
  return deg/360 * 2*M_PI;
}

static float radtodeg(float rad) {
  return rad / (2*M_PI) * 360;
}


static float max(float a, float b)
{
  return (a>b) ? a : b;
}

static float min(float a, float b)
{
  return (a<b) ? a : b;
}


static void compute_field_components(const struct model* mod,
                                     float lng, float lat, float alt_km,
                                     float* x, float* y, float* z)
{
  // Given a spherical harmonic model and a position, compute the field components
  // at that position.

  float slat = sin(degtorad(lat));
  float lat_sat = max(min(lat,89.999),-89.999);
  float clat = cos(degtorad(lat_sat));

  float sl[14];
  float cl[14];
  sl[1] = sin(degtorad(lng));
  cl[1] = cos(degtorad(lng));

  // Convert from geodetic to geocentric coordinates:
  const float a2 = 40680631.59;
  const float b2 = 40408299.98;
  float aa = a2 * clat * clat;
  float bb = b2 * slat * slat;
  float cc = aa + bb;
  float dd = sqrt(cc);
  float r = sqrt(alt_km*(alt_km+2*dd) + (a2*aa+b2*bb)/cc);
  float cd = (alt_km+dd)/r;
  float sd = (a2-b2)/dd * slat * clat/r;
  float slat_1 = slat*cd - clat*sd;
  float clat_1 = clat*cd + slat*sd;

  const float earths_radius = 6371.2;
  float ratio = earths_radius / r;
  float p[119];
  float q[119];
  p[1] = 2 * slat_1;
  p[2] = 2 * clat_1;
  p[3] = 4.5 * slat_1 * slat_1 - 1.5;
  p[4] = 3 * sqrt(3) * clat_1 * slat_1;
  q[1] = -clat_1;
  q[2] = slat_1;
  q[3] = -3 * clat_1 * slat_1;
  q[4] = sqrt(3) * (slat_1*slat_1 - clat_1*clat_1);

  *x = *y = *z = 0;
  int l = 0;
  int n = 0;
  int m = 1;
  int fn = 0;
  int fm = 0;
  float rr = 0;
  int npq = mod->max1 * (mod->max1+3) / 2;
  for (int k=1; k<npq; ++k) {
    if (n<m) {
      m = 0;
      ++n;
      rr = pow(ratio,n+2);
      fn = n;
    }
    fm = m;
    if (k>=5) {
      if (m==n) {
        float a = sqrt(1-0.5/fm);
        int j = k-n-1;
        p[k] = (1.0 + 1.0/fm) * a * clat_1 * p[j];
        q[k] = a * (clat_1 * q[j] + slat_1/fm * p[j]);
        sl[m] = sl[m-1] * cl[1] + cl[m-1] * sl[1];
        cl[m] = cl[m-1] * cl[1] - sl[m-1] * sl[1];
      } else {
        float a = sqrt(fn*fn - fm*fm);
        float b = sqrt(((fn-1.0)*(fn-1.0)) - (fm*fm))/a;
        float c = (2.0*fn-1)/a;
        int ii = k-n;
        int j = k - 2*n + 1;
        p[k] = (fn+1.0) * (c*slat_1/fn*p[ii] - b/(fn-1.0)*p[j]);
        q[k] = c * (slat_1*q[ii] - clat_1/fn*p[ii]) - b*q[j];
      }
    }
    float a = rr * mod->params[l];
    if (m==0) {
      (*x) += a*q[k];
      (*z) -= a*p[k];
      ++l;
    } else {
      float b = rr * mod->params[l+1];
      float c = a*cl[m] + b*sl[m];
      (*x) += c*q[k];
      (*z) -= c*p[k];
      if (clat_1>0) {
        (*y) += (a*sl[m] - b*cl[m]) * fm * p[k]/((fn+1)*clat_1);
      } else {
        (*y) += (a*sl[m] - b*cl[m]) * q[k] * slat_1;
      }
      l += 2;
    }
    ++m;
  }
  float old_x = (*x);
  (*x) = old_x*cd + (*z)*sd;
  (*z) = (*z)*cd - old_x*sd;
}


static void compute_mag_values(float x, float y, float z,
                               float* decl, float* incl, float* hintensity, float* tintensity)
{
  // Given the X-Y-Z magnetic field components, compute the
  // declination, inclination and horizontal and total intensity.

  float h2 = x*x + y*y;
  *hintensity = sqrt(h2);
  *tintensity = sqrt(h2 + z*z);
  if (*tintensity < 0.0001) {
    *decl = *incl = 0;
  } else {
    *incl = radtodeg(atan2(z,*hintensity));
    if (*hintensity < 0.0001) {
      *decl = 0;
    } else {
      float hpx = *hintensity + x;
      if (hpx < 0.0001) {
        *decl = 180;
      } else {
        *decl = radtodeg(2 * atan2(y,hpx));
      }
    }
  }
}


// Given a time_t, latitude, longditude (both in degrees with E and N positive) 
// and altitude (above sea level in metres), compute the deviation from true
// N to magnetic N (in degrees, WHICH WAY?).

float magdev(time_t t, float lng, float lat, float alt)
{
  // Compute decimal year from time_t:
  float yr = t/(60*60*24*365.25) + 1970;

  const struct model* mod = find_model_for_year(yr);
  if (!mod) {
    // If we don't have a model for the requested date, i.e. it is before the
    // first model's start time, we return nan.
    return NAN;
  }

  // If this model is the last one we extrapolate to the required date; the
  // model includes extra parameters for this purpose.
  // Otherwise we interpolate between this model and the next model to get
  // a model for the required date.
  float interp_params[200];  // large enough for an order=13 model.
  struct model interp_mod;
  interp_mod.params = interp_params;
  if (mod->diffs) {
    extrapolate_model(&interp_mod, mod, yr);
  } else {
    interpolate_models(&interp_mod, mod, mod+1, yr);
  }

  // The code uses km, not m, for altitude.
  float alt_km = alt/1000;

  // Compute the x, y and z vector components of the magnetic field:
  float x, y, z;
  compute_field_components(&interp_mod, lng,lat,alt_km, &x,&y,&z);

  // Convert that to angles:
  float declination, inclination, hintensity, vintensity;
  compute_mag_values(x,y,z, &declination,&inclination,&hintensity,&vintensity);

  return declination;
}