#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <wcslib/cel.h>
#include <cpgplot.h>
#include "qfits.h"
#include "sgdp4h.h"
#include <giza.h>

#define LIM 80
#define MMAX 10
#define D2R M_PI/180.0
#define R2D 180.0/M_PI
#define XKMPER 6378.135		// Earth radius in km
#define XKMPAU 149597879.691	// AU in km
#define FLAT (1.0/298.257)
#define STDMAG 6.0

long Isat = 0;
long Isatsel = 0;
extern double SGDP4_jd0;

struct map
{
  double lat, lng;
  float alt;
  char observer[32];
  int site_id;
} m;
struct point
{
  double mjd;
  xyz_t obspos, sunpos;
  double zeta, z, theta;
} p[MMAX];
struct image
{
  char filename[64];
  int naxis, naxis1, naxis2, nframes;
  float *zavg, *zstd, *zmax, *znum;
  double ra0, de0;
  float x0, y0;
  float a[3], b[3], xrms, yrms;
  double mjd;
  float *dt, exptime;
  char nfd[32];
  int cospar, tracked;
};
struct image read_fits (char *filename);
void get_site (int site_id);
double modulo (double, double);
void obspos_xyz (double, xyz_t *, xyz_t *);
void sunpos_xyz (double, xyz_t *);
double gmst (double);
double dgmst (double);
void precession_angles (double mjd0, double mjd, double *zeta, double *z,
			double *theta);
void initialize (struct image img);
void forward (double ra0, double de0, double ra, double de, double *x,
	      double *y);
void reverse (double ra0, double de0, double x, double y, double *ra,
	      double *de);
struct sat apparent_position (double mjd);
int giza_open_device_size_float (const char *, const char *, float, float,
				 int);

void
plot_satellites (char *tlefile, struct image img, long satno, double mjd0,
		 float dt, int color)
{
  int i;
  orbit_t orb;
  int imode, flag, textflag, sflag;
  FILE *fp = NULL, *file;
  xyz_t satpos, satvel;
  float x, y, x0, y0;
  char norad[7], satname[30], state[16];
  float isch;
  char filename[128];
  double dx, dy, dz, r, ra, de, d, rsun, rearth;
  double psun, pearth, ptot;
  double a, b, c;
  double rx, ry;

  giza_get_character_height_float (&isch);

  // Image determinant
  d = img.a[1] * img.b[2] - img.a[2] * img.b[1];

  // Open TLE file
  fp = fopen (tlefile, "rb");
  if (fp == NULL)
    return;

  giza_set_colour_index (color);

  // Open file
  sprintf (filename, "%s.id", img.filename);
  file = fopen (filename, "a");

  // Read TLEs
  while (read_twoline (fp, satno, &orb) == 0)
    {
      Isat = orb.satno;
      imode = init_sgdp4 (&orb);

      sprintf (norad, " %05ld", Isat);

      if (imode == SGDP4_ERROR)
	continue;

      // Loop over times
      for (flag = 0, textflag = 0, sflag = 0, i = 0; i < MMAX; i++)
	{
	  // Satellite position
	  satpos_xyz (p[i].mjd + 2400000.5, &satpos, &satvel);

	  // Check on radius
	  r =
	    sqrt (satpos.x * satpos.x + satpos.y * satpos.y +
		  satpos.z * satpos.z);
	  if (r > 300000)
	    continue;

	  // Relative to observer
	  dx = satpos.x - p[i].obspos.x;
	  dy = satpos.y - p[i].obspos.y;
	  dz = satpos.z - p[i].obspos.z;

	  // Celestial position
	  r = sqrt (dx * dx + dy * dy + dz * dz);
	  ra = modulo (atan2 (dy, dx), 2.0 * M_PI);
	  de = asin (dz / r);


	  // Correct for precession
	  a = cos (de) * sin (ra + p[i].zeta);
	  b =
	    cos (p[i].theta) * cos (de) * cos (ra + p[i].zeta) -
	    sin (p[i].theta) * sin (de);
	  c =
	    sin (p[i].theta) * cos (de) * cos (ra + p[i].zeta) +
	    cos (p[i].theta) * sin (de);
	  ra = modulo ((atan2 (a, b) + p[i].z) * R2D, 360.0);
	  de = asin (c) * R2D;

	  // Adjust for stationary camera
	  if (img.tracked == 0)
	    ra +=
	      gmst (img.mjd + 0.5 * img.exptime / 86400.0) - gmst (p[i].mjd);

	  // Check if nearby enough
	  r =
	    acos (sin (img.de0 * D2R) * sin (de * D2R) +
		  cos (img.de0 * D2R) * cos (de * D2R) * cos ((img.ra0 - ra) *
							      D2R)) * R2D;
	  if (r < 90.0)
	    forward (img.ra0, img.de0, ra, de, &rx, &ry);
	  else
	    continue;

	  // Satellite position relative to the Sun
	  dx = -satpos.x + p[i].sunpos.x;
	  dy = -satpos.y + p[i].sunpos.y;
	  dz = -satpos.z + p[i].sunpos.z;

	  // Distances
	  rsun = sqrt (dx * dx + dy * dy + dz * dz);
	  rearth =
	    sqrt (satpos.x * satpos.x + satpos.y * satpos.y +
		  satpos.z * satpos.z);

	  // Angles
	  psun = asin (696.0e3 / rsun) * R2D;
	  pearth = asin (6378.135 / rearth) * R2D;
	  ptot =
	    acos ((-dx * satpos.x - dy * satpos.y -
		   dz * satpos.z) / (rsun * rearth)) * R2D;

	  // Visibility state
	  if (ptot - pearth < -psun)
	    {
	      giza_set_line_style (4);
	      strcpy (state, "eclipsed");
	    }
	  else if (ptot - pearth > -psun && ptot - pearth < psun)
	    {
	      giza_set_line_style (2);
	      strcpy (state, "umbra");
	      sflag = 1;
	    }
	  else if (ptot - pearth > psun)
	    {
	      giza_set_line_style (1);
	      strcpy (state, "sunlit");
	      sflag = 1;
	    }

	  // Convert image position
	  dx = rx - img.a[0];
	  dy = ry - img.b[0];
	  x = (img.b[2] * dx - img.a[2] * dy) / d + img.x0;
	  y = (img.a[1] * dy - img.b[1] * dx) / d + img.y0;

	  // Print name if in viewport
	  if (x > 0.0 && x < img.naxis1 && y > 0.0 && y < img.naxis2
	      && textflag == 0)
	    {
	      if (flag != 0)
		{
		  giza_draw_float (x, y);
		  giza_move_float (x, y);
		}
	      giza_set_character_height_float (0.65);
	      giza_set_colour_index (1);	// Satellite names text color
	      giza_text_float (x, y, norad);
	      giza_set_character_height_float (isch);
	      giza_move_float (x, y);
	      textflag = 1;
	    }
	  if (i == 0)
	    {
	      x0 = x;
	      y0 = y;
	    }

	  // Plot satellites
	  if (flag == 0)
	    {
	      giza_single_point_float (x, y, 17);
	      giza_move_float (x, y);
	      flag = 1;
	    }
	  else
	    {
	      giza_draw_float (x, y);
	      giza_move_float (x, y);
	    }
	}
      if (textflag == 1)
	{
	  if (sflag == 0)
	    fprintf (file,
		     "%.23s %8.3f %8.3f %8.3f %8.3f %8.5f %s %s eclipsed\n",
		     img.nfd + 1, x0, y0, x, y, img.exptime, norad, tlefile);
	  else
	    fprintf (file,
		     "%.23s %8.3f %8.3f %8.3f %8.3f %8.5f %s %s sunlit\n",
		     img.nfd + 1, x0, y0, x, y, img.exptime, norad, tlefile);
	}
    }
  fclose (fp);
  fclose (file);
  giza_set_colour_index (1);

  return;
}


int
main (int argc, char *argv[])
{
  int i;
  struct image img;
  float zmin, zmax, zavg, zstd;
  float tr[] = { -0.5, 1.0, 0.0, -0.5, 0.0, 1.0 };
  float heat_l[] = { 0.0, 0.2, 0.4, 0.6, 1.0 };
  float heat_r[] = { 0.0, 0.5, 1.0, 1.0, 1.0 };
  float heat_g[] = { 0.0, 0.0, 0.5, 1.0, 1.0 };
  float heat_b[] = { 0.0, 0.0, 0.0, 0.3, 1.0 };
  char text[128];
  char *env, filename[128];
  float sx, sy, wx, wy;

  // Read fits file
  img = read_fits (argv[1]);

  // Set site
  get_site (img.cospar);

  // Initialize
  initialize (img);

  // Fill buffer
  if (img.naxis == 3)
    {
      for (i = 0, zavg = 0.0; i < img.naxis1 * img.naxis2; i++)
	zavg += img.zmax[i];
      zavg /= (float) img.naxis1 * img.naxis2;
      for (i = 0, zstd = 0.0; i < img.naxis1 * img.naxis2; i++)
	zstd += pow (img.zmax[i] - zavg, 2);
      zstd = sqrt (zstd / (float) (img.naxis1 * img.naxis2));
      zmin = zavg - 2 * zstd;
      zmax = zavg + 6 * zstd;
    }
  else
    {
      for (i = 0, zavg = 0.0; i < img.naxis1 * img.naxis2; i++)
	zavg += img.zavg[i];
      zavg /= (float) img.naxis1 * img.naxis2;
      for (i = 0, zstd = 0.0; i < img.naxis1 * img.naxis2; i++)
	zstd += pow (img.zavg[i] - zavg, 2);
      zstd = sqrt (zstd / (float) (img.naxis1 * img.naxis2));
      zmin = zavg - 2 * zstd;
      zmax = zavg + 6 * zstd;
    }

  // Sizes
  sx = sqrt (img.a[1] * img.a[1] + img.b[1] * img.b[1]);
  sy = sqrt (img.a[2] * img.a[2] + img.b[2] * img.b[2]);
  wx = img.naxis1 * sx / 3600.0;
  wy = img.naxis2 * sy / 3600.0;

  if (argc == 3)
    {
      giza_open_device_size_float ("/png", "satid", 680, 680, 3);
      giza_set_colour_palette (1);
      giza_set_font ("Helvetica");
      giza_set_colour_representation_float (0, 0.0, 0.0, 0.0);
      giza_draw_background ();
    }
  else
    {
      giza_open_device_size_float ("/xs", "satid", 680, 680, 3);
      giza_set_colour_palette (1);
      giza_set_font ("Helvetica");
      giza_set_colour_representation_float (0, 0.0, 0.0, 0.0);
      giza_draw_background ();
    }
  giza_set_viewport_float (0.1, 0.95, 0.1, 0.8);

  giza_set_character_height_float (0.8);
  giza_set_colour_index (255);	// White top date/COSPAR ID text
  sprintf (text, "UT Date: %.23s  COSPAR ID: %04d", img.nfd + 1, img.cospar);
  giza_annotate_float ("T", 6.0, 0.0, 0.0, text);
  sprintf (text, "R.A.: %10.5f (%4.1f'') Decl.: %10.5f (%4.1f'')", img.ra0,
	   img.xrms, img.de0, img.yrms);
  if (img.xrms < 1e-3 || img.yrms < 1e-3 || img.xrms / sx > 2.0
      || img.yrms / sy > 2.0)
    giza_set_colour_index (2);
  else
    giza_set_colour_index (255);	// R.A. text white
  giza_annotate_float ("T", 4.8, 0.0, 0.0, text);
  giza_set_colour_index (255);	// White main text


  sprintf (text,
	   "FoV: %.2f\\(2218)x%.2f\\(2218) Scale: %.2f''x%.2f'' pix\\u-1\\d",
	   wx, wy, sx, sy);
  giza_annotate_float ("T", 3.6, 0.0, 0.0, text);
  sprintf (text, "Stat: %5.1f+-%.1f (%.1f-%.1f)", zavg, zstd, zmin, zmax);
  giza_annotate_float ("T", 2.4, 0.0, 0.0, text);

  giza_set_character_height_float (1.0);
  giza_set_window_equal_scale_float (0.0, img.naxis1, 0.0, img.naxis2);

  giza_label ("x (pix)", "y (pix)", " ");
  giza_set_colour_table_float (heat_l, heat_r, heat_g, heat_b, 5, 1.0, 0.5);

  if (img.naxis == 3)
    cpgimag (img.zmax, img.naxis1, img.naxis2, 1, img.naxis1, 1, img.naxis2,
	     zmin, zmax, tr);
  else
    cpgimag (img.zavg, img.naxis1, img.naxis2, 1, img.naxis1, 1, img.naxis2,
	     zmin, zmax, tr);
  giza_box ("BCTSNI", 0., 0, "BCTSNI", 0., 0);

  giza_set_text_background (255);	// Satellite names text background color

  // Environment variables
  env = getenv ("ST_TLEDIR");
  sprintf (filename, "%s/classfd.tle", env);
  plot_satellites (filename, img, 0, img.mjd, img.exptime, 4);
  sprintf (filename, "%s/inttles.tle", env);
  plot_satellites (filename, img, 0, img.mjd, img.exptime, 3);
  sprintf (filename, "%s/catalog.tle", env);
  plot_satellites (filename, img, 0, img.mjd, img.exptime, 0);
  sprintf (filename, "%s/jsc.txt", env);
  plot_satellites (filename, img, 0, img.mjd, img.exptime, 5);

  giza_stop_prompting ();
  giza_close_device ();

  return 0;
}

// Read fits image
struct image
read_fits (char *filename)
{
  int i, j, k, l, m;
  qfitsloader ql;
  char key[FITS_LINESZ + 1];
  char val[FITS_LINESZ + 1];
  struct image img;

  // Copy filename
  strcpy (img.filename, filename);

  // Image size
  img.naxis = atoi (qfits_query_hdr (filename, "NAXIS"));
  img.naxis1 = atoi (qfits_query_hdr (filename, "NAXIS1"));
  img.naxis2 = atoi (qfits_query_hdr (filename, "NAXIS2"));

  // MJD
  img.mjd = (double) atof (qfits_query_hdr (filename, "MJD-OBS"));
  strcpy (img.nfd, qfits_query_hdr (filename, "DATE-OBS"));
  img.exptime = atof (qfits_query_hdr (filename, "EXPTIME"));

  // COSPAR ID
  img.cospar = atoi (qfits_query_hdr (filename, "COSPAR"));

  // Tracked
  if (qfits_query_hdr (filename, "TRACKED") != NULL)
    img.tracked = atoi (qfits_query_hdr (filename, "TRACKED"));
  else
    img.tracked = 0;

  // Transformation
  img.ra0 = atof (qfits_query_hdr (filename, "CRVAL1"));
  img.de0 = atof (qfits_query_hdr (filename, "CRVAL2"));
  img.x0 = atof (qfits_query_hdr (filename, "CRPIX1"));
  img.y0 = atof (qfits_query_hdr (filename, "CRPIX2"));
  img.a[0] = 0.0;
  img.a[1] = 3600.0 * atof (qfits_query_hdr (filename, "CD1_1"));
  img.a[2] = 3600.0 * atof (qfits_query_hdr (filename, "CD1_2"));
  img.b[0] = 0.0;
  img.b[1] = 3600.0 * atof (qfits_query_hdr (filename, "CD2_1"));
  img.b[2] = 3600.0 * atof (qfits_query_hdr (filename, "CD2_2"));
  img.xrms = 3600.0 * atof (qfits_query_hdr (filename, "CRRES1"));
  img.yrms = 3600.0 * atof (qfits_query_hdr (filename, "CRRES2"));

  // Set parameters
  ql.xtnum = 0;
  ql.ptype = PTYPE_FLOAT;
  ql.filename = filename;

  // Read four-frame info
  if (img.naxis == 3)
    {
      // Number of frames
      img.nframes = atoi (qfits_query_hdr (filename, "NFRAMES"));

      // Timestamps
      img.dt = (float *) malloc (sizeof (float) * img.nframes);
      for (i = 0; i < img.nframes; i++)
	{
	  sprintf (key, "DT%04d", i);
	  strcpy (val, qfits_query_hdr (filename, key));
	  sscanf (val + 1, "%f", &img.dt[i]);
	  //    img.dt[i]=atof(qfits_query_hdr(filename,key));
	}

      // Allocate image memory
      img.zavg = (float *) malloc (sizeof (float) * img.naxis1 * img.naxis2);
      img.zstd = (float *) malloc (sizeof (float) * img.naxis1 * img.naxis2);
      img.zmax = (float *) malloc (sizeof (float) * img.naxis1 * img.naxis2);
      img.znum = (float *) malloc (sizeof (float) * img.naxis1 * img.naxis2);

      // Loop over planes
      for (k = 0; k < 4; k++)
	{
	  ql.pnum = k;

	  // Initialize load
	  if (qfitsloader_init (&ql) != 0)
	    printf ("Error initializing data loading\n");

	  // Test load
	  if (qfits_loadpix (&ql) != 0)
	    printf ("Error loading actual data\n");

	  // Fill z array
	  for (i = 0, l = 0; i < img.naxis1; i++)
	    {
	      for (j = 0; j < img.naxis2; j++)
		{
		  if (k == 0)
		    img.zavg[l] = ql.fbuf[l];
		  if (k == 1)
		    img.zstd[l] = ql.fbuf[l];
		  if (k == 2)
		    img.zmax[l] = ql.fbuf[l];
		  if (k == 3)
		    img.znum[l] = ql.fbuf[l];
		  l++;
		}
	    }
	}
    }
  else
    {
      // Allocate image memory
      img.zavg = (float *) malloc (sizeof (float) * img.naxis1 * img.naxis2);

      ql.pnum = 0;

      // Initialize load
      if (qfitsloader_init (&ql) != 0)
	printf ("Error initializing data loading\n");

      // Test load
      if (qfits_loadpix (&ql) != 0)
	printf ("Error loading actual data\n");

      // Fill z array
      for (i = 0, l = 0; i < img.naxis1; i++)
	{
	  for (j = 0; j < img.naxis2; j++)
	    {
	      img.zavg[l] = ql.fbuf[l];
	      l++;
	    }
	}
    }

  return img;
}

// Get observing site
void
get_site (int site_id)
{
  int i = 0;
  char line[LIM];
  FILE *file;
  int id;
  double lat, lng;
  float alt;
  char abbrev[3], observer[64], filename[LIM], *env;

  env = getenv ("ST_DATADIR");
  sprintf (filename, "%s/data/sites.txt", env);
  file = fopen (filename, "r");
  if (file == NULL)
    {
      printf ("File with site information not found!\n");
      return;
    }
  while (fgets (line, LIM, file) != NULL)
    {
      // Skip
      if (strstr (line, "#") != NULL)
	continue;

      // Strip newline
      line[strlen (line) - 1] = '\0';

      // Read data
      sscanf (line, "%4d %2s %lf %lf %f", &id, abbrev, &lat, &lng, &alt);
      strcpy (observer, line + 38);

      // Change to km
      alt /= 1000.0;

      if (id == site_id)
	{
	  m.lat = lat;
	  m.lng = lng;
	  m.alt = alt;
	  m.site_id = id;
	  strcpy (m.observer, observer);
	}

    }
  fclose (file);

  return;
}

// Greenwich Mean Sidereal Time
double
gmst (double mjd)
{
  double t, gmst;

  t = (mjd - 51544.5) / 36525.0;

  gmst =
    modulo (280.46061837 + 360.98564736629 * (mjd - 51544.5) +
	    t * t * (0.000387933 - t / 38710000), 360.0);

  return gmst;
}

// Greenwich Mean Sidereal Time
double
dgmst (double mjd)
{
  double t, dgmst;

  t = (mjd - 51544.5) / 36525.0;

  dgmst = 360.98564736629 + t * (0.000387933 - t / 38710000);

  return dgmst;
}

// Return x modulo y [0,y)
double
modulo (double x, double y)
{
  x = fmod (x, y);
  if (x < 0.0)
    x += y;

  return x;
}

// Observer position
void
obspos_xyz (double mjd, xyz_t * pos, xyz_t * vel)
{
  double ff, gc, gs, theta, s, dtheta;

  s = sin (m.lat * D2R);
  ff = sqrt (1.0 - FLAT * (2.0 - FLAT) * s * s);
  gc = 1.0 / ff + m.alt / XKMPER;
  gs = (1.0 - FLAT) * (1.0 - FLAT) / ff + m.alt / XKMPER;

  theta = gmst (mjd) + m.lng;
  dtheta = dgmst (mjd) * D2R / 86400;

  pos->x = gc * cos (m.lat * D2R) * cos (theta * D2R) * XKMPER;
  pos->y = gc * cos (m.lat * D2R) * sin (theta * D2R) * XKMPER;
  pos->z = gs * sin (m.lat * D2R) * XKMPER;
  vel->x = -gc * cos (m.lat * D2R) * sin (theta * D2R) * XKMPER * dtheta;
  vel->y = gc * cos (m.lat * D2R) * cos (theta * D2R) * XKMPER * dtheta;
  vel->z = 0.0;

  return;
}

// Solar position
void
sunpos_xyz (double mjd, xyz_t * pos)
{
  double jd, t, l0, m, e, c, r;
  double n, s, ecl, ra, de;

  jd = mjd + 2400000.5;
  t = (jd - 2451545.0) / 36525.0;
  l0 = modulo (280.46646 + t * (36000.76983 + t * 0.0003032), 360.0) * D2R;
  m = modulo (357.52911 + t * (35999.05029 - t * 0.0001537), 360.0) * D2R;
  e = 0.016708634 + t * (-0.000042037 - t * 0.0000001267);
  c = (1.914602 + t * (-0.004817 - t * 0.000014)) * sin (m) * D2R;
  c += (0.019993 - 0.000101 * t) * sin (2.0 * m) * D2R;
  c += 0.000289 * sin (3.0 * m) * D2R;

  r = 1.000001018 * (1.0 - e * e) / (1.0 + e * cos (m + c));
  n = modulo (125.04 - 1934.136 * t, 360.0) * D2R;
  s = l0 + c + (-0.00569 - 0.00478 * sin (n)) * D2R;
  ecl =
    (23.43929111 +
     (-46.8150 * t - 0.00059 * t * t + 0.001813 * t * t * t) / 3600.0 +
     0.00256 * cos (n)) * D2R;

  ra = atan2 (cos (ecl) * sin (s), cos (s));
  de = asin (sin (ecl) * sin (s));

  pos->x = r * cos (de) * cos (ra) * XKMPAU;
  pos->y = r * cos (de) * sin (ra) * XKMPAU;
  pos->z = r * sin (de) * XKMPAU;

  return;
}

// Compute precession angles
void
precession_angles (double mjd0, double mjd, double *zeta, double *z,
		   double *theta)
{
  double t0, t;

  // Time in centuries
  t0 = (mjd0 - 51544.5) / 36525.0;
  t = (mjd - mjd0) / 36525.0;

  // Precession angles
  *zeta = (2306.2181 + 1.39656 * t0 - 0.000139 * t0 * t0) * t;
  *zeta += (0.30188 - 0.000344 * t0) * t * t + 0.017998 * t * t * t;
  *zeta *= D2R / 3600.0;
  *z = (2306.2181 + 1.39656 * t0 - 0.000139 * t0 * t0) * t;
  *z += (1.09468 + 0.000066 * t0) * t * t + 0.018203 * t * t * t;
  *z *= D2R / 3600.0;
  *theta = (2004.3109 - 0.85330 * t0 - 0.000217 * t0 * t0) * t;
  *theta += -(0.42665 + 0.000217 * t0) * t * t - 0.041833 * t * t * t;
  *theta *= D2R / 3600.0;

  return;
}

// Initialize observer and sun position and precession angles
void
initialize (struct image img)
{
  int i;
  double t;
  xyz_t obsvel;

  // Loop over points
  for (i = 0; i < MMAX; i++)
    {
      // Compute time
      t = img.exptime * (float) i / (float) (MMAX - 1);
      p[i].mjd = img.mjd + t / 86400.0;

      // Compute observer and sun position
      obspos_xyz (p[i].mjd, &p[i].obspos, &obsvel);
      sunpos_xyz (p[i].mjd, &p[i].sunpos);

      // Compute precession angles
      precession_angles (p[i].mjd, 51544.5, &p[i].zeta, &p[i].z, &p[i].theta);
    }

  return;
}