// gondolageom.cc
// Eric Aubourg         CEA/DAPNIA/SPP   octobre 1999

#include <math.h>
#include "gondolageom.h"


extern "C" {
#include "aa_hadec.h"
}

#ifndef M_PI
#define M_PI 3.1415926535
#endif

double GondolaGeom::elevFPC   = 41;
double GondolaGeom::azFPC     = 0;
double GondolaGeom::elevSST0  = 39.6567;
double GondolaGeom::sstPixelHeight = 0.031359;

 			//	 ;
  			//	 ;                            /\         4     2
  			//	 ;                  elevation ||                       positive scanning, clockwise
  			//	 ;                            ||       6    1    5      ---------->
  			//	 ;                            ||                      == positive azimut
  			//	 ;                            ||         x     3
            //                                         |----|      = 0.78 deg / cos(elev)
            // bol 1 = 41° elevation
            

GondolaGeom::GondolaGeom()
: fit(200, 2)
{
  azPend = 0;
  angPend = 0;
  
  nstars = -1;
}

void GondolaGeom::setEarthPos(double lon, double lat) {
  this->lon = lon;
  this->lat = lat;
}

void GondolaGeom::setTSid(double ts) {
  this->ts = ts;
}

void GondolaGeom::setPendulation(double azimuth, double ampl) {
  azPend = azimuth;
  angPend = ampl;
}

void GondolaGeom::addStar(double deltasn, double az, double elv, double /*diod*/) {
  if (nstars<0) {
    nstars = 0;
    fit.clear();
  }
  
  nstars++;
  double azCor = (az - angPend * cos((az - azPend) * M_PI/180) * tan(elv * M_PI/180));
  if (nstars == 1) {
    az0 = azCor;
  }
  if (azCor - az0 >  180) azCor -= 360;
  if (azCor - az0 < -180) azCor += 360;
  
  fit.addData(deltasn, azCor);
}

// $CHECK$ do a higher order fit ?
int GondolaGeom::solveStars() {
  if (nstars<2) return -1;
  fit.doFit();
  azimut = fit.value(0);
  
  if (azimut > 360) azimut -= 360;
  if (azimut < 0)   azimut += 360;

  nstars = -1;
  
  return 0;
}

void GondolaGeom::getPointing(double elv, double az, double& trueElv, double& trueAz) {
  trueElv = elv - angPend * sin((az - azPend) * M_PI/180);
  trueAz  = az +  angPend * cos((az - azPend) * M_PI/180) * tan(elv * M_PI/180);
}

void GondolaGeom::getSkyPointing(double elv, double az, double& ra, double& dec) {
  double trueElv, trueAz;
  getPointing(elv, az, trueElv, trueAz);
  
  double ha;
  aa_hadec (lat * M_PI/180, elv * M_PI/180, az * M_PI/180, &ha, &dec);
  ra = - (ha * 180. / M_PI / 15) + (ts/3600.);
  while (ra>24) ra -= 24;
  while (ra<0) ra += 24;
  dec = dec * 180. / M_PI;
}

double GondolaGeom::getAzimutSST() {
  double elv, az;
  getPointing(elevSST0, azimut, elv, az);
  return az;
}

double GondolaGeom::getElvSST() {
  double elv, az;
  getPointing(elevSST0, azimut, elv, az);
  return elv;
}

double GondolaGeom::getAlphaSST() {
  double ra, dec;
  getSkyPointing(elevSST0, azimut, ra, dec);
  return ra;
}

double GondolaGeom::getDeltaSST() {
  double ra, dec;
  getSkyPointing(elevSST0, azimut, ra, dec);
  return dec;
}

void GondolaGeom::getAltAzBolo(int ibolo, double& elv, double& az) { // azimut relative to SST.
  double delElv = 0;
  double delAz = 0;  // relative to ch1-bolo1
  switch (ibolo) {
    case 11:
      delElv =   0;
      delAz  =   0;
      break;
    case 8:
      delElv =   0.78 * sqrt(3.)/2.;
      delAz  =   0.78 * 1./2.;
      break;
    case 13:
      delElv = - 0.78 * sqrt(3.)/2.;
      delAz  =   0.78 * 1./2.;
      break;
    case 9:
      delElv =   0.78 * sqrt(3.)/2.;
      delAz  = - 0.78 * 1./2.;
      break;
    case 4:
      delElv =   0.;
      delAz  =   0.78;
      break;
    case 15:
      delElv =   0.;
      delAz  = - 0.78;
      break;
    default:
      return;
  }
  elv = elevFPC + delElv;
  delAz /= cos(elv * M_PI/180);
  az  = azFPC + delAz;
  return;
}

double GondolaGeom::getAzimutBolo(int ibolo) {
  double elv, az;
  getAltAzBolo(ibolo, elv, az);
  double trueelv, trueaz;
  getPointing(elv, az, trueelv, trueaz);
  return trueaz;
}

double GondolaGeom::getElvBolo(int ibolo) {
  double elv, az;
  getAltAzBolo(ibolo, elv, az);
  double trueelv, trueaz;
  getPointing(elv, az, trueelv, trueaz);
  return trueelv;
}


double GondolaGeom::getAlphaBolo(int ibolo) {
  double elv, az;
  getAltAzBolo(ibolo, elv, az);
  double ra, dec;
  getSkyPointing(elv, az, ra, dec);
  return ra;
}


double GondolaGeom::getDeltaBolo(int ibolo) {
  double elv, az;
  getAltAzBolo(ibolo, elv, az);
  double ra, dec;
  getSkyPointing(elv, az, ra, dec);
  return dec;
}


double GondolaGeom::getAlphaCenter() {
  return getAlphaBolo(11);
}

double GondolaGeom::getDeltaCenter() {
  return getDeltaBolo(11);
}

double GondolaGeom::getAzimutCenter() {
  return getAzimutBolo(11);
}

double GondolaGeom::getElvCenter() {
  return getElvBolo(11);
}

double GondolaGeom::getAzimutAxis() {
  return azPend;
}

double GondolaGeom::getElvAxis() {
  return 90-angPend;
}

double GondolaGeom::getAlphaAxis() {
  double elv = getElvAxis();
  double az  = getAzimutAxis();
  double ha;
  double ra, dec;
  aa_hadec (lat * M_PI/180, elv * M_PI/180, az * M_PI/180, &ha, &dec);
  ra = - (ha * 180. / M_PI / 15) + (ts/3600.);
  while (ra>24) ra -= 24;
  while (ra<0) ra += 24;
  dec = dec * 180. / M_PI;
  return ra;
}

double GondolaGeom::getDeltaAxis() {
  double elv = getElvAxis();
  double az  = getAzimutAxis();
  double ha;
  double ra, dec;
  aa_hadec (lat * M_PI/180, elv * M_PI/180, az * M_PI/180, &ha, &dec);
  ra = - (ha * 180. / M_PI / 15) + (ts/3600.);
  while (ra>24) ra -= 24;
  while (ra<0) ra += 24;
  dec = dec * 180. / M_PI;
  return dec;
}