#include "auxinterpgps.h"
#include "fitsio.h"
#include "archtoi.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.lower_bound(elem(mjd,0,0,0));
  ilastok = (ilast != elems.end());
}


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;
  if (mjd <= (*ilast).mjd) return -1;
  set<elem>::const_iterator i = ilast; i++;
  if (mjd > (*i).mjd) return -1;
  
  double x = (mjd - (*ilast).mjd) / ((*i).mjd - (*ilast).mjd);
  
  lat = (*ilast).lat*(1-x) + (*i).lat*x;
  lon = (*ilast).lon*(1-x) + (*i).lon*x;
  alt = (*ilast).alt*(1-x) + (*i).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, &(*i).mjd, &fitsStatus);
    fits_write_col_dbl(fptr, 2, fitsLine, 1, 1, &(*i).lat, &fitsStatus);
    fits_write_col_dbl(fptr, 3, fitsLine, 1, 1, &(*i).lon, &fitsStatus);
    fits_write_col_dbl(fptr, 4, fitsLine, 1, 1, &(*i).alt, &fitsStatus);
  }

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