// 
// smeg - A satellite modelling and prediction tool
// Copyright (C) 1999  Tom Rothamel
// 
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with this program; see the file COPYING.  If not, write to
// the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
// Boston, MA 02111-1307, USA.


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

static const double F = (1.0/298.257222);  /* Flattening of the Earth */
static double e2 = 2 * F - F * F;

LLA *Locatable::calclla() {
        double theta, r, phi, c, s, sinphi;
	double lat, lon, alt;
	LLA *lla;

	lla = new LLA;
	
	theta = atan2(x[1], x[0]);

        lon = fmod(theta - sgp_gmst(ct), 2*M_PI);
        r = hypot(x[0], x[1]);
        lat = atan2(x[2], r);

        do {
                phi = lat;
                sinphi = sin(phi);
                c = 1.0/sqrt(1.00 - (e2*sinphi*sinphi));
                lat = atan2(x[2] +  c*e2*sinphi * R, r);
        } while (fabs(lat-phi) > 0.0000001);

	lla->alt = r/cos(lat) - c * R;
	
        if (lon < -M_PI) lon += 2*M_PI;

	lla->lon = lon * RADEG;
	lla->lat = lat * RADEG;
	return lla;
}



Azrael::Azrael(Locatable *s, Site *si, double t) {
	int i;
	double theta;

	ct = t;
	s->calcpos(t);
	si->calcpos(t);
	
	for (i = 0; i < 3; i++) {
		sx[i] = s->x[i];
		six[i] = si->x[i];
		sdx[i] = s->dx[i];
		sidx[i] = si->dx[i];
		ox[i] = sx[i] - six[i];
	}

	ra = len(ox);

	righta = M_PI - atan2(-ox[1], ox[0]);
	dec = atan2(ox[2], hypot(ox[1], ox[0]));

	theta = sgp_gmst(ct) + si->lonr + M_PI;
	ox[0] = -ox[0];
	zrot(ox, sin(theta), cos(theta));
	yrot(ox, si->coslat, si->sinlat);

	az = M_PI - atan2(-ox[1], ox[0]);
	el = atan2(ox[2], hypot(ox[1], ox[0]));

	righta *= RADEG;
	dec *= RADEG;
	az *= RADEG;
	el *= RADEG;
}

double Elevation(Locatable *s, Site *si, double t) {
	double ox[3];
	double theta;
	double el;
	int i;
	
	s->calcpos(t);
	si->calcpos(t);

	for (i = 0; i < 3; i++) {
		ox[i] = s->x[i] - si->x[i];
	}

	theta = sgp_gmst(t) + si->lonr + M_PI;
	ox[0] = -ox[0];

	zrot(ox, sin(theta), cos(theta));
	yrot(ox, si->coslat, si->sinlat);
	el = atan2(ox[2], hypot(ox[1], ox[0]));

	return el * RADEG;
}