/******************************************************************************
 * $Id: gdal_nrgcrs.c,v 1.1 2002/12/11 18:21:18 dron Exp $
 *
 * Project:  Mapinfo Image Warper
 * Purpose:  Implemention of the GDALTransformer wrapper around CRS.C functions
 *           to build a polynomial transformation based on ground control 
 *           points.
 * Author:   Frank Warmerdam, warmerdam@pobox.com
 *
 ***************************************************************************

 ***************************************************************************
 *
 * $Log: gdal_nrgcrs.c,v $
 * Revision 1.1  2002/12/11 18:21:18  dron
 * New
 *
 * 
 */

#include "gdal_alg.h"
#include "cpl_conv.h"

extern int TwoDPolyFit( double *, double *, int, int, double *, double *, double * );
extern double TwoDPolyEval( double *, int, double, double );

typedef struct
{
    double adfToGeoX[20];
    double adfToGeoY[20];
    
    double adfFromGeoX[20];
    double adfFromGeoY[20];

    int    nOrder;
    int    bReversed;
    
} GCPTransformInfo;


/************************************************************************/
/*                      GDALCreateGCPTransformer()                      */
/************************************************************************/

/**
 * Create GCP based polynomial transformer.
 *
 * Computes least squares fit polynomials from a provided set of GCPs,
 * and stores the coefficients for later transformation of points between
 * pixel/line and georeferenced coordinates. 
 *
 * The return value should be used as a TransformArg in combination with
 * the transformation function GDALGCPTransform which fits the 
 * GDALTransformerFunc signature.  The returned transform argument should
 * be deallocated with GDALDestroyGCPTransformer when no longer needed.
 *
 * This function may fail (returning NULL) if the provided set of GCPs
 * are inadequate for the requested order, the determinate is zero or they
 * are otherwise "ill conditioned".  
 *
 * Note that 2nd order requires at least 6 GCPs, and 3rd order requires at
 * least 10 gcps.  If nReqOrder is 0 the highest order possible with the
 * provided gcp count will be used.
 *
 * @param nGCPCount the number of GCPs in pasGCPList.
 * @param pasGCPList an array of GCPs to be used as input.
 * @param nReqOrder the requested polynomial order.  It should be 1, 2 or 3.
 * 
 * @return the transform argument or NULL if creation fails. 
 */

void *GDALCreateGCPTransformer( int nGCPCount, const GDAL_GCP *pasGCPList, 
                                int nReqOrder, int bReversed )

{
    GCPTransformInfo *psInfo;
    double *padfGeoX, *padfGeoY, *padfRasterX, *padfRasterY;
    int    *panStatus, iGCP;
    double rms_err;

    if( nReqOrder == 0 )
    {
        if( nGCPCount >= 10 )
            nReqOrder = 3;
        else if( nGCPCount >= 6 )
            nReqOrder = 2;
        else
            nReqOrder = 1;
    }
    
    psInfo = (GCPTransformInfo *) CPLCalloc(sizeof(GCPTransformInfo),1);
    psInfo->bReversed = bReversed;
    psInfo->nOrder = nReqOrder;

/* -------------------------------------------------------------------- */
/*      Allocate and initialize the working points list.                */
/* -------------------------------------------------------------------- */
    padfGeoX = (double *) CPLCalloc(sizeof(double),nGCPCount);
    padfGeoY = (double *) CPLCalloc(sizeof(double),nGCPCount);
    padfRasterX = (double *) CPLCalloc(sizeof(double),nGCPCount);
    padfRasterY = (double *) CPLCalloc(sizeof(double),nGCPCount);
    panStatus = (int *) CPLCalloc(sizeof(int),nGCPCount);
    
    for( iGCP = 0; iGCP < nGCPCount; iGCP++ )
    {
        panStatus[iGCP] = 1;
        padfGeoX[iGCP] = pasGCPList[iGCP].dfGCPX;
        padfGeoY[iGCP] = pasGCPList[iGCP].dfGCPY;
        padfRasterX[iGCP] = pasGCPList[iGCP].dfGCPPixel;
        padfRasterY[iGCP] = pasGCPList[iGCP].dfGCPLine;
    }

/* -------------------------------------------------------------------- */
/*      Compute the forward and reverse polynomials.                    */
/* -------------------------------------------------------------------- */
    if ( TwoDPolyFit( &rms_err, psInfo->adfFromGeoX, nReqOrder, nGCPCount,
		      padfRasterX, padfGeoX, padfGeoY ) < 0 )
    {
        CPLError( CE_Failure, CPLE_AppDefined,
                  "Failed to compute polynomial equations of desired order\n"
                  "for provided control points." );
        goto CleanupAfterError;
    }
    if ( TwoDPolyFit( &rms_err, psInfo->adfFromGeoY, nReqOrder, nGCPCount,
		      padfRasterY, padfGeoX, padfGeoY ) < 0 )
    {
        CPLError( CE_Failure, CPLE_AppDefined,
                  "Failed to compute polynomial equations of desired order\n"
                  "for provided control points." );
        goto CleanupAfterError;
    }
    if ( TwoDPolyFit( &rms_err, psInfo->adfToGeoX, nReqOrder, nGCPCount,
		      padfGeoX, padfRasterX, padfRasterY ) < 0 )
    {
        CPLError( CE_Failure, CPLE_AppDefined,
                  "Failed to compute polynomial equations of desired order\n"
                  "for provided control points." );
        goto CleanupAfterError;
    }
    if ( TwoDPolyFit( &rms_err, psInfo->adfToGeoY, nReqOrder, nGCPCount,
		      padfGeoY, padfRasterX, padfRasterY ) < 0 )
    {
        CPLError( CE_Failure, CPLE_AppDefined,
                  "Failed to compute polynomial equations of desired order\n"
                  "for provided control points." );
        goto CleanupAfterError;
    }

/* -------------------------------------------------------------------- */
/*      Dump residuals.                                                 */
/* -------------------------------------------------------------------- */
    CPLDebug( "GDALCreateGCPTransformer",
	      "Number of GCPs %d, transformation order %d",
	      nGCPCount, psInfo->nOrder );
    
    for( iGCP = 0; iGCP < nGCPCount; iGCP++ )
    {
	double x = pasGCPList[iGCP].dfGCPX;
	double y = pasGCPList[iGCP].dfGCPY;
	double z = pasGCPList[iGCP].dfGCPZ;
	int bSuccess;
	GDALGCPTransform( psInfo, TRUE, 1, &x, &y, &z, &bSuccess );
	CPLDebug( "GDALCreateGCPTransformer",
		  "GCP %d. Residuals: X: %f, Y: %f", iGCP,
		  pasGCPList[iGCP].dfGCPPixel - x, pasGCPList[iGCP].dfGCPLine - y );
    }
    
    return psInfo;

  CleanupAfterError:
    CPLFree( padfGeoX );
    CPLFree( padfGeoY );
    CPLFree( padfRasterX );
    CPLFree( padfRasterX );
    CPLFree( panStatus );
    
    CPLFree( psInfo );
    return NULL;
}

/************************************************************************/
/*                     GDALDestroyGCPTransformer()                      */
/************************************************************************/

/**
 * Destroy GCP transformer.
 *
 * This function is used to destroy information about a GCP based
 * polynomial transformation created with GDALCreateGCPTransformer(). 
 *
 * @param pTransformArg the transform arg previously returned by 
 * GDALCreateGCPTransformer(). 
 */

void GDALDestroyGCPTransformer( void *pTransformArg )

{
    CPLFree( pTransformArg );
}

/************************************************************************/
/*                          GDALGCPTransform()                          */
/************************************************************************/

/**
 * Transforms point based on GCP derived polynomial model.
 *
 * This function matches the GDALTransformerFunc signature, and can be
 * used to transform one or more points from pixel/line coordinates to
 * georeferenced coordinates (SrcToDst) or vice versa (DstToSrc). 
 *
 * @param pTransformArg return value from GDALCreateGCPTransformer(). 
 * @param bDstToSrc TRUE if transformation is from the destination 
 * (georeferenced) coordinates to pixel/line or FALSE when transforming
 * from pixel/line to georeferenced coordinates.
 * @param nPointCount the number of values in the x, y and z arrays.
 * @param x array containing the X values to be transformed.
 * @param y array containing the Y values to be transformed.
 * @param z array containing the Z values to be transformed.
 * @param panSuccess array in which a flag indicating success (TRUE) or
 * failure (FALSE) of the transformation are placed.
 *
 * @return TRUE.
 */

int GDALGCPTransform( void *pTransformArg, int bDstToSrc, 
                      int nPointCount, 
                      double *x, double *y, double *z, 
                      int *panSuccess )

{
    int    i;
    double X, Y;
    GCPTransformInfo *psInfo = (GCPTransformInfo *) pTransformArg;

    if( psInfo->bReversed )
        bDstToSrc = !bDstToSrc;
    
    for( i = 0; i < nPointCount; i++ )
    {
	X = x[i];
	Y = y[i];
        if( bDstToSrc )
        {
            x[i] = TwoDPolyEval( psInfo->adfFromGeoX, psInfo->nOrder, X, Y );
	    y[i] = TwoDPolyEval( psInfo->adfFromGeoY, psInfo->nOrder, X, Y );
        }
        else
        {
            x[i] = TwoDPolyEval( psInfo->adfToGeoX, psInfo->nOrder, X, Y );
	    y[i] = TwoDPolyEval( psInfo->adfToGeoY, psInfo->nOrder, X, Y );
        }
	z[i] = 0;
        panSuccess[i] = TRUE;
    }

    return TRUE;
}

