# include "addstar.h"

XY_to_RD (ra, dec, x, y, coords)
double *ra, *dec;
double  x, y;
Coords coords[];
{

  double L, M, X, Y, T;
  double R, sphi, cphi, stht, ctht;
  double alpha, delta, salp, calp, sdel, sdp, cdp;
  
  *ra  = 0;
  *dec = 0;

# if 1
  if (!strcmp(&coords[0].ctype[4], "-PLY")) {
    X = coords[0].cdelt1*(x - coords[0].crpix1 + x*x*coords[0].polyterms[0][0] + x*y*coords[0].polyterms[1][0] + y*y*coords[0].polyterms[2][0]
			  + x*x*x*coords[0].polyterms[3][0] + x*x*y*coords[0].polyterms[4][0] + x*y*y*coords[0].polyterms[5][0] + y*y*y*coords[0].polyterms[6][0]);
    Y = coords[0].cdelt2*(y - coords[0].crpix2 + x*x*coords[0].polyterms[0][1] + x*y*coords[0].polyterms[1][1] + y*y*coords[0].polyterms[2][1]
			  + x*x*x*coords[0].polyterms[3][1] + x*x*y*coords[0].polyterms[4][1] + x*y*y*coords[0].polyterms[5][1] + y*y*y*coords[0].polyterms[6][1]);
  } else {
# else
  {
# endif
    X = coords[0].cdelt1*(x - coords[0].crpix1);
    Y = coords[0].cdelt2*(y - coords[0].crpix2);
  }    

  L = (X*coords[0].pc1_1 + Y*coords[0].pc1_2);
  M = (X*coords[0].pc2_1 + Y*coords[0].pc2_2);

  if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") || !strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
    R = hypot (L,M);
    if ((L == 0) && (M == 0)) {
      sphi = 0;
      cphi = 1;
    }
    else {
      sphi =  L / R;
      cphi = -M / R;
    }

    if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN")) {
      if (R == 0) {
	stht = 1.0;
	ctht = 0.0;
      }
      else {
	T = DEG_RAD / R;
	stht =   T / sqrt ( 1.0 + T*T);
	ctht = 1.0 / sqrt ( 1.0 + T*T);
      }
    }
    if (!strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
      ctht = RAD_DEG * R;
      stht = sqrt (1 - ctht*ctht);
    }

    sdp  = sin(RAD_DEG*coords[0].crval2);
    cdp  = cos(RAD_DEG*coords[0].crval2);
    
    sdel = stht*sdp - ctht*cphi*cdp;
    salp = ctht*sphi;
    calp = stht*cdp + ctht*cphi*sdp;
    alpha = atan2 (salp, calp);
    delta = asin (sdel);
    
    *ra  = DEG_RAD*alpha + coords[0].crval1;
    *dec = DEG_RAD*delta;
  }
  if (!strcmp(&coords[0].ctype[4], "-LIN") || !strcmp(&coords[0].ctype[0], "GENE")) {
    *ra  = L + coords[0].crval1;
    *dec = M + coords[0].crval2;
  }

}


fXY_to_RD (ra, dec, x, y, coords)
float *ra, *dec;
double  x, y;
Coords coords[];
{

  double L, M, X, Y, T;
  double R, sphi, cphi, stht, ctht;
  double alpha, delta, salp, calp, sdel, sdp, cdp;
  
  *ra  = 0;
  *dec = 0;

  if (!strcmp(&coords[0].ctype[4], "-PLY")) {
    X = coords[0].cdelt1*(x - coords[0].crpix1 + x*x*coords[0].polyterms[0][0] + x*y*coords[0].polyterms[1][0] + y*y*coords[0].polyterms[2][0]
			  + x*x*x*coords[0].polyterms[3][0] + x*x*y*coords[0].polyterms[4][0] + x*y*y*coords[0].polyterms[5][0] + y*y*y*coords[0].polyterms[6][0]);
    Y = coords[0].cdelt2*(y - coords[0].crpix2 + x*x*coords[0].polyterms[0][1] + x*y*coords[0].polyterms[1][1] + y*y*coords[0].polyterms[2][1]
			  + x*x*x*coords[0].polyterms[3][1] + x*x*y*coords[0].polyterms[4][1] + x*y*y*coords[0].polyterms[5][1] + y*y*y*coords[0].polyterms[6][1]);
  } else {
    X = coords[0].cdelt1*(x - coords[0].crpix1);
    Y = coords[0].cdelt2*(y - coords[0].crpix2);
  }    
  L = (X*coords[0].pc1_1 + Y*coords[0].pc1_2);
  M = (X*coords[0].pc2_1 + Y*coords[0].pc2_2);

  if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") || !strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
    R = hypot (L,M);
    if ((L == 0) && (M == 0)) {
      sphi = 0;
      cphi = 1;
    }
    else {
      sphi =  L / R;
      cphi = -M / R;
    }

    if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") ) {
      if (R == 0) {
	stht = 1.0;
	ctht = 0.0;
      }
      else {
	T = DEG_RAD / R;
	stht =   T / sqrt ( 1.0 + T*T);
	ctht = 1.0 / sqrt ( 1.0 + T*T);
      }
    }
    if (!strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
      ctht = RAD_DEG * R;
      stht = sqrt (1 - ctht*ctht);
    }

    sdp  = sin(RAD_DEG*coords[0].crval2);
    cdp  = cos(RAD_DEG*coords[0].crval2);
    
    sdel = stht*sdp - ctht*cphi*cdp;
    salp = ctht*sphi;
    calp = stht*cdp + ctht*cphi*sdp;
    alpha = atan2 (salp, calp);
    delta = asin (sdel);
    
    *ra  = DEG_RAD*alpha + coords[0].crval1;
    *dec = DEG_RAD*delta;
  }
  if (!strcmp(&coords[0].ctype[4], "-LIN") || !strcmp(&coords[0].ctype[0], "GENE")) {
    *ra  = L + coords[0].crval1;
    *dec = M + coords[0].crval2;
  }

}


RD_to_XY (x, y, ra, dec, coords)
double *x, *y;
double  ra, dec;
Coords coords[];
{

  double tmp_d;
  double X, Y, sphi, cphi, stht;
  double salp, calp, sdel, cdel, sdp, cdp;

  if (!strcmp(&coords[0].ctype[4], "-PLY")) {
    /* fprintf (stderr, "approximate to polynomial TAN plane fit\n");  */
  }

  *x = 0;
  *y = 0;
  if (!strcmp(&coords[0].ctype[0], "GENE") || !strcmp(&coords[0].ctype[4], "-LIN")) {
    X = (ra  - coords[0].crval1);
    Y = (dec - coords[0].crval2);
  }
  
  if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") || !strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
    sdp  = sin(RAD_DEG*coords[0].crval2);
    cdp  = cos(RAD_DEG*coords[0].crval2);
    salp = sin(RAD_DEG*(ra - coords[0].crval1));
    calp = cos(RAD_DEG*(ra - coords[0].crval1));
    sdel = sin(RAD_DEG*dec);
    cdel = cos(RAD_DEG*dec);

    stht = sdel*sdp + cdel*cdp*calp;  /* sin(theta) */
    sphi = cdel*salp;  /* = cos(theta)*sin(phi) */
    cphi = cdel*sdp*calp - sdel*cdp; /* = cos(theta)*cos(phi) */

    if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") ) {
      X =  DEG_RAD * sphi / stht;
      Y = -DEG_RAD * cphi / stht;
    }
    if (!strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
      X =  DEG_RAD * sphi;
      Y = -DEG_RAD * cphi;
    }
    X = X;
    Y = Y;
  }
  
  tmp_d = 1.0 / (coords[0].pc1_1*coords[0].pc2_2 - coords[0].pc1_2*coords[0].pc2_1);
  *x = tmp_d * (coords[0].pc2_2*X - coords[0].pc1_2*Y) / coords[0].cdelt1 + coords[0].crpix1;
  *y = tmp_d * (coords[0].pc1_1*Y - coords[0].pc2_1*X) / coords[0].cdelt2 + coords[0].crpix2;
 
}


fRD_to_XY (x, y, ra, dec, coords)
float *x, *y;
double  ra, dec;
Coords coords[];
{

  double tmp_d;
  double X, Y, sphi, cphi, stht;
  double salp, calp, sdel, cdel, sdp, cdp;

  *x = 0;
  *y = 0;
  if (!strcmp(&coords[0].ctype[0], "GENE") || !strcmp(&coords[0].ctype[4], "-LIN")) {
    X = (ra  - coords[0].crval1);
    Y = (dec - coords[0].crval2);
  }
  
  if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") || !strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
    sdp  = sin(RAD_DEG*coords[0].crval2);
    cdp  = cos(RAD_DEG*coords[0].crval2);
    salp = sin(RAD_DEG*(ra - coords[0].crval1));
    calp = cos(RAD_DEG*(ra - coords[0].crval1));
    sdel = sin(RAD_DEG*dec);
    cdel = cos(RAD_DEG*dec);

    stht = sdel*sdp + cdel*cdp*calp;  /* sin(theta) */
    sphi = cdel*salp;  /* = cos(theta)*sin(phi) */
    cphi = cdel*sdp*calp - sdel*cdp; /* = cos(theta)*cos(phi) */

    if (!strcmp(&coords[0].ctype[4], "-PLY") || !strcmp(&coords[0].ctype[4], "-TAN") ) {
      X =  DEG_RAD * sphi / stht;
      Y = -DEG_RAD * cphi / stht;
    }
    if (!strcmp(&coords[0].ctype[4], "-SIN") || !strcmp(&coords[0].ctype[0], "MM")) {
      X =  DEG_RAD * sphi;
      Y = -DEG_RAD * cphi;
    }
    X = X;
    Y = Y;
  }
  
  tmp_d = 1.0 / (coords[0].pc1_1*coords[0].pc2_2 - coords[0].pc1_2*coords[0].pc2_1);
  *x = tmp_d * (coords[0].pc2_2*X - coords[0].pc1_2*Y) / coords[0].cdelt1 + coords[0].crpix1;
  *y = tmp_d * (coords[0].pc1_1*Y - coords[0].pc2_1*X) / coords[0].cdelt2 + coords[0].crpix2;
 
}



GetCoords (coords, header) 
Coords coords[];
Header header[];
{

  int status;
  double rotate;

  rotate = 0.0;
  coords[0].crval1 = coords[0].crpix1 = coords[0].cdelt1 = 0.0;
  coords[0].crval2 = coords[0].crpix2 = coords[0].cdelt2 = 0.0;
  coords[0].pc1_1 = coords[0].pc2_2 = 1.0;
  coords[0].pc2_1 = coords[0].pc1_2 = 0.0;
  strcpy (coords[0].ctype, "NONE");

  status = TRUE; 
  if (fits_scan (header, "CTYPE2", "%s", 1, coords[0].ctype)) {
    status  = fits_scan (header, "CRVAL1", "%lf", 1, &coords[0].crval1);
    status &= fits_scan (header, "CRPIX1", "%lf", 1, &coords[0].crpix1);
    status &= fits_scan (header, "CRVAL2", "%lf", 1, &coords[0].crval2);  
    status &= fits_scan (header, "CRPIX2", "%lf", 1, &coords[0].crpix2);
    if (fits_scan (header, "CDELT1", "%lf", 1, &coords[0].cdelt1)) {
      status &= fits_scan (header, "CDELT2", "%lf", 1, &coords[0].cdelt2);
      if (fits_scan (header, "CROTA2", "%lf", 1, &rotate)) {
	coords[0].pc1_1 =  cos(rotate*RAD_DEG);
	coords[0].pc1_2 = -sin(rotate*RAD_DEG);
	coords[0].pc2_1 =  sin(rotate*RAD_DEG);
	coords[0].pc2_2 =  cos(rotate*RAD_DEG);
      }
      if (fits_scan (header, "PC001001", "%lf", 1, &coords[0].pc1_1)) {
	status &= fits_scan (header, "PC001002", "%lf", 1, &coords[0].pc1_2);
	status &= fits_scan (header, "PC002001", "%lf", 1, &coords[0].pc2_1);
	status &= fits_scan (header, "PC002002", "%lf", 1, &coords[0].pc2_2);
      }
    }
    else {
      if (fits_scan (header, "CD1_1", "%lf", 1, &coords[0].pc1_1)) {
	status &= fits_scan (header, "CD1_2", "%lf", 1, &coords[0].pc1_2);
	status &= fits_scan (header, "CD2_1", "%lf", 1, &coords[0].pc2_1);
	status &= fits_scan (header, "CD2_2", "%lf", 1, &coords[0].pc2_2);
	coords[0].cdelt1 = coords[0].cdelt2 = 1.0;
      }
      else {
	status = FALSE;
      }
    }
  }
  else {
    if (fits_scan (header, "RA_O", "%lf", 1, &coords[0].crval1)) {
      status  = fits_scan (header, "RA_X", "%lf", 1, &coords[0].pc1_1);
      status &= fits_scan (header, "RA_Y", "%lf", 1, &coords[0].pc1_2);
      status &= fits_scan (header, "DEC_O", "%lf", 1, &coords[0].crval2);  
      status &= fits_scan (header, "DEC_X", "%lf", 1, &coords[0].pc2_1);
      status &= fits_scan (header, "DEC_Y", "%lf", 1, &coords[0].pc2_2);
      coords[0].crpix1 = coords[0].crpix2 = 0.0;
      coords[0].cdelt1 = coords[0].cdelt2 = 1.0;
      strcpy (coords[0].ctype, "GENE");
    }
  }
  if (!status) {
    fprintf (stderr, "error getting all elements for coordinate mode %d\n", coords[0].ctype);
    coords[0].crval1 = coords[0].crpix1 = coords[0].cdelt1 = 0.0;
    coords[0].crval2 = coords[0].crpix2 = coords[0].cdelt2 = 0.0;
    coords[0].pc1_1 = coords[0].pc2_2 = 1.0;
    coords[0].pc2_1 = coords[0].pc1_2 = 0.0;
    strcpy (coords[0].ctype, "NONE");
  }
}


  /* -PLY projection is an extrapolation of the -TAN projection. 
     In addition to the usual linear terms of CRPIXi, PC00i00j, there are 
     higher order polynomial terms (up to 3rd order):
     Axis 1 terms:
     PCA1X2Y0 = coords.polyterm[0][0] = x^2                                             
     PCA1X1Y1 = coords.polyterm[1][0] = xy                                          
     PCA1X0Y2 = coords.polyterm[2][0] = y^2                                             
     PCA1X3Y0 = coords.polyterm[3][0] = x^3                                             
     PCA1X2Y1 = coords.polyterm[4][0] = x^2 y                                             
     PCA1X1Y2 = coords.polyterm[5][0] = x y^2                                             
     PCA1X0Y3 = coords.polyterm[6][0] = y^2                                              
     Axis 2 terms:
     PCA2X2Y0 = coords.polyterm[0][1] = x^2                                               
     PCA2X1Y1 = coords.polyterm[1][1] = xy                                                
     PCA2X0Y2 = coords.polyterm[2][1] = y^2                                               
     PCA2X3Y0 = coords.polyterm[3][1] = x^3                                               
     PCA2X2Y1 = coords.polyterm[4][1] = x^2 y                                             
     PCA2X1Y2 = coords.polyterm[5][1] = x y^2                                             
     PCA2X0Y3 = coords.polyterm[6][1] = y^2                                               
  */
