# include "psastroStandAlone.h"
# define NONLIN_TOL 0.001 /* tolerance in pixels */

bool psastroModelAdjust (pmConfig *config) {

    bool status;

    // select the current recipe
    psMetadata *recipe  = psMetadataLookupPtr (&status, config->recipes, PSASTRO_RECIPE);
    if (!recipe) {
	psError(PSASTRO_ERR_CONFIG, true, "Can't find PSASTRO recipe");
	return false;
    }

    pmFPAfile *output = psMetadataLookupPtr (&status, config->files, "PSASTRO.OUT.MODEL");
    if (!status) psAbort ("Can't find output pmFPAfile PSASTRO.OUT.MODEL");
    if (!output->fpa) psAbort ("no existing input fpa contains the reference chip");

    // physical pixel scale in microns per pixel
    char *refChipName = psMetadataLookupStr (&status, recipe, "PSASTRO.MODEL.REF.CHIP");
    if (!refChipName) {
	psError(PS_ERR_IO, true, "reference chip is missing from recipe"); 
	return false; 
    } 

    // get reference chip from name
    pmChip *refChip = pmConceptsChipFromName (output->fpa, refChipName);
    if (!refChip) psAbort ("invalid chip name for reference");
    if (!refChip->toFPA) psAbort ("invalid astrometry for reference chip");

    psPlane *boreCH = psPlaneAlloc();
    psPlane *boreFP = psPlaneAlloc();    
    psPlane *boreTP = psPlaneAlloc();    
    psSphere *boreSky = psSphereAlloc();    

    // correct Xo,Yo to Lo,Mo using the ref chip toFPA
    // ref chip position of the true boresite center
    boreCH->x = psMetadataLookupF32 (&status, output->fpa->concepts, "FPA.BORE.X0"); 
    boreCH->y = psMetadataLookupF32 (&status, output->fpa->concepts, "FPA.BORE.Y0"); 
    psPlaneTransformApply (boreFP, refChip->toFPA, boreCH);

    // adjust the reference pixel for all chips
    for (int i = 0; i < output->fpa->chips->n; i++) {
	pmChip *chip = output->fpa->chips->data[i];
	if (!chip->fromFPA) continue;
	// skip the chips without astrometry

	psPlaneTransform *fromFPA = psPlaneTransformSetCenter (NULL, chip->fromFPA, boreFP->x, boreFP->y);
	psFree (chip->fromFPA);
	chip->fromFPA = fromFPA;

	// invert the new fromFPA transform to get the new toFPA transform
	psRegion *region = pmChipPixels (chip);
	psFree (chip->toFPA);
	chip->toFPA = psPlaneTransformInvert(NULL, chip->fromFPA, *region, 50);
	psFree (region);
    }

    // XXX we have now shifted the (0,0) pixel of the focal plane to the true boresite from the
    // reported boresite.  At this point, we need to shift the tangent plane to use the new
    // center as well.  if the toTPA transform were not linear, we would need to modify fromFPA
    // to yield 0,0 at the new boresite location (ie, find Po,Qo = toTPA(Lo,Mo), probably could modify the
    // toTPA/fromTPA transforms to use the new ref pixel, but this would only give us a tranf

    // save the old (L,M) = (0,0) TP coordinate
    float Po = output->fpa->toTPA->x->coeff[0][0];
    float Qo = output->fpa->toTPA->y->coeff[0][0];

    // the new toTPA yields the same TP coordinates for FP coordinates offset by Lo,Mo
    psPlaneTransform *toTPA = psPlaneTransformSetCenter (NULL, output->fpa->toTPA, boreFP->x, boreFP->y);
    psFree (output->fpa->toTPA);
    output->fpa->toTPA = toTPA;
    
    // we are going to shift P,Q to have toTPA(0,0) = Po, Qo.  
    // find the sky coordinates of the 0,0 pixel for the new frame
    boreTP->x = output->fpa->toTPA->x->coeff[0][0] - Po;
    boreTP->y = output->fpa->toTPA->y->coeff[0][0] - Qo;
    psDeproject (boreSky, boreTP, output->fpa->toSky); // find the RA,DEC coord of the focal-plane coordinate

    // adjust the new TP frame to yield the same old (L,M) = 0,0 TP coordinate:
    output->fpa->toTPA->x->coeff[0][0] = Po;
    output->fpa->toTPA->y->coeff[0][0] = Qo;

    // set the projection to use the new (P,Q) = (0,0) position
    output->fpa->toSky->R = boreSky->r;
    output->fpa->toSky->D = boreSky->d;

    psRegion *fpaRegion = pmAstromFPAExtent (output->fpa);

    psFree (output->fpa->fromTPA);
    output->fpa->fromTPA = psPlaneTransformInvert(NULL, output->fpa->toTPA, *fpaRegion, 50);

    // rotate the chip to FPA transforms to have 0.0 posangle; 
    // compensate by rotating fpa to TPA transforms

    // get the current posangle of the ref chip
    float posangle = atan2 (refChip->toFPA->y->coeff[1][0], refChip->toFPA->x->coeff[1][0]);
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.POSANGLE", PS_META_REPLACE, "boresite parameter", posangle);

    // rotate the chip transforms
    for (int i = 0; i < output->fpa->chips->n; i++) {
	pmChip *chip = output->fpa->chips->data[i];
	if (!chip->toFPA) continue;
	// skip chips without astrometry

	psPlaneTransform *toFPA = psPlaneTransformRotate (NULL, chip->toFPA, posangle);
	psFree (chip->toFPA);
	chip->toFPA = toFPA;

	// invert the new fromFPA transform to get the new toFPA transform
	psRegion *region = pmChipPixels (chip);
	psFree (chip->fromFPA);
	chip->fromFPA = psPlaneTransformInvert(NULL, chip->toFPA, *region, 50);
	psFree (region);

	// XXX temporary output dump
	psMetadata *header = chip->hdu->header;
	// XXX to make the output single-level, this needs to be in a loop *after* the fromTPA rotation below
	// pmAstromWriteWCS (header, output->fpa, chip, NONLIN_TOL);
	pmAstromWriteBilevelChip (header, chip, NONLIN_TOL);
    }

    psPlaneTransform *fromTPA = psPlaneTransformRotate (NULL, output->fpa->fromTPA, posangle);
    psFree (output->fpa->fromTPA);
    output->fpa->fromTPA = fromTPA;

    psFree (output->fpa->toTPA);
    output->fpa->toTPA = psPlaneTransformInvert(NULL, output->fpa->fromTPA, *fpaRegion, 50);

    psMetadata *header = output->fpa->hdu->header;
    pmAstromWriteBilevelMosaic (header, output->fpa, NONLIN_TOL);

    psFree (fpaRegion);

    return true;
}
