#include "i3_wcs.h"
#include "i3_image.h"

i3_transform get_null_transform(){
	i3_transform o = {0};
	return o;
}

// Note that the delta_ra here is in actual RA.
void i3_wcs_to_image(i3_flt delta_ra, i3_flt delta_dec, i3_flt e1_w, i3_flt e2_w, i3_flt ab_w, i3_transform T, i3_flt *x, i3_flt *y, i3_flt *e1, i3_flt *e2, i3_flt *ab){

        // Locally linearized approx to x, y at ra, dec for i3_transform T
	*x = T.x0 + T.A[0][0] * (delta_ra) + T.A[0][1] * (delta_dec);
	*y = T.y0 + T.A[1][0] * (delta_ra) + T.A[1][1] * (delta_dec);
    *ab = (T.A[0][0] * T.A[1][1] - T.A[0][1] * T.A[1][0]) * ab_w; // Jacobian determinant
    *ab = i3_fabs(*ab); // The determinant can legitimately be < 0 if the image is flipped wrt sky

        // Then do ellipticities
        i3_flt e1_q, e2_q; // For converting to (a^2 - b^2) / (a^2 + b^2) style ellipticities
	i3_e_linear_to_quadratic(e1_w, e2_w, &e1_q, &e2_q);

        // Build the "ellipticity tensor" in world (e_w) and image/pixel (e) coords
	i3_flt e_w[2][2];
	e_w[0][0] = 1. + e1_q;
	e_w[1][0] = e2_q;
	e_w[0][1] = e2_q;
	e_w[1][1] = 1. - e1_q;
	i3_flt e[2][2];
	
	// Double matrix multiplication e  = A e_w A^T
	for (int i=0; i<=1; i++){
        for (int j=i; j<=1; j++){ // symmetric so can ignore lower triangle
			e[i][j] = 0.;
			for (int k=0; k<=1; k++){
				for (int ell=0; ell<=1; ell++){
					e[i][j] += T.A[i][k] * T.A[j][ell] * e_w[k][ell];
				}
			}
		}
	}

        // Extract ellipticites (still a^2-b^2 style) from ellipticity tensor e
	i3_flt trace = e[0][0] + e[1][1];
	*e1 = (e[0][0] - e[1][1]) / trace;
	*e2 = 2. * (e[0][1]) / trace;
        // Convert back to im3shape (a-b)/(a+b) ellipticities        
	i3_e_quadratic_to_linear(*e1, *e2, e1, e2);  // Pass copies and write over pointer values

}


void i3_image_to_wcs(i3_flt x, i3_flt y, i3_flt e1_p, i3_flt e2_p, i3_flt ab_p, i3_transform T, i3_flt *delta_ra, i3_flt *delta_dec, i3_flt *e1, i3_flt *e2, i3_flt *ab){

        // Locally linearized approx to ra, dec at x, y for i3_transform T
	*delta_ra =  T.A[0][0] * (x - T.x0) + T.A[0][1] * (y - T.y0);
	*delta_dec = T.A[1][0] * (x - T.x0) + T.A[1][1] * (y - T.y0);
    *ab = (T.A[0][0] * T.A[1][1] - T.A[0][1] * T.A[1][0]) * ab_p; // Jacobian determinant
    *ab = i3_fabs(*ab); // The determinant can legitimately be < 0 if the image is flipped wrt sky

        // Then do ellipticities
        i3_flt e1_q, e2_q; // For converting to (a^2 - b^2) / (a^2 + b^2) style ellipticities
	i3_e_linear_to_quadratic(e1_p, e2_p, &e1_q, &e2_q);

        // Build the "ellipticity tensor" in image/pixel (e_p) and world (e) coords
	i3_flt e_p[2][2];
	e_p[0][0] = 1. + e1_q;
	e_p[1][0] = e2_q;
	e_p[0][1] = e2_q;
	e_p[1][1] = 1. - e1_q;
	i3_flt e[2][2];

        // Double matrix multiplication e  = A e_p A^T
	for (int i=0; i<=1; i++){
		for (int j=i; j<=1; j++){ // symmetric so can ignore lower triangle
			e[i][j] = 0.;
			for (int k=0; k<=1; k++){
				for (int ell=0; ell<=1; ell++){
					e[i][j] += T.A[i][k] * T.A[j][ell] * e_p[k][ell];
				}
			}
		}
	}

        // Extract ellipticites (still a^2-b^2 style) from ellipticity tensor e
	i3_flt trace = e[0][0] + e[1][1];
	*e1 = (e[0][0] - e[1][1]) / trace;
	*e2 = 2. * (e[0][1]) / trace;
	
        // Convert back to im3shape (a-b)/(a+b) ellipticities        
	i3_e_quadratic_to_linear(*e1, *e2, e1, e2); // Pass copies and write over pointer values

}

// 
// struct wcsprm * i3_get_wcsprm_from_file(char * filename, int hdu){
// 
// 	int status=0;
// 	int hdutype;
// 	fitsfile * input_file;
// 
// 	/* Open the FITS image.	 We do not check the status immediately as FITS routines
// 		 check the input value of status and return immediately if non-zero.
// 	
// 		 However this routine exits we make sure to close the file before we leave.
// 	*/
// 	fits_open_file(&input_file, filename, READONLY, &status);
// 
// 	/* Move to the relevant HDU and check the image type and dimension.
// 		 For now we assume that the image is 2D but this might change depending on how 
// 		 files are structured.
// 	*/
// 	fits_movabs_hdu(input_file, hdu, &hdutype, &status);
// 
// 	if (status){
// 		fprintf(stderr,"Error opening fits file %s HDU %d:",filename,hdu);
// 		fits_report_error(stderr,status);
// 		fits_close_file(input_file,&status);
// 		return NULL;
// 	}
// 
// 	char * header = NULL;
// 	int nkeys = 0;
// 	fits_hdr2str(input_file, 1, NULL, 0, &header, &nkeys, &status);
// 	if (status){
// 		fprintf(stderr,"Error getting header from fits file %s HDU %d:",filename, hdu);
// 		fits_report_error(stderr,status);
// 		fits_close_file(input_file,&status);
// 		return NULL;
// 	}
// 	int nrejected;
// 	int nwcsfound;
// 	struct wcsprm * wcs;
// 	status |= wcspih(header, nkeys, WCSHDR_all, 2, &nrejected, &nwcsfound, &wcs);
// 	if (status){
// 		fprintf(stderr,"Error parsing header from fits file %s HDU %d:",filename, hdu);
// 		fits_report_error(stderr,status);
// 		fits_close_file(input_file,&status);
// 		free(header);
// 		return NULL;
// 	}
// 	return wcs;
// }
// 
//