#include "geodesic.h"
#include "observer_track.h"

#include <math.h>
#include <stdio.h>
#include <string.h>

#define CHECK(condition) do { if (!(condition)) { \
  fprintf(stderr, "observer regression failed at line %d: %s\n", __LINE__, #condition); \
  return 1; } } while (0)

/* Independent covariant four-metric contraction in extended precision. */
static long double dot(const MetricData *m, const double a[4], const double b[4]) {
  long double g[4][4] = {{0}};
  g[0][0] = -(long double)m->alpha * m->alpha;
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j) {
      g[i + 1][j + 1] = m->gamma[i][j];
      g[0][0] += (long double)m->gamma[i][j] * m->beta[i] * m->beta[j];
      g[0][i + 1] += (long double)m->gamma[i][j] * m->beta[j];
      g[i + 1][0] = g[0][i + 1];
    }
  long double result = 0;
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j) result += g[i][j] * a[i] * b[j];
  return result;
}

static int check_state(const MetricData *m, const ObserverCamera *c,
                       const ObserverState *o) {
  CHECK(o->coordinate_time == c->coordinate_time);
  CHECK(o->tetrad[0][0] > 0);
  for (int i = 0; i < 3; ++i) {
    CHECK(o->coordinate_position[i] == c->position[i]);
    CHECK(fabs(o->tetrad[0][i + 1] / o->tetrad[0][0] - c->velocity[i]) < 1e-12);
  }
  for (int a = 0; a < 4; ++a)
    for (int b = 0; b < 4; ++b)
      CHECK(fabsl(dot(m, o->tetrad[a], o->tetrad[b]) -
                  (a == b ? (a == 0 ? -1 : 1) : 0)) < 1e-11L);
  const double n[3] = {0.36, 0.48, 0.8};
  double k[4];
  for (int mu = 0; mu < 4; ++mu) {
    k[mu] = o->tetrad[0][mu];
    for (int a = 0; a < 3; ++a) k[mu] -= n[a] * o->tetrad[a + 1][mu];
  }
  CHECK(k[0] > 0 && fabsl(dot(m, k, k)) < 1e-11L);
  CHECK(fabsl(dot(m, k, o->tetrad[0]) + 1) < 1e-11L);
  return 0;
}

int main(int argc, char **argv) {
  SpacetimeSource source = {0};
  CHECK(spacetime_create_default(&source) == 0);
  ObserverCamera camera = {.position = {3, -4, 5}, .velocity = {0.2, -0.1, 0.3},
                          .look_ra_deg = 37, .look_dec_deg = -23, .roll_deg = 19};
  MetricData metric;
  ObserverState state;
  CHECK(spacetime_eval(&source, 0, camera.position, &metric) == 0);
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
  CHECK(check_state(&metric, &camera, &state) == 0);
  if (argc == 2) {
    ObserverSample sample = {.coordinate_time = 0, .proper_time = 0};
    memcpy(sample.coordinate_position, state.coordinate_position, sizeof sample.coordinate_position);
    memcpy(sample.tetrad, state.tetrad, sizeof sample.tetrad);
    ObserverTrack track = {.samples = &sample, .count = 1};
    CHECK(observer_track_write_csv(&track, argv[1]) == 0);
  }
  /* A rescaled/shifted coordinate system can have timelike |dx/dt| > 1.
   * The builder must use the metric, not impose a Euclidean speed limit. */
  const MetricData shifted_metric = {.alpha = 2, .beta = {0.1, -0.2, 0.3},
      .gamma = {{1, 0.1, 0}, {0.1, 1.2, 0.1}, {0, 0.1, 0.9}}};
  const ObserverCamera fast_coordinate = {.velocity = {1.2, 0, 0},
      .look_ra_deg = 123, .look_dec_deg = 45, .roll_deg = -31};
  CHECK(observer_from_coordinate_camera(&shifted_metric, &fast_coordinate,
                                        &state, NULL) == OBSERVER_BUILD_OK);
  CHECK(check_state(&shifted_metric, &fast_coordinate, &state) == 0);
  MetricData rounded_metric = shifted_metric;
  rounded_metric.gamma[1][0] = nextafter(rounded_metric.gamma[1][0], INFINITY);
  CHECK(observer_from_coordinate_camera(&rounded_metric, &fast_coordinate,
                                        &state, NULL) == OBSERVER_BUILD_OK);
  CHECK(check_state(&rounded_metric, &fast_coordinate, &state) == 0);
  MetricData invalid_metric = shifted_metric;
  invalid_metric.gamma[2][2] = -1;
  CHECK(observer_from_coordinate_camera(&invalid_metric, &fast_coordinate,
                                        &state, NULL) == OBSERVER_BUILD_INVALID_INPUT);
  const double ras[] = {0, 37, 90, 180, 359.9};
  const double decs[] = {-90, -23, 0, 45, 90};
  for (int a = 0; a < 5; ++a)
    for (int b = 0; b < 5; ++b) {
      camera.look_ra_deg = ras[a]; camera.look_dec_deg = decs[b];
      CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
      CHECK(check_state(&metric, &camera, &state) == 0);
    }
  camera.look_ra_deg = 0; camera.look_dec_deg = 0; camera.roll_deg = 0;
  ObserverState unrolled;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &unrolled, NULL) == OBSERVER_BUILD_OK);
  camera.roll_deg = 90;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
  for (int mu = 0; mu < 4; ++mu) {
    CHECK(fabs(state.tetrad[2][mu] - unrolled.tetrad[3][mu]) < 1e-12);
    CHECK(fabs(state.tetrad[3][mu] + unrolled.tetrad[2][mu]) < 1e-12);
  }
  camera.velocity[0] = NAN;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_INVALID_INPUT);
  camera.velocity[0] = 10;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_NON_TIMELIKE);
  camera.velocity[0] = 0;
  camera.roll_deg = INFINITY;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_INVALID_INPUT);

#ifdef SPACETIME_SCHWARZSCHILD
  /* Ingoing radial light seen from the horizon and its interior must still
   * trace backwards to the external sky, rather than be classified captured. */
  const GeodesicTraceConfig trace = {.coordinate_time_step = 0.05,
      .max_steps = 8192, .threshold = {.kind = THRESHOLD_LOG_ENERGY_GROWTH, .value = 8.0, .policy_version = 3}};
  for (int i = 0; i < 3; ++i) {
    camera = (ObserverCamera){.position = {2.25 - 0.25 * i, 0, 0},
                              .velocity = {-0.5, 0, 0}};
    CHECK(spacetime_eval(&source, 0, camera.position, &metric) == 0);
    CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
    CHECK(check_state(&metric, &camera, &state) == 0);
    const RayEndpoint ray = geodesic_trace_past(&source, &state, (double[]){1, 0, 0}, &trace);
    CHECK(ray.outcome == RAY_OUTCOME_ESCAPED);
    CHECK(fabs(ray.n_infinity[0] - 1) < 1e-12);
    /* Radial ingoing KS photon has k^r=-k^t and conserved E=k^t.  The
     * asymptotic exterior transfers the photon to infinity, where
     * g = E_camera / E_infinity = 1 / k^t. */
    const double energy = state.tetrad[0][0] - state.tetrad[1][0];
    CHECK(fabs(ray.frequency_ratio - 1.0 / energy) < 1e-10 * (1.0 / energy));
    memset(camera.velocity, 0, sizeof camera.velocity);
    if (i > 0)
      CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_NON_TIMELIKE);
  }
#else
  /* For transverse velocity along Y, projected +X remains F=(0,1,0,0).
   * Independently, k=(gamma,-1,gamma*v,0) gives aberration and Doppler. */
  camera = (ObserverCamera){.velocity = {0, 0.6, 0}};
  CHECK(spacetime_eval(&source, 0, camera.position, &metric) == 0);
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
  const GeodesicTraceConfig trace = {.coordinate_time_step = 1, .max_steps = 2048};
  const RayEndpoint ray = geodesic_trace_past(&source, &state, (double[]){1, 0, 0}, &trace);
  CHECK(ray.outcome == RAY_OUTCOME_ESCAPED);
  CHECK(fabs(ray.n_infinity[0] - 0.8) < 1e-12);
  CHECK(fabs(ray.n_infinity[1] + 0.6) < 1e-12);
  CHECK(fabs(ray.frequency_ratio - 0.8) < 1e-12);
  camera.position[0] = 25; camera.position[1] = -30; camera.position[2] = 10;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_OK);
  const RayEndpoint shifted = geodesic_trace_past(&source, &state, (double[]){1, 0, 0}, &trace);
  CHECK(shifted.outcome == ray.outcome && fabs(shifted.frequency_ratio - ray.frequency_ratio) < 1e-12);
  for (int i = 0; i < 3; ++i) CHECK(fabs(shifted.n_infinity[i] - ray.n_infinity[i]) < 1e-12);
  camera.velocity[1] = 1;
  CHECK(observer_from_coordinate_camera(&metric, &camera, &state, NULL) == OBSERVER_BUILD_NON_TIMELIKE);
#endif
  spacetime_destroy(&source);
  puts("coordinate-camera regression passed");
  return 0;
}