# include "psastroStandAlone.h"

bool psastroModelAnalysis (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");

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

    /* model analysis:
     *
     * determine POS_ZERO via comparison of measured and reported posangles
     * POS_ZERO = FPA.POSANGLE - posangle
     *
     * determine boresite model:
     * X = Xo + R_X cos(FPA.POSANGLE - T_0) cos(P_0) + R_Y sin(FPA.POSANGLE - T_0) sin(P_0) 
     * Y = Yo + R_Y sin(FPA.POSANGLE - T_0) cos(P_0) - R_X cos(FPA.POSANGLE - T_0) sin(P_0) 
     * position of reported boresite in reference chip pixels
     * Xo, Yo : true coordinate of boresite (rotator center) in reference chip pixels
     * R_X, R_Y : amplitude of boresite offset
     * T_0 : reference angle for rotator
     * P_0 : orientation of boresite ellipse
     */

    // select the input pmFPAfile pointers
    psMetadataItem *item = psMetadataLookup (config->files, "PSASTRO.WCS");
    if (item == NULL) psAbort("missing PSASTRO.WCS entries in config->files");
    if (item->type != PS_DATA_METADATA_MULTI) psAbort("unexpected type for PSASTRO.WCS");
    psArray *files = psListToArray (item->data.list);

    // data storage vectors for measurements
    psVector *posZero  = psVectorAlloc (files->n, PS_TYPE_F32);
    psVector *Po       = psVectorAlloc (files->n, PS_TYPE_F32);
    psVector *Xo       = psVectorAlloc (files->n, PS_TYPE_F32);
    psVector *Yo       = psVectorAlloc (files->n, PS_TYPE_F32);

    // counter for accepted measured values
    int n = 0;

    // Re-select the output fpa.  The output fpa needs to have valid astrometry for the
    // refChip.  output->fpa is a copy of the pointer to one of the input->fpa, but the choice
    // is arbitrary.  select a new one that has an existing ref chip
    psFree (output->fpa);
    output->fpa = NULL;

    // extract the relevant measured and reported values from the reference chip
    for (int i = 0; i < files->n; i++) {
	psMetadataItem *file = files->data[i];
	pmFPAfile *input = file->data.V;

	// reported rotator position angle (this should perhaps be ROT, not POS)
	double POSANGLE = psMetadataLookupF64 (&status, input->fpa->concepts, "FPA.POSANGLE"); 
	if (!status) psAbort ("missing FPA.POSANGLE");

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

	fprintf (stderr, "input %d : %zx : %zx : %zx\n", i, (size_t) input, (size_t) chip, (size_t) chip->toFPA);
	if (!chip->toFPA) continue;

	if (!output->fpa) {
	    // this one matches
	    output->fpa = psMemIncrRefCounter(input->fpa);
	}

	// we have two measurements of the posangle (may be parity flipped to different quadrants)
	float posangle = PM_DEG_RAD * atan2 (chip->toFPA->y->coeff[1][0], chip->toFPA->x->coeff[1][0]);
	posZero->data.F32[n] = POSANGLE - posangle;
	while (posZero->data.F32[n] > 360.0) posZero->data.F32[n] -= 360.0;
	while (posZero->data.F32[n] <   0.0) posZero->data.F32[n] += 360.0;

	Po->data.F32[n] = POSANGLE * PM_RAD_DEG; // reported position angle
	Xo->data.F32[n] = chip->fromFPA->x->coeff[0][0]; // reported boresite x position in ref chip coordinates
	Yo->data.F32[n] = chip->fromFPA->y->coeff[0][0]; // reported boresite y position in ref chip coordinates
	fprintf (stderr, "%d : %f %f : %f = %f - %f\n", i, Xo->data.F32[n], Yo->data.F32[n], posZero->data.F32[n], POSANGLE, posangle);
	n ++;
    }
      
    Xo->n = n;
    Yo->n = n;
    Po->n = n;
    posZero->n = n;

    psStats *stats = psStatsAlloc (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
    psVectorStats (stats, posZero, NULL, NULL, 0);

    fprintf (stderr, "pos zero %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.POS_ZERO", PS_META_REPLACE, "offset between obs and meas posangle", stats->sampleMedian); 

    psVector *params = psastroModelFitBoresite (Xo, Yo, Po);
    if (params->n != 6) psAbort ("error");

    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.X0", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_X0]); 
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.Y0", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_Y0]); 
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.RX", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_RX]); 
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.RY", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_RY]); 
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.T0", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_T0]); 
    psMetadataAddF32 (output->fpa->concepts, PS_LIST_TAIL, "FPA.BORE.P0", PS_META_REPLACE, "boresite parameter", params->data.F32[PAR_P0]); 
    psMetadataAddStr (output->fpa->concepts, PS_LIST_TAIL, "FPA.REF.CHIP", PS_META_REPLACE, "boresite parameter", refChip);

    return true;
}
