/*
# This file is part of the Astrometry.net suite.
# Licensed under a 3-clause BSD style license - see LICENSE
*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <sys/param.h>

#include "sip.h"
#include "starutil.h"
#include "mathutil.h"

static anbool has_distortions(const sip_t* sip) {
	return (sip->a_order >= 0);
}

anbool sip_xyz2pixelxy(const sip_t* sip, double x, double y, double z, double *px, double *py) {
	double xyz[3];
	xyz[0] = x;
	xyz[1] = y;
	xyz[2] = z;
	return sip_xyzarr2pixelxy(sip, xyz, px, py);
}

void sip_wrap_tan(const tan_t* tan, sip_t* sip) {
	memset(sip, 0, sizeof(sip_t));
	memcpy(&(sip->wcstan), tan, sizeof(tan_t));
}

void sip_get_crval(const sip_t* sip, double* ra, double* dec) {
	*ra = sip->wcstan.crval[0];
	*dec = sip->wcstan.crval[1];
}

double sip_imagew(sip_t* sip) {
	assert(sip);
	return sip->wcstan.imagew;
}
double sip_imageh(sip_t* sip) {
	assert(sip);
	return sip->wcstan.imageh;
}
sip_t* sip_create() {
	sip_t* sip = calloc(1, sizeof(sip_t));

	sip->wcstan.cd[0][0] = 1;
	sip->wcstan.cd[0][1] = 0;
	sip->wcstan.cd[1][0] = 0;
	sip->wcstan.cd[1][1] = 1;

	return sip;
}

void sip_free(sip_t* sip) {
	free(sip);
}

void sip_copy(sip_t* dest, const sip_t* src) {
	memcpy(dest, src, sizeof(sip_t));
}

static void sip_distortion(const sip_t* sip, double x, double y,
                           double* X, double* Y) {
	// Get pixel coordinates relative to reference pixel
	double u = x - sip->wcstan.crpix[0];
	double v = y - sip->wcstan.crpix[1];
	sip_calc_distortion(sip, u, v, X, Y);
	*X += sip->wcstan.crpix[0];
	*Y += sip->wcstan.crpix[1];
}

// Pixels to RA,Dec in degrees.
void sip_pixelxy2radec(const sip_t* sip, double px, double py,
					   double *ra, double *dec) {
	if (has_distortions(sip)) {
		double U, V;
		sip_distortion(sip, px, py, &U, &V);
		// Run a normal TAN conversion on the distorted pixel coords.
		tan_pixelxy2radec(&(sip->wcstan), U, V, ra, dec);
	} else
		// Run a normal TAN conversion
		tan_pixelxy2radec(&(sip->wcstan), px, py, ra, dec);
}

// Pixels to Intermediate World Coordinates in degrees.
void sip_pixelxy2iwc(const sip_t* sip, double px, double py,
					 double *iwcx, double* iwcy) {
	if (has_distortions(sip)) {
		double U, V;
		sip_distortion(sip, px, py, &U, &V);
		// Run a normal TAN conversion on the distorted pixel coords.
		tan_pixelxy2iwc(&(sip->wcstan), U, V, iwcx, iwcy);
	} else
		// Run a normal TAN conversion
		tan_pixelxy2iwc(&(sip->wcstan), px, py, iwcx, iwcy);
}

// Pixels to XYZ unit vector.
void sip_pixelxy2xyzarr(const sip_t* sip, double px, double py, double *xyz) {
	if (has_distortions(sip)) {
		double U, V;
		sip_distortion(sip, px, py, &U, &V);
		// Run a normal TAN conversion on the distorted pixel coords.
		tan_pixelxy2xyzarr(&(sip->wcstan), U, V, xyz);
	} else
		// Run a normal TAN conversion
		tan_pixelxy2xyzarr(&(sip->wcstan), px, py, xyz);
}

// Pixels to RA,Dec in degrees.
void tan_pixelxy2radec(const tan_t* tan, double px, double py, double *ra, double *dec) {
	double xyz[3];
	tan_pixelxy2xyzarr(tan, px, py, xyz);
	xyzarr2radecdeg(xyz, ra,dec);
}

void   tan_pixelxy2radecarr(const tan_t* wcs_tan, double px, double py, double *radec) {
	tan_pixelxy2radec(wcs_tan, px, py, radec+0, radec+1);
}

void tan_iwc2pixelxy(const tan_t* tan, double x, double y,
					 double *px, double* py) {
	double U,V;
	Unused int r;
	double cdi[2][2];

	// Invert CD
    r = invert_2by2_arr((const double*)tan->cd, (double*)cdi);
	assert(r == 0);

	// Linear pixel coordinates
	U = cdi[0][0]*x + cdi[0][1]*y;
	V = cdi[1][0]*x + cdi[1][1]*y;

	// Re-add crpix to get pixel coordinates
	*px = U + tan->crpix[0];
	*py = V + tan->crpix[1];
}

void tan_pixelxy2iwc(const tan_t* tan, double px, double py, double *iwcx, double* iwcy)
{
	// Get pixel coordinates relative to reference pixel
	double U = px - tan->crpix[0];
	double V = py - tan->crpix[1];

	// Get intermediate world coordinates
	double x = tan->cd[0][0] * U + tan->cd[0][1] * V;
	double y = tan->cd[1][0] * U + tan->cd[1][1] * V;

	if (iwcx)
		*iwcx = x;
	if (iwcy)
		*iwcy = y;
}

void tan_iwc2radec(const tan_t* tan, double x, double y, double *p_ra, double *p_dec) {
    double xyz[3];
    tan_iwc2xyzarr(tan, x, y, xyz);
    xyzarr2radecdeg(xyz, p_ra, p_dec);
}

void tan_iwc2xyzarr(const tan_t* tan, double x, double y, double *xyz)
{
	double rx, ry, rz;
	double ix,iy,norm;
	double jx,jy,jz;

	// Mysterious factor of -1 correcting for vector directions below.
	x = -deg2rad(x);
	y =  deg2rad(y);

	// Take r to be the threespace vector of crval
	radecdeg2xyz(tan->crval[0], tan->crval[1], &rx, &ry, &rz);
	//	printf("rx=%lf ry=%lf rz=%lf\n",rx,ry,rz);

	// Form i = r cross north pole (0,0,1)
	ix = ry;
	iy = -rx;
	// iz = 0
	norm = hypot(ix, iy);
	ix /= norm;
	iy /= norm;
	//	printf("ix=%lf iy=%lf iz=0.0\n",ix,iy);
	//	printf("r.i = %lf\n",ix*rx+iy*ry);

	// Form j = i cross r;   iz=0 so some terms drop out
	jx = iy * rz;
	jy =         - ix * rz;
	jz = ix * ry - iy * rx;
	// norm should already be 1, but normalize anyway
	normalize(&jx, &jy, &jz);
	//	printf("jx=%lf jy=%lf jz=%lf\n",jx,jy,jz);
	//	printf("r.j = %lf\n",jx*rx+jy*ry+jz*rz);
	//	printf("i.j = %lf\n",ix*jx+iy*jy);

	if (tan->sin) {
		assert((x*x + y*y) < 1.0);
		// Figure out what factor of r we have to add in to make the resulting length = 1
		double rfrac = sqrt(1.0 - (x*x + y*y));
		// Don't scale the projected x,y positions, just add in the right amount of r to
		// bring it onto the unit sphere
		xyz[0] = ix*x + jx*y + rx * rfrac;
		xyz[1] = iy*x + jy*y + ry * rfrac;
		xyz[2] =        jz*y + rz * rfrac; // iz = 0

	} else {
		// Form the point on the tangent plane relative to observation point,
		xyz[0] = ix*x + jx*y + rx;
		xyz[1] = iy*x + jy*y + ry;
		xyz[2] =        jz*y + rz; // iz = 0
		// and normalize back onto the unit sphere
		normalize_3(xyz);
	}
}

// Pixels to XYZ unit vector.
void tan_pixelxy2xyzarr(const tan_t* tan, double px, double py, double *xyz)
{
	double x,y;
	tan_pixelxy2iwc(tan, px, py, &x, &y);
	tan_iwc2xyzarr(tan, x, y, xyz);
}

void sip_iwc2pixelxy(const sip_t* sip, double u, double v,
					 double *px, double* py) {
    double x,y;
    tan_iwc2pixelxy(&(sip->wcstan), u, v, &x, &y);
    sip_pixel_undistortion(sip, x, y, px, py);
}

// SIP:  (RA,Dec) --> IWC --> x,y --> x',y'

// RA,Dec in degrees to Pixels.
anbool sip_radec2pixelxy(const sip_t* sip, double ra, double dec, double *px, double *py) {
	double x,y;
	if (!tan_radec2pixelxy(&(sip->wcstan), ra, dec, &x, &y))
		return FALSE;
    sip_pixel_undistortion(sip, x, y, px, py);
    return TRUE;
}

void sip_iwc2radec(const sip_t* sip, double x, double y, double *p_ra, double *p_dec) {
    tan_iwc2radec(&(sip->wcstan), x, y, p_ra, p_dec);
}


// RA,Dec in degrees to Pixels.
anbool sip_radec2pixelxy_check(const sip_t* sip, double ra, double dec, double *px, double *py) {
	double u, v;
	double U, V;
	double U2, V2;
	if (!tan_radec2pixelxy(&(sip->wcstan), ra, dec, px, py))
		return FALSE;
	if (!has_distortions(sip))
		return TRUE;

	// Subtract crpix, invert SIP distortion, add crpix.
	// Sanity check:
	if (sip->a_order != 0 && sip->ap_order == 0) {
		fprintf(stderr, "suspicious inversion; no inversion SIP coeffs "
				"yet there are forward SIP coeffs\n");
	}
	U = *px - sip->wcstan.crpix[0];
	V = *py - sip->wcstan.crpix[1];
	sip_calc_inv_distortion(sip, U, V, &u, &v);
    // Check that we're dealing with the right range of the polynomial by inverting it and
    // checking that we end up back in the right place.
    sip_calc_distortion(sip, u, v, &U2, &V2);
    if (fabs(U2 - U) + fabs(V2 - V) > 10.0)
        return FALSE;
	*px = u + sip->wcstan.crpix[0];
	*py = v + sip->wcstan.crpix[1];
	return TRUE;
}

anbool sip_xyzarr2pixelxy(const sip_t* sip, const double* xyz, double *px, double *py) {
	double ra, dec;
	xyzarr2radecdeg(xyz, &ra, &dec);
	return sip_radec2pixelxy(sip, ra, dec, px, py);
}


anbool sip_xyzarr2iwc(const sip_t* sip, const double* xyz,
					double* iwcx, double* iwcy) {
	return tan_xyzarr2iwc(&(sip->wcstan), xyz, iwcx, iwcy);
}
anbool sip_radec2iwc(const sip_t* sip, double ra, double dec,
				   double* iwcx, double* iwcy) {
	return tan_radec2iwc(&(sip->wcstan), ra, dec, iwcx, iwcy);
}

anbool tan_xyzarr2iwc(const tan_t* tan, const double* xyz,
					double* iwcx, double* iwcy) {
	double xyzcrval[3];

	// FIXME be robust near the poles
	// Calculate intermediate world coordinates (x,y) on the tangent plane
	radecdeg2xyzarr(tan->crval[0], tan->crval[1], xyzcrval);

	if (!star_coords(xyz, xyzcrval, !tan->sin, iwcx, iwcy))
		return FALSE;

	*iwcx = rad2deg(*iwcx);
	*iwcy = rad2deg(*iwcy);
	return TRUE;
}

anbool tan_radec2iwc(const tan_t* tan, double ra, double dec,
				   double* iwcx, double* iwcy) {
	double xyz[3];
	radecdeg2xyzarr(ra, dec, xyz);
	return tan_xyzarr2iwc(tan, xyz, iwcx, iwcy);
}

// xyz unit vector to Pixels.
anbool tan_xyzarr2pixelxy(const tan_t* tan, const double* xyzpt, double *px, double *py) {
	double iwx=0, iwy=0;
	if (!tan_xyzarr2iwc(tan, xyzpt, &iwx, &iwy)) {
		return FALSE;
	}
	tan_iwc2pixelxy(tan, iwx, iwy, px, py);
	return TRUE;
}

// RA,Dec in degrees to Pixels.
anbool tan_radec2pixelxy(const tan_t* tan, double a, double d, double *px, double *py)
{
	double xyzpt[3];
	radecdeg2xyzarr(a,d,xyzpt);
	return tan_xyzarr2pixelxy(tan, xyzpt, px, py);
}

void sip_calc_distortion(const sip_t* sip, double u, double v, double* U, double *V) {
	// Do SIP distortion (in relative pixel coordinates)
	// See the sip_t struct definition in header file for details
	int p, q;
	double fuv=0.;
	double guv=0.;

    // avoid using pow() function
    double powu[SIP_MAXORDER];
    double powv[SIP_MAXORDER];
    powu[0] = 1.0;
    powu[1] = u;
    powv[0] = 1.0;
    powv[1] = v;
	for (p=2; p <= MAX(sip->a_order, sip->b_order); p++) {
        powu[p] = powu[p-1] * u;
        powv[p] = powv[p-1] * v;
    }

	for (p=0; p<=sip->a_order; p++)
		for (q=0; q<=sip->a_order; q++)
			//if ((p+q > 0) && (p+q <= sip->a_order))
			if (p+q <= sip->a_order)
				fuv += sip->a[p][q] * powu[p] * powv[q];
	for (p=0; p<=sip->b_order; p++) 
		for (q=0; q<=sip->b_order; q++) 
			//if ((p+q > 0) && (p+q <= sip->b_order))
			if (p+q <= sip->b_order)
				guv += sip->b[p][q] * powu[p] * powv[q];

	*U = u + fuv;
	*V = v + guv;
}

void sip_pixel_distortion(const sip_t* sip, double x, double y, double* X, double *Y) {
	sip_distortion(sip, x, y, X, Y);
}

void sip_pixel_undistortion(const sip_t* sip, double x, double y, double* X, double *Y) {
	if (!has_distortions(sip)) {
        *X = x;
        *Y = y;
        return;
    }
	// Sanity check:
	if (sip->a_order != 0 && sip->ap_order == 0) {
		fprintf(stderr, "suspicious inversion; no inverse SIP coeffs "
				"yet there are forward SIP coeffs\n");
	}

	// Get pixel coordinates relative to reference pixel
	double u = x - sip->wcstan.crpix[0];
	double v = y - sip->wcstan.crpix[1];
	sip_calc_inv_distortion(sip, u, v, X, Y);
	*X += sip->wcstan.crpix[0];
	*Y += sip->wcstan.crpix[1];
}

void sip_calc_inv_distortion(const sip_t* sip, double U, double V, double* u, double *v)
{
	int p, q;
	double fUV=0.;
	double gUV=0.;

    // avoid using pow() function
    double powu[SIP_MAXORDER];
    double powv[SIP_MAXORDER];
    powu[0] = 1.0;
    powu[1] = U;
    powv[0] = 1.0;
    powv[1] = V;
	for (p=2; p <= MAX(sip->ap_order, sip->bp_order); p++) {
        powu[p] = powu[p-1] * U;
        powv[p] = powv[p-1] * V;
    }

	for (p=0; p<=sip->ap_order; p++)
		for (q=0; q<=sip->ap_order; q++)
			fUV += sip->ap[p][q] * powu[p] * powv[q];
	for (p=0; p<=sip->bp_order; p++) 
		for (q=0; q<=sip->bp_order; q++) 
            gUV += sip->bp[p][q] * powu[p] * powv[q];

	*u = U + fUV;
	*v = V + gUV;
}

double tan_det_cd(const tan_t* tan) {
	return (tan->cd[0][0]*tan->cd[1][1] - tan->cd[0][1]*tan->cd[1][0]);
}

double sip_det_cd(const sip_t* sip) {
	return tan_det_cd(&(sip->wcstan));
}

double tan_pixel_scale(const tan_t* tan) {
	double scale = deg2arcsec(sqrt(fabs(tan_det_cd(tan))));
	return scale;
}

// returns pixel scale in arcseconds
double sip_pixel_scale(const sip_t* sip) {
	return tan_pixel_scale(&(sip->wcstan));
}

static void print_to(const tan_t* tan, FILE* f, char* type) {
	fprintf(f,"%s Structure:\n", type);
	fprintf(f,"  crval=(%g, %g)\n", tan->crval[0], tan->crval[1]);
	fprintf(f,"  crpix=(%g, %g)\n", tan->crpix[0], tan->crpix[1]);
	fprintf(f,"  CD = ( %12.5g   %12.5g )\n", tan->cd[0][0], tan->cd[0][1]);
	fprintf(f,"       ( %12.5g   %12.5g )\n", tan->cd[1][0], tan->cd[1][1]);
	fprintf(f,"  image size = (%g x %g)\n", tan->imagew, tan->imageh);
}

void tan_print_to(const tan_t* tan, FILE* f) {
	if (tan->sin) {
		print_to(tan, f, "SIN");
	} else {
		print_to(tan, f, "TAN");
	}
}

void tan_print(const tan_t* tan) {
	tan_print_to(tan, stderr);
}

void sip_print_to(const sip_t* sip, FILE* f) {
   double det,pixsc;

   if (sip->wcstan.sin) {
	   print_to(&(sip->wcstan), f, "SIN-SIP");
   } else {
	   print_to(&(sip->wcstan), f, "TAN-SIP");
   }

   fprintf(f, "  SIP order: A=%i, B=%i, AP=%i, BP=%i\n",
		   sip->a_order, sip->b_order, sip->ap_order, sip->bp_order);

	if (sip->a_order > 0) {
		int p, q;
		for (p=0; p<=sip->a_order; p++) {
			fprintf(f, (p ? "      " : "  A = "));
			for (q=0; q<=sip->a_order; q++)
				if (p+q <= sip->a_order)
					//fprintf(f,"a%d%d=%le\n", p,q,sip->a[p][q]);
					fprintf(f,"%12.5g", sip->a[p][q]);
			fprintf(f,"\n");
		}
	}
	if (sip->b_order > 0) {
		int p, q;
		for (p=0; p<=sip->b_order; p++) {
			fprintf(f, (p ? "      " : "  B = "));
			for (q=0; q<=sip->b_order; q++)
				if (p+q <= sip->a_order)
					fprintf(f,"%12.5g", sip->b[p][q]);
			//if (p+q <= sip->b_order && p+q > 0)
			//fprintf(f,"b%d%d=%le\n", p,q,sip->b[p][q]);
			fprintf(f,"\n");
		}
	}

	if (sip->ap_order > 0) {
		int p, q;
		for (p=0; p<=sip->ap_order; p++) {
			fprintf(f, (p ? "      " : "  AP = "));
			for (q=0; q<=sip->ap_order; q++)
				if (p+q <= sip->ap_order)
					fprintf(f,"%12.5g", sip->ap[p][q]);
			fprintf(f,"\n");
		}
	}
	if (sip->bp_order > 0) {
		int p, q;
		for (p=0; p<=sip->bp_order; p++) {
			fprintf(f, (p ? "      " : "  BP = "));
			for (q=0; q<=sip->bp_order; q++)
				if (p+q <= sip->bp_order)
					fprintf(f,"%12.5g", sip->bp[p][q]);
			fprintf(f,"\n");
		}
	}


	det = sip_det_cd(sip);
	pixsc = 3600*sqrt(fabs(det));
	//fprintf(f,"  det(CD)=%g\n", det);
	fprintf(f,"  sqrt(det(CD))=%g [arcsec]\n", pixsc);
	//fprintf(f,"\n");
}

void sip_print(const sip_t* sip) {
	sip_print_to(sip, stderr);
}

double sip_get_orientation(const sip_t* sip) {
    return tan_get_orientation(&(sip->wcstan));
}

double tan_get_orientation(const tan_t* tan) {
  double T, A, orient;
  double det, parity;
  det = tan_det_cd(tan);
  parity = (det >= 0 ? 1.0 : -1.0);
  T = parity * tan->cd[0][0] + tan->cd[1][1];
  A = parity * tan->cd[1][0] - tan->cd[0][1];
  orient = -rad2deg(atan2(A, T));
  return orient;
}