// auxinterpgps.cc
// Eric Aubourg         CEA/DAPNIA/SPP   aout 1999

#include "auxinterpgps.h"
#include "fitsio.h"
#include "archvers.h"

AuxInterpGPS::AuxInterpGPS() {
  ilastok = false;
}

void AuxInterpGPS::AddValue(double mjd, double lat, double lon, double alt) {
  elems.insert(elem(mjd, lat, lon, alt));
  ilastok = false;
}

void AuxInterpGPS::FindElem(double mjd) {
  ilast = elems.upper_bound(elem(mjd,0,0,0));
  ilastok = (ilast != elems.end() && ilast != elems.begin());
}

bool AuxInterpGPS::canGetLocation(double mjd) {
  return (mjd > (*elems.begin()).mjd && mjd < (*--elems.end()).mjd);
}

double AuxInterpGPS::getStartMJD() {
  return (*elems.begin()).mjd;
}

double AuxInterpGPS::getEndMJD() {
  return (*--elems.end()).mjd;
}


int AuxInterpGPS::getLocation(double mjd, double& lat, double& lon, double& alt) {
  if (!ilastok ||
      (mjd > (*ilast).mjd)) {
    FindElem(mjd);
  } else {
    set<elem>::const_iterator i = ilast; i--;
    if (mjd <= (*i).mjd) FindElem(mjd);
  }
  
  

  if (!ilastok) return -1;
  double lastmjd = (*ilast).mjd;
  if (mjd > lastmjd) return -1;
  if (ilast == elems.begin()) return -1;
  set<elem>::const_iterator i = ilast; i--;
  double prevmjd = (*i).mjd;
  if (mjd < prevmjd) return -1;
  
  double x = (mjd - prevmjd) / (lastmjd - prevmjd);
  
  lat = (*i).lat*(1-x) + (*ilast).lat*x;
  lon = (*i).lon*(1-x) + (*ilast).lon*x;
  alt = (*i).alt*(1-x) + (*ilast).alt*x;
  
  return 0;
}

void AuxInterpGPS::FitsDump(string const& filename) {
  int fitsStatus=0;
  fitsfile* fptr;
  remove(filename.c_str());
  fits_create_file(&fptr, filename.c_str(), &fitsStatus);
  char* colnames[4] = {"MJD","LATITUDE","LONGITUDE","ALTITUDE"};
  char* coltypes[4] = {"1D","1D","1D","1D"};
  char* colunits[4] = {" ", " " , " ", " "};
  fits_create_tbl(fptr, BINARY_TBL, 0, 4, colnames, coltypes, colunits, NULL, &fitsStatus);
  fits_write_date(fptr, &fitsStatus);
  fits_write_comment(fptr, "Generated with archtoi " ARCHTOI_VERS, &fitsStatus);
  int fitsLine = 1;
  for (set<elem>::iterator i = elems.begin(); i != elems.end(); i++,fitsLine++) {
    fits_write_col_dbl(fptr, 1, fitsLine, 1, 1, (double*) &(*i).mjd, &fitsStatus);
    fits_write_col_dbl(fptr, 2, fitsLine, 1, 1, (double*) &(*i).lat, &fitsStatus);
    fits_write_col_dbl(fptr, 3, fitsLine, 1, 1, (double*) &(*i).lon, &fitsStatus);
    fits_write_col_dbl(fptr, 4, fitsLine, 1, 1, (double*) &(*i).alt, &fitsStatus);
  }

  fits_close_file(fptr, &fitsStatus);
  fits_report_error(stderr, fitsStatus);  /* print out any error messages */
}