#include <stdio.h>
#include <string.h>
#include "pslib.h"
#include "stac.h"

#define ORDER 2

psArray *stacReadImages(stacConfig *config)
{
    psArray *filenames = config->inputs;// The file names
    int nFiles = filenames->n;		// The number of input files
    psArray *files = psArrayAlloc(nFiles); // The input files, to be returned

    psTrace("stac.read.images",1,"Reading input images....\n");
    for (int i = 0; i < nFiles; i++) {
	// We only read PHUs --- not mucking around with extensions for now
	psTrace("stac.read.images",2,"Reading input image %s....\n",filenames->data[i]);
	files->data[i] = psImageReadSection(NULL, 0, 0, 0, 0, 0, NULL, 0, filenames->data[i]);
	if (files->data[i] == NULL) {
	    psErrorStackPrint(stderr,"Fatal error: Unable to read %s\n",filenames->data[i]);
	    exit(EXIT_FAILURE);
	}
	// Big assumption: input images are 32-bit.
	if (((psImage*)(files->data[i]))->type.type != PS_TYPE_F32) {
	    psError("stac.read.images", "Image %d is not 32-bit\n",i);
	}
    }
    psTrace("stac.read.images",1,"%d input images read.\n",nFiles);

    return files;
}


psArray *stacReadMaps(stacConfig *config)
{
    psArray *filenames = config->inputs;// The file names
    int nFiles = filenames->n;		// The number of input files
    psArray *maps = psArrayAlloc(nFiles); // The maps, to be returned
    char mapfile[MAXCHAR];		// Filename of map
    
    psTrace("stac.read.maps",1,"Reading maps....\n");
    for (int i = 0; i < nFiles; i++) {
	if (strlen(filenames->data[i]) > MAXCHAR - 4) {
	    psLogMsg("stac.read.maps",PS_LOG_ERROR,"Filename %s is too long.\n",filenames->data[i]);
	    exit(EXIT_FAILURE);
	}
	// Open the file
	sprintf(mapfile,"%s.map",filenames->data[i]);
	FILE *mapfp = fopen(mapfile,"r");
	if (mapfp == NULL) {
	    psLogMsg("stac.read.maps", PS_LOG_ERROR, "Cannot open map file, %s\n",mapfile);
	    exit(EXIT_FAILURE);
	}
	// Read the file
	psTrace("stac.read.maps",5,"Reading map file %s....\n",mapfile);
	psPlaneTransform *map = psPlaneTransformAlloc(ORDER,ORDER);

	// Format is as following:
	// A0  A1
	// B00 B01
	// B10 B11
	//
	// where:
	// x_sky = A0 + B00*x + B10*y
	// y_sky = A1 + B01*x + B11*y

	double a0, a1, b00, b10, b01, b11;

	if (fscanf(mapfp,"%lf %lf %lf %lf %lf %lf", &a0, &a1, &b00, &b01, &b10, &b11) != 6) {
	    psLogMsg("stac.read.maps", PS_LOG_ERROR, "Couldn't read all 6 transformation values from %s\n",
		     mapfile);
	    exit(EXIT_FAILURE);
	}
	fclose(mapfp);
	map->x->coeff[0][0] = a0;
	map->x->coeff[1][0] = b00;
	map->x->coeff[0][1] = b10;
	map->y->coeff[0][0] = a1;
	map->y->coeff[1][0] = b01;
	map->y->coeff[0][1] = b11;

	// Mask out the cross-terms
	map->x->mask[1][1] = 0;
	map->y->coeff[1][1] = 0;
	// Set the cross-terms to zero, just in case...
	map->x->coeff[1][1] = 0.0;
	map->y->coeff[1][1] = 0.0;

	// Plug it into the array
	maps->data[i] = map;

#ifdef TESTING
	// Check psLib's answer is the same as I would get with old version
	double detx = 256.0, dety = 256.0;
	// My code
	double myx = a0 + b00*detx + b10*dety;
	double myy = a1 + b01*detx + b11*dety;
	// psLib
	psPlane *det = psAlloc(sizeof(psPlane));
	det->x = 256.0;
	det->y = 256.0;
	psPlane *pslib = psPlaneTransformApply(NULL, map, det);

	psTrace("stac.read.maps.test",0,"me: %lf,%lf\n",myx,myy);
	psTrace("stac.read.maps.test",0,"ps: %lf,%lf\n",pslib->x,pslib->y);
	psFree(det);
	psFree(pslib);
#endif

    }
    psTrace("stac.read.maps",1,"%d maps read.\n",nFiles);

    return maps;
}
