/* $Id: casu_platesol.c,v 1.3 2015/08/07 13:06:54 jim Exp $
 *
 * This file is part of the CASU Pipeline utilities
 * Copyright (C) 2015 European Southern Observatory
 *
 * 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; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/*
 * $Author: jim $
 * $Date: 2015/08/07 13:06:54 $
 * $Revision: 1.3 $
 * $Name:  $
 */

/* Includes */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <cpl.h>
#include <math.h>

#include "casu_mods.h"
#include "casu_utils.h"
#include "casu_wcsutils.h"
#include "casu_stats.h"

static int casu_plate6(double *xpos, double *ypos, double *xi, double *eta,
                       unsigned char *flag, int nstds, double *a, double *b,
                       double *c, double *d, double *e, double *f);
static int casu_plate4(double *xpos, double *ypos, double *xi, double *eta,
                       unsigned char *flag, int nstds, double *a, double *b,
                       double *c, double *d, double *e, double *f);
static void tidy(double *work, unsigned char *iwork);

/**@{*/

/*---------------------------------------------------------------------------*/
/**
    \ingroup casu_modules
    \brief Work out a WCS for an image

    \par Name:
        casu_platesol
    \par Purpose:
        Work out a WCS for an image
    \par Description:
        Given a matched standards table (a table of astrometric standards 
        matched with objects on an image), work out a world coordinate
        system for the image. A full 6 constant plate solution (two zero
        points, two scales and two rotations) or a more restricted 4 constant
        solution (two zero points, one scale and one rotation) can be fit.
    \par Language:
        C
    \param plist
        The propertylist which represents the FITS header for an input image.
        Must have a rough FITS WCS if the tangent point is going to be 
        repositioned.
    \param tlist
        The propertylist which represents the FITS header for the input 
        catalogue that went into the matched standards table. This is needed
        so that the catalogue header can be updated with the new WCS 
        values. If it is NULL, then obviously no updating with be done
    \param matchedstds
        A matched standards table containing at least the following columns:
        X_coordinate, Y_coordinate, xpredict, ypredict, ra, dec. 
    \param nconst
        The number of plate constants for the fit. The allowed values are
        4 and 6 (4 constants or 6 constants).
    \param shiftan
        If set, then the difference between the predicted and true cartesian
        coordinates are used to redefine the equatorial coordinates of the
        pole of the WCS.
    \param status
        An input/output status that is the same as the returned values below.
    \retval CASU_OK
        if everything is ok
    \retval CASU_FATAL
        if the plate solution has failed, there aren't enough standards or
        the requested number of constants is neither 4 nor 6.
    \par QC headers:
        The following QC keywords are written to the plist propertylist
        - \b WCS_DCRVAL1
            Change in crval1 (degrees)
        - \b WCS_DCRVAL2
            Change in crval2 (degrees)
        - \b WCS_DTHETA
            Change in rotation (degrees)
        - \b WCS_SCALE
            The mean plate scale (arcsec/pixel)
        - \b WCS_SHEAR
            Shear in the WCS solution (degrees)
        - \b WCS_RMS
            Average error in WCS fit in (arcsec)
    \par DRS headers:
        The following DRS keywords are written to the plist propertylist
        - \b NUMBRMS
            The number of stars in the WCS fit
        - \b STDCRMS 
            The average error in the WCS fit (arcsec)
        - \b WCSRAOFF
            Central pixel RA_after - RA_before (arcsec)
        - \b WCSDECOFF
            Central pixel Dec_after - Dec_before (arcsec)
    \par Other headers:
        The input FITS WCS keywords are overwritten by updated values
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

extern int casu_platesol(cpl_propertylist *plist, cpl_propertylist *tlist,
                         cpl_table *matchedstds, int nconst, int shiftan, 
                         int *status) {
    int nstds,nc2,i,niter,nrej,ngood,nreq=6,xcol,ycol,nulval;
    long n1,n2;
    const char *fctid = "casu_platesol";
    float *tptr;
    double *xptr,*yptr,*xptr2,*yptr2,*ra,*dec,*xiptr,*etaptr,*wptr,averr;
    double r1,r2,d1,d2,newcrval1,newcrval2,a,b,c,d,e,f,xifit,etafit,dxi,deta;
    double crpix1,crpix2,xi,eta,rac_before,rac_after,decc_before,decc_after;
    double xcen,ycen,oldtheta,scale,oldcrpix1,oldcrpix2,*dptr;
    double *work=NULL;
    const double *crdata;
    unsigned char *isbad,*wptr2,*iwork=NULL;
    const char *reqcols[] = {"X_coordinate","Y_coordinate","xpredict",
                             "ypredict","RA","Dec"};
    char key[9];
    cpl_wcs *wcs;
    const cpl_array *cr;
    const cpl_matrix *crm;

    /* Inherited status */

    if (*status != CASU_OK)
        return(*status);

    /* Check the value of nconst */

    if (nconst != 4 && nconst != 6) {
        cpl_msg_error(fctid,
                      "Value of nconst = %" CPL_SIZE_FORMAT " is unsupported",
                      (cpl_size)nconst);
        FATAL_ERROR
    }

    /* How many standards are in the input matched standards table? */

    nstds = (int)cpl_table_get_nrow(matchedstds);
    nc2 = nconst/2;
    if (nstds < nc2) {
        cpl_msg_error(fctid,
                      "Too few standards (%" CPL_SIZE_FORMAT ") in table for %" CPL_SIZE_FORMAT " coefficient fit",
                      (cpl_size)nstds,(cpl_size)nconst);
        FATAL_ERROR
    }

    /* Check that the matched standards table has all the required columns */

    for (i = 0; i < nreq; i++) {
        if (cpl_table_has_column(matchedstds,reqcols[i]) != 1) {
            cpl_msg_error(fctid,"Matched standards table missing column %s",
                          reqcols[i]);
            FATAL_ERROR
        }
    }
            
    /* Get some workspace now */

    work = cpl_malloc(10*nstds*sizeof(*work));
    iwork = cpl_calloc(3*nstds,sizeof(*isbad));
    xptr = work;
    yptr = work + nstds;
    xptr2 = work + 2*nstds;
    yptr2 = work + 3*nstds;
    ra = work + 4*nstds;
    dec = work + 5*nstds;
    xiptr = work + 6*nstds;
    etaptr = work + 7*nstds;
    wptr = work + 8*nstds;
    isbad = iwork;
    wptr2 = iwork + nstds;
    
    /* Get the data from the table and put it all into double precision
       arrays */

    tptr = cpl_table_get_data_float(matchedstds,"X_coordinate");
    for (i = 0; i < nstds; i++)
        xptr[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedstds,"Y_coordinate");
    for (i = 0; i < nstds; i++)
        yptr[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedstds,"xpredict");
    for (i = 0; i < nstds; i++)
        xptr2[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedstds,"ypredict");
    for (i = 0; i < nstds; i++)
        yptr2[i] = (double)tptr[i];
    if (cpl_table_get_column_type(matchedstds,"RA") == CPL_TYPE_FLOAT) {
        tptr = cpl_table_get_data_float(matchedstds,"RA");
        for (i = 0; i < nstds; i++)
            ra[i] = (double)tptr[i];
        tptr = cpl_table_get_data_float(matchedstds,"Dec");
        for (i = 0; i < nstds; i++)
            dec[i] = (double)tptr[i];
    } else {
        dptr = cpl_table_get_data_double(matchedstds,"RA");
        for (i = 0; i < nstds; i++)
            ra[i] = dptr[i];
        dptr = cpl_table_get_data_double(matchedstds,"Dec");
        for (i = 0; i < nstds; i++)
            dec[i] = dptr[i];
    }

    /* If you want to shift the RA and Dec of the tangent point, then
       do that now */

    newcrval1 = 0.0;
    newcrval2 = 0.0;
    if (shiftan) {
        wcs = cpl_wcs_new_from_propertylist(plist);
        cr = cpl_wcs_get_crval(wcs);
        crdata = cpl_array_get_data_double_const(cr);
        for (i = 0; i < nstds; i++) {
            casu_xytoradec(wcs,xptr[i],yptr[i],&r1,&d1);
            casu_xytoradec(wcs,xptr2[i],yptr2[i],&r2,&d2);
            xiptr[i] = r2 - r1;
            etaptr[i] = d2 - d1;
        }
        r1 = casu_dmed(xiptr,NULL,nstds);
        d1 = casu_dmed(etaptr,NULL,nstds);
        newcrval1 = crdata[0] + r1;
        newcrval2 = crdata[1] + d1;
        cpl_propertylist_update_double(plist,"CRVAL1",newcrval1);
        cpl_propertylist_update_double(plist,"CRVAL2",newcrval2);
        cpl_wcs_delete(wcs);
    }

    /* Calculate the central RA and Dec */

    wcs = cpl_wcs_new_from_propertylist(plist);
    n1 = cpl_array_get_int(cpl_wcs_get_image_dims(wcs),0,&nulval);
    n2 = cpl_array_get_int(cpl_wcs_get_image_dims(wcs),1,&nulval);
    cr = cpl_wcs_get_crpix(wcs);
    crdata = cpl_array_get_data_double_const(cr);
    oldcrpix1 = crdata[0];
    oldcrpix2 = crdata[1];
    xcen = 0.5*(double)n1;
    ycen = 0.5*(double)n2;
    casu_xytoradec(wcs,xcen,ycen,&rac_before,&decc_before);

    /* Right, calculate xi and eta for each of the standards */

    for (i = 0; i < nstds; i++) {
        casu_radectoxieta(wcs,ra[i],dec[i],&xi,&eta);
        xiptr[i] = xi;
        etaptr[i] = eta;
    }

    /* Right, now loop for maximum number of iterations or until
       convergence */

    niter = 0;
    while (niter >= 0) {

        /* Do a plate solution */

        switch (nconst) {
        case 6:
            *status = casu_plate6(xptr,yptr,xiptr,etaptr,isbad,nstds,&a,&b,
                                  &c,&e,&d,&f);
            break;
        case 4:
            *status = casu_plate4(xptr,yptr,xiptr,etaptr,isbad,nstds,&a,&b,
                                  &c,&e,&d,&f);
            break;
        default:
            *status = casu_plate6(xptr,yptr,xiptr,etaptr,isbad,nstds,&a,&b,
                                  &c,&e,&d,&f);
            break;
        }
        if (*status != CASU_OK) {
            cpl_msg_error(fctid,"Plate constant solution failed");
            tidy(work,iwork);
            FATAL_ERROR
        }

        /* Now look at the residuals and see if any should be rejected */

        for (i = 0; i < nstds; i++) {
            xifit = xptr[i]*a + yptr[i]*b + c;
            etafit = xptr[i]*d + yptr[i]*e + f;
            dxi = fabs(xifit - xiptr[i]);
            deta = fabs(etafit - etaptr[i]);
            wptr[i*2] = dxi;
            wptr[i*2+1] = deta;
            wptr2[i*2] = isbad[i];
            wptr2[i*2+1] = isbad[i];
        }
        averr = casu_dmed(wptr,wptr2,2*nstds);
        averr *= 1.48;
        if (niter == 3)
            break;
        nrej = 0;
        ngood = 0;
        for (i = 0; i < nstds; i++) {
            if (!isbad[i] && (wptr[i*2] > 3.0*averr || wptr[i*2+1] > 3.0*averr))                nrej++;
            if (!isbad[i])
                ngood++;
        }
        ngood -= nrej;
        if (nrej == 0 || ngood < nconst)
            break;
        for (i = 0; i < nstds; i++) {
            if (!isbad[i] && (wptr[i*2] > 3.0*averr || wptr[i*2+1] > 3.0*averr))                isbad[i] = 1;
        }
        niter++;
    }

    /* Convert values to degrees now */

    crpix1 = (e*c - b*f)/(d*b - e*a);
    crpix2 = (a*f - d*c)/(d*b - e*a);
    a *= DEGRAD;
    b *= DEGRAD;
    d *= DEGRAD;
    e *= DEGRAD;

    /* Number of good points fit and average error in arcsec*/

    ngood = 0;
    for (i = 0; i < nstds; i++)
        if (! isbad[i])
            ngood++;
    averr *= DEGRAD*3600.0;

    /* Right, now update the header */

    casu_propertylist_update_double(plist,"CRPIX1",crpix1);
    casu_propertylist_update_double(plist,"CRPIX2",crpix2);
    cpl_propertylist_update_double(plist,"CD1_1",a);
    cpl_propertylist_update_double(plist,"CD1_2",b);
    cpl_propertylist_update_double(plist,"CD2_1",d);
    cpl_propertylist_update_double(plist,"CD2_2",e);
    cpl_propertylist_update_int(plist,"ESO DRS NUMBRMS",ngood);
    cpl_propertylist_set_comment(plist,"ESO DRS NUMBRMS",
                                 "Number of stars in WCS fit");
    cpl_propertylist_update_double(plist,"ESO DRS STDCRMS",averr);
    cpl_propertylist_set_comment(plist,"ESO DRS STDCRMS",
                                 "[arcsec] Average error in WCS fit");

    /* Add some extra stuff for PhaseIII compliance */

    cpl_propertylist_update_string(plist,"CUNIT1","deg");
    cpl_propertylist_set_comment(plist,"CUNIT1",
                                 "Unit of coordinate transformation");
    cpl_propertylist_update_string(plist,"CUNIT2","deg");
    cpl_propertylist_set_comment(plist,"CUNIT2",
                                 "Unit of coordinate transformation");
    cpl_propertylist_update_double(plist,"CSYER1",0.0);
    cpl_propertylist_set_comment(plist,"CSYER1","Systematic error axis 1");
    cpl_propertylist_update_double(plist,"CSYER2",0.0);
    cpl_propertylist_set_comment(plist,"CSYER2","Systematic error axis 2");
    cpl_propertylist_update_double(plist,"CRDER1",averr);
    cpl_propertylist_set_comment(plist,"CRDER1","Random error axis 1");
    cpl_propertylist_update_double(plist,"CRDER2",averr);
    cpl_propertylist_set_comment(plist,"CRDER2","Random error axis 2");
    cpl_propertylist_update_string(plist,"RADECSYS","ICRS");
    cpl_propertylist_set_comment(plist,"RADECSYS","Coordinate reference frame");
    cpl_propertylist_update_double(plist,"EQUINOX",2000.0);
    cpl_propertylist_set_comment(plist,"EQUINOX","Coordinate equinox");

    /* Calculate the central RA and Dec again */

    crm = cpl_wcs_get_cd(wcs);
    crdata = cpl_matrix_get_data_const(crm);
    oldtheta = 0.5*(fabs(atan2(crdata[1],crdata[0])) + 
                    fabs(atan2(crdata[2],crdata[3])));
    cpl_wcs_delete(wcs);
    wcs = cpl_wcs_new_from_propertylist(plist);
    casu_xytoradec(wcs,xcen,ycen,&rac_after,&decc_after);
    xcen = 3600.0*(rac_after - rac_before);
    ycen = 3600.0*(decc_after - decc_before);
    cpl_propertylist_update_double(plist,"ESO DRS WCSRAOFF",xcen);
    cpl_propertylist_set_comment(plist,"ESO DRS WCSRAOFF",
                                 "[arcsec] cenpix RA_after - RA_before)");
    cpl_propertylist_update_double(plist,"ESO DRS WCSDECOFF",ycen);
    cpl_propertylist_set_comment(plist,"ESO DRS WCSDECOFF",
                                 "[arcsec] cenpix Dec_after - Dec_before)");

    /* Update the table header */

    if (tlist != NULL) {
        xcol = casu_findcol(tlist,"X");
        ycol = casu_findcol(tlist,"Y");
        if (xcol != -1 && ycol != -1) {
            snprintf(key,9,"TCRPX%d",xcol);
            casu_propertylist_update_double(tlist,key,crpix1);
            snprintf(key,9,"TCRPX%d",ycol);
            casu_propertylist_update_double(tlist,key,crpix2);
            if (shiftan) {
                snprintf(key,9,"TCRVL%d",xcol);
                cpl_propertylist_update_double(tlist,key,newcrval1);
                snprintf(key,9,"TCRVL%d",ycol);
                cpl_propertylist_update_double(tlist,key,newcrval2);
            }
            snprintf(key,9,"TC%d_%d",xcol,xcol);
            cpl_propertylist_update_double(tlist,key,a);
            snprintf(key,9,"TC%d_%d",xcol,ycol);
            cpl_propertylist_update_double(tlist,key,b);
            snprintf(key,9,"TC%d_%d",ycol,xcol);
            cpl_propertylist_update_double(tlist,key,d);
            snprintf(key,9,"TC%d_%d",ycol,ycol);
            cpl_propertylist_update_double(tlist,key,e);
            cpl_propertylist_update_int(tlist,"ESO DRS NUMBRMS",ngood);
            cpl_propertylist_set_comment(tlist,"ESO DRS NUMBRMS",
                                         "Number of stars in WCS fit");
            cpl_propertylist_update_double(tlist,"ESO DRS STDCRMS",averr);
            cpl_propertylist_set_comment(tlist,"ESO DRS STDCRMS",
                                         "[arcsec] Average error in WCS fit");
            cpl_propertylist_update_double(tlist,"ESO DRS WCSRAOFF",xcen);
            cpl_propertylist_set_comment(tlist,"ESO DRS WCSRAOFF",
                                         "[arcsec] cenpix RA_after - RA_before)");
            cpl_propertylist_update_double(tlist,"ESO DRS WCSDECOFF",ycen);
            cpl_propertylist_set_comment(tlist,"ESO DRS WCSDECOFF",
                                         "[arcsec] cenpix Dec_after - Dec_before)");
            
            /* PhaseIII stuff for table headers */
            
            snprintf(key,9,"TCUNI%d",xcol);
            cpl_propertylist_update_string(tlist,key,"deg");
            cpl_propertylist_set_comment(tlist,key,
                                         "Unit of coordinate transformation");
            snprintf(key,9,"TCUNI%d",ycol);
            cpl_propertylist_update_string(tlist,key,"deg");
            cpl_propertylist_set_comment(tlist,key,
                                         "Unit of coordinate transformation");
            snprintf(key,9,"TCSY%d",xcol);
            cpl_propertylist_update_double(tlist,key,0.0);
            cpl_propertylist_set_comment(tlist,key,"Systematic error x-axis ");
            snprintf(key,9,"TCSY%d",ycol);
            cpl_propertylist_update_double(tlist,key,0.0);
            cpl_propertylist_set_comment(tlist,key,"Systematic error y-axis");
            snprintf(key,9,"TCRD%d",xcol);
            cpl_propertylist_update_double(tlist,key,averr);
            cpl_propertylist_set_comment(tlist,key,"Random error axis 1");
            snprintf(key,9,"TCRD%d",ycol);
            cpl_propertylist_update_double(tlist,key,averr);
            cpl_propertylist_set_comment(tlist,key,"Random error axis 2");
            cpl_propertylist_update_string(tlist,"RADECSYS","ICRS");
            cpl_propertylist_set_comment(tlist,"RADECSYS",
                                         "Coordinate reference frame");
            cpl_propertylist_update_double(tlist,"EQUINOX",2000.0);
            cpl_propertylist_set_comment(tlist,"EQUINOX","Coordinate equinox");
        }
    }

    /* Back-calculate a crval for the old value of crpix. Compare to the 
       WCS crval and write to QC header */

    cr = cpl_wcs_get_crval(wcs);
    crdata = cpl_array_get_data_double_const(cr);
    casu_xytoradec(wcs,oldcrpix1,oldcrpix2,&rac_after,&decc_after);
    rac_after -= crdata[0];
    decc_after -= crdata[1];
    cpl_propertylist_update_double(plist,"ESO QC WCS_DCRVAL1",rac_after);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_DCRVAL1",
                                 "[deg] change in crval1");
    cpl_propertylist_update_double(plist,"ESO QC WCS_DCRVAL2",decc_after);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_DCRVAL2",
                                 "[deg] change in crval2");

    /* Work out the change in the rotation */

    crm = cpl_wcs_get_cd(wcs);
    crdata = cpl_matrix_get_data_const(crm);
    oldtheta = 0.5*(fabs(atan2(crdata[1],crdata[0])) + 
                    fabs(atan2(crdata[2],crdata[3]))) - oldtheta;
    oldtheta *= DEGRAD;
    cpl_propertylist_update_double(plist,"ESO QC WCS_DTHETA",oldtheta);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_DTHETA",
                                 "[deg] change in rotation");
    
    /* Work out the mean plate scale */

    scale = 1800.0*(sqrt(pow(crdata[0],2.0) +  pow(crdata[1],2.0)) + 
                    sqrt(pow(crdata[2],2.0) +  pow(crdata[3],2.0)));
    cpl_propertylist_update_double(plist,"ESO QC WCS_SCALE",scale);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_SCALE",
                                 "[arcsec] mean plate scale");

    /* Work out the shear of this new WCS */

    oldtheta = fabs(atan2(crdata[1],crdata[0])) - 
        fabs(atan2(crdata[2],crdata[3]));
    cpl_propertylist_update_double(plist,"ESO QC WCS_SHEAR",oldtheta);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_SHEAR",
                                 "[deg] abs(xrot) - abs(yrot)");

    /* Now just add in the RMS */

    cpl_propertylist_update_double(plist,"ESO QC WCS_RMS",averr);
    cpl_propertylist_set_comment(plist,"ESO QC WCS_RMS",
                                 "[arcsec] Average error in WCS fit");

    /* Right, get out of here now... */

    cpl_wcs_delete(wcs);
    tidy(work,iwork);
    GOOD_STATUS
}

/*---------------------------------------------------------------------------*/
/**
    \brief Work out an xy transformation for two coodinate lists
    \ingroup casu_modules

    \par Name:
        casu_platexy
    \par Purpose:
        Work out an xy transformation for two coordinate lists
    \par Description:
        Given a matched xy table (a table of objects with two sets of xy
        coordinates), work out a linear transformation between the coordinate
        sets. A full 6 constant plate solution (two zero points, two scales 
        and two rotations) or a more restricted 4 constant solution (two 
        zero points, one scale and one rotation) can be fit.
    \par Language:
        C
    \param matchedxy
        A matchedxy table. Must contain the following columns: X_coordinate_1,
        Y_coordinate_1, X_coordinate_2, Y_coordinate_2. The transformation 
        will be done in the sense of transforming from frame 2 to frame 1.
    \param nconst
        The number of plate constants for the fit. The allowed values are
        4 and 6 (4 constants or 6 constants).
    \param coefs
        An array of coefs.
    \param status
        An input/output status that is the same as the returned values below.
    \retval CASU_OK
        if everything is ok
    \retval CASU_FATAL
        if the plate solution has failed, there aren't enough standards or
        the requested number of constants is neither 4 nor 6.
    \par QC headers:
        None
    \par DRS headers:
        None
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

extern int casu_platexy(cpl_table *matchedxy, int nconst, cpl_array **coefs, 
                        int *status) {
    const char *fctid = "casu_platexy";
    int nxy,nc2,i,niter,nrej,ngood;
    unsigned char *isbad,*wptr2,*iwork=NULL;
    double *xptr1,*xptr2,*yptr1,*yptr2,*wptr,a,b,c,d,e,f,*cc,xfit,yfit;
    double dx,dy,averr,*work=NULL;
    float *tptr;
    int nreq = 4;
    const char *reqcols[] = {"X_coordinate_1","Y_coordinate_1","X_coordinate_2",
                             "Y_coordinate_2"};

    /* Inherited status */

    *coefs = NULL;
    if (*status != CASU_OK)
        return(*status);

    /* Check the value of nconst */

    if (nconst != 4 && nconst != 6) {
        cpl_msg_error(fctid,
                      "Value of nconst = %" CPL_SIZE_FORMAT " is unsupported",
                      (cpl_size)nconst);
        FATAL_ERROR
    }

    /* How many objects are in the input table? */

    nxy = (int)cpl_table_get_nrow(matchedxy);
    nc2 = nconst/2;
    if (nxy < nc2) {
        cpl_msg_error(fctid,
                      "Too few objects (%" CPL_SIZE_FORMAT ") in table for %" CPL_SIZE_FORMAT " coefficient fit",
                      (cpl_size)nxy,(cpl_size)nconst);
        FATAL_ERROR
    }

    /* Check that the matched standards table has all the required columns */

    for (i = 0; i < nreq; i++) {
        if (cpl_table_has_column(matchedxy,reqcols[i]) != 1) {
            cpl_msg_error(fctid,"Input table missing column %s\n",reqcols[i]);
            FATAL_ERROR
        }
    }
            
    /* Get some workspace now */

    work = cpl_malloc(6*nxy*sizeof(*work));
    iwork = cpl_calloc(3*nxy,sizeof(*isbad));
    xptr1 = work;
    yptr1 = work + nxy;
    xptr2 = work + 2*nxy;
    yptr2 = work + 3*nxy;
    wptr = work + 4*nxy;
    isbad = iwork;
    wptr2 = iwork + nxy;
    
    /* Get the data from the table and put it all into double precision
       arrays */

    tptr = cpl_table_get_data_float(matchedxy,"X_coordinate_1");
    for (i = 0; i < nxy; i++)
        xptr1[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedxy,"Y_coordinate_1");
    for (i = 0; i < nxy; i++)
        yptr1[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedxy,"X_coordinate_2");
    for (i = 0; i < nxy; i++)
        xptr2[i] = (double)tptr[i];
    tptr = cpl_table_get_data_float(matchedxy,"Y_coordinate_2");
    for (i = 0; i < nxy; i++)
        yptr2[i] = (double)tptr[i];

    /* Right, now loop for maximum number of iterations or until
       convergence */

    niter = 0;
    while (niter >= 0) {

        /* Do a plate solution */

        switch (nconst) {
        case 6:
            *status = casu_plate6(xptr2,yptr2,xptr1,yptr1,isbad,nxy,&a,&b,
                                  &c,&e,&d,&f);
            break;
        case 4:
            *status = casu_plate4(xptr2,yptr2,xptr1,yptr1,isbad,nxy,&a,&b,
                                  &c,&e,&d,&f);
            break;
        default:
            *status = casu_plate6(xptr2,yptr2,xptr1,yptr1,isbad,nxy,&a,&b,
                                  &c,&e,&d,&f);
            break;
        }
        if (*status != CASU_OK) {
            cpl_msg_error(fctid,"Plate constant solution failed");
            tidy(work,iwork);
            FATAL_ERROR
        }

        /* Now look at the residuals and see if any should be rejected */

        for (i = 0; i < nxy; i++) {
            xfit = xptr2[i]*a + yptr2[i]*b + c;
            yfit = xptr2[i]*d + yptr2[i]*e + f;
            dx = fabs(xfit - xptr1[i]);
            dy = fabs(yfit - yptr1[i]);
            wptr[i*2] = dx;
            wptr[i*2+1] = dy;
            wptr2[i*2] = isbad[i];
            wptr2[i*2+1] = isbad[i];
        }
        averr = casu_dmed(wptr,wptr2,2*nxy);
        averr *= 1.48;
        if (niter == 3)
            break;
        nrej = 0;
        ngood = 0;
        for (i = 0; i < nxy; i++) {
            if (!isbad[i] && (wptr[i*2] > 3.0*averr || wptr[i*2+1] > 3.0*averr))                nrej++;
            if (!isbad[i])
                ngood++;
        }
        ngood -= nrej;
        if (nrej == 0 || ngood < nconst)
            break;
        for (i = 0; i < nxy; i++) {
            if (!isbad[i] && (wptr[i*2] > 3.0*averr || wptr[i*2+1] > 3.0*averr))                isbad[i] = 1;
        }
        niter++;
    }

    /* Set the output array */

    *coefs = cpl_array_new(6,CPL_TYPE_DOUBLE);
    cc = cpl_array_get_data_double(*coefs);
    cc[0] = a;
    cc[1] = b;
    cc[2] = c;
    cc[3] = d;
    cc[4] = e;
    cc[5] = f;

    /* Right, get out of here now... */

    tidy(work,iwork);
    GOOD_STATUS
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        casu_plate6
    \par Purpose:
        Fit a 6 constant WCS to standard star information
    \par Description:
        A standard 6 constant WCS is fit to standard star equatorial and
        pixel position. The equations to model the solution are:
        xi = ax + by + c and eta = dx + ey + f.
    \par Language:
        C
    \param xpos
        The X positions of the standards
    \param ypos
        The Y positions of the standards
    \param xi
        The xi standard coordinates of the standards
    \param eta
        The eta standard coordinates of the standards
    \param flag
        A array of flags. If an array element is set, then that element's
        standard is remvoed from the analysis.
    \param nstds
        The number of standard stars.
    \param a,b,c,d,e,f
        The fit coefficients
    \retval CASU_OK
        if everything is ok
    \retval CASU_FATAL
        if there aren't enough standards
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static int casu_plate6(double *xpos, double *ypos, double *xi, double *eta,
                       unsigned char *flag, int nstds, double *a, double *b,
                       double *c, double *d, double *e, double *f) {
    double sx1sq,sy1sq,sx1y1,sx1x2,sy1x2;
    double sy1y2,sx1y2,xposmean,yposmean,ximean,etamean,xx1,yy1,xx2,yy2;
    int i,ngood,nbad;

    /* Is it worthwhile even being here? */

    (void)casu_sumbpm(flag,(long)nstds,&nbad);
    ngood = nstds - nbad;
    if (ngood < 2)
        return(CASU_FATAL);

    /* Initialise all the counters and summations */

    sx1sq = 0.0;
    sy1sq = 0.0;
    sx1y1 = 0.0;
    sx1x2 = 0.0;
    sy1x2 = 0.0;
    sy1y2 = 0.0;
    sx1y2 = 0.0;

    /* Find means in each coordinate system */

    xposmean = casu_dmean(xpos,flag,nstds);
    yposmean = casu_dmean(ypos,flag,nstds);
    ximean = casu_dmean(xi,flag,nstds);
    etamean = casu_dmean(eta,flag,nstds);

    /* Now accumulate the sums */

    for (i = 0; i < nstds; i++) {
        if (!flag[i]) {
            xx1 = xpos[i] - xposmean;
            yy1 = ypos[i] - yposmean;
            xx2 = xi[i] - ximean;
            yy2 = eta[i] - etamean;
            sx1sq += xx1*xx1;
            sy1sq += yy1*yy1;
            sx1y1 += xx1*yy1;
            sx1x2 += xx1*xx2;
            sy1x2 += yy1*xx2;
            sy1y2 += yy1*yy2;
            sx1y2 += xx1*yy2;
        }
    }

    /* Do solution for X */

    *a = (sx1y1*sy1x2 - sx1x2*sy1sq)/(sx1y1*sx1y1 - sx1sq*sy1sq);
    *b = (sx1x2*sx1y1 - sx1sq*sy1x2)/(sx1y1*sx1y1 - sx1sq*sy1sq);
    *c = -xposmean*(*a) - yposmean*(*b) + ximean;

    /* Now the solution for Y */

    *d = (sx1y1*sx1y2 - sy1y2*sx1sq)/(sx1y1*sx1y1 - sy1sq*sx1sq);
    *e = (sy1y2*sx1y1 - sy1sq*sx1y2)/(sx1y1*sx1y1 - sy1sq*sx1sq);
    *f = -xposmean*(*e) - yposmean*(*d) + etamean;

    /* Get outta here */

    return(CASU_OK);
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        casu_plate4
    \par Purpose:
        Fit a 4 constant WCS to standard star information
    \par Description:
        A standard 4 constant WCS is fit to standard star equatorial and
        pixel position. The equations to model the solution are:
        xi = ax + by + c and eta = dx + ey + f where a = e and b = d
    \par Language:
        C
    \param xpos
        The X positions of the standards
    \param ypos
        The Y positions of the standards
    \param xi
        The xi standard coordinates of the standards
    \param eta
        The eta standard coordinates of the standards
    \param flag
        A array of flags. If an array element is set, then that element's
        standard is remvoed from the analysis.
    \param nstds
        The number of standard stars.
    \param a,b,c,d,e,f
        The fit coefficients
    \retval CASU_OK
        if everything is ok
    \retval CASU_FATAL
        if there aren't enough standards
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static int casu_plate4(double *xpos, double *ypos, double *xi, double *eta,
                       unsigned char *flag, int nstds, double *a, double *b,
                       double *c, double *d, double *e, double *f) {
    double sx1sq,sy1sq,sx1x2,sy1x2,sy1y2,sx1y2,xposmean,yposmean;
    double ximean,etamean,xx1,yy1,xx2,yy2,det,num,denom,theta,mag;
    double stheta,ctheta;
    int i,ngood,nbad;

    /* Is it worthwhile even being here? */

    (void)casu_sumbpm(flag,(long)nstds,&nbad);
    ngood = nstds - nbad;
    if (ngood < 2)
        return(CASU_FATAL);

    /* Initialise all the counters and summations */

    sx1sq = 0.0;
    sy1sq = 0.0;
    sx1x2 = 0.0;
    sy1x2 = 0.0;
    sy1y2 = 0.0;
    sx1y2 = 0.0;

    /* Find means in each coordinate system */

    xposmean = casu_dmean(xpos,flag,nstds);
    yposmean = casu_dmean(ypos,flag,nstds);
    ximean = casu_dmean(xi,flag,nstds);
    etamean = casu_dmean(eta,flag,nstds);

    /* Now accumulate the sums */

    for (i = 0; i < nstds; i++) {
        if (!flag[i]) {
            xx1 = xpos[i] - xposmean;
            yy1 = ypos[i] - yposmean;
            xx2 = xi[i] - ximean;
            yy2 = eta[i] - etamean;
            sx1sq += xx1*xx1;
            sy1sq += yy1*yy1;
            sx1x2 += xx1*xx2;
            sy1x2 += yy1*xx2;
            sy1y2 += yy1*yy2;
            sx1y2 += xx1*yy2;
        }
    }

    /* Compute the rotation angle */

    det = sx1x2*sy1y2 - sy1x2*sx1y2;
    if (det < 0.0) {
        num = sy1x2 + sx1y2;
        denom = -sx1x2 + sy1y2;
    } else {
        num = sy1x2 - sx1y2;
        denom = sx1x2 + sy1y2;
    }
    if (num == 0.0 && denom == 0.0) {
        theta = 0.0;
    } else {
        theta = atan2(num,denom);
        if (theta < 0.0)
            theta += CPL_MATH_2PI;
    }

    /* Compute magnification factor */

    ctheta = cos(theta);
    stheta = sin(theta);
    num = denom*ctheta  + num*stheta;
    denom = sx1sq + sy1sq;
    if (denom <= 0.0) {
        mag = 1.0;
    } else {
        mag = num/denom;
    }

    /* Compute coeffs */

    if (det < 0.0) {
        *a = -mag*ctheta;
        *e = mag*stheta;
    } else {
        *a = mag*ctheta;
        *e = -mag*stheta;
    }
    *b = mag*stheta;
    *d = mag*ctheta;
    *c = -xposmean*(*a) - yposmean*(*b) + ximean;
    *f = -xposmean*(*e) - yposmean*(*d) + etamean;

    /* Get outta here */

    return(CASU_OK);
}

static void tidy(double *work, unsigned char *iwork) {
    if(work != NULL) {cpl_free(work);}
    //freespace(work);
    if(iwork != NULL) {cpl_free(iwork);}
    //freespace(iwork);
}

/**@}*/


/*

$Log: casu_platesol.c,v $
Revision 1.3  2015/08/07 13:06:54  jim
Fixed copyright to ESO

Revision 1.2  2015/08/06 05:34:02  jim
Fixes to get rid of compiler moans

Revision 1.1.1.1  2015/06/12 10:44:32  jim
Initial import

Revision 1.8  2015/05/13 11:46:30  jim
Now used casu_propertylist_update_double to get around datatype mismatches
in propertylists

Revision 1.7  2015/01/29 11:56:27  jim
modified comments

Revision 1.6  2015/01/09 12:13:15  jim
*** empty log message ***

Revision 1.5  2014/12/11 12:23:33  jim
new version

Revision 1.4  2014/04/26 18:39:45  jim
uses new version of casu_sumbpm

Revision 1.3  2014/03/26 15:56:46  jim
Removed globals

Revision 1.2  2013/11/21 09:38:14  jim
detabbed

Revision 1.1.1.1  2013-08-27 12:07:48  jim
Imported


*/
