/*
 *  Aptdec
 *  Copyright (c) 2004 by Thierry Leconte (F4DWV)
 *
 *      $Id$
 *
 *   This library is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU Library 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 Library General Public License for more details.
 *
 *   You should have received a copy of the GNU Library General Public
 *   License along with this library; if not, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include <stdio.h>
#include <string.h>
#include <sndfile.h>
#include <math.h>

#include "offsets.h"
#include "messages.h"

#define REGORDER 3
typedef struct {
	double cf[REGORDER + 1];
} rgparam;

static void rgcomp(double x[16], rgparam * rgpr) {
	//const double y[9] = { 0.106, 0.215, 0.324, 0.433, 0.542, 0.652, 0.78, 0.87, 0.0 };
    const double y[9] = { 31.07, 63.02, 94.96, 126.9, 158.86, 191.1, 228.62, 255.0, 0.0 };
    extern void polyreg(const int m, const int n, const double x[], const double y[], double c[]);

    polyreg(REGORDER, 9, x, y, rgpr -> cf);
}

static double rgcal(float x, rgparam * rgpr) {
    double y, p;
    int i;

    for (i = 0, y = 0.0, p = 1.0; i < REGORDER + 1; i++) {
		y += rgpr->cf[i] * p;
		p = p * x;
    }
    return(y);
}

static double tele[16];
static double Cs;
static int nbtele;

// Contrast enchance
void equalise(float **prow, int nrow, int offset, int telestart, rgparam regr[30]){
	for (int n = 0; n < nrow; n++) {
		float *pixelv;
		int i;

		pixelv = prow[n];
		for (i = 0; i < CH_WIDTH; i++) {
			float pv;
			int k, kof;

			pv = pixelv[i + offset];

			k = (n - telestart) / 128;
			if (k >= nbtele)
				k = nbtele - 1;
			kof = (n - telestart) % 128;

			if (kof < 64) {
				if (k < 1) {
					pv = rgcal(pv, &(regr[k]));
				} else {
					pv = rgcal(pv, &(regr[k])) * (64 + kof) / 128.0 + rgcal(pv, &(regr[k - 1])) * (64 - kof) / 128.0;
				}
			} else {
				if ((k + 1) >= nbtele) {
					pv = rgcal(pv, &(regr[k]));
				} else {
					pv = rgcal(pv, &(regr[k])) * (192 - kof) / 128.0 + rgcal(pv, &(regr[k + 1])) * (kof - 64) / 128.0;
				}
			}

			pv = CLIP(pv, 0, 255);
			pixelv[i + offset] = pv;
		}
	}
}

// Get telemetry data for thermal calibration
int calibrate(float **prow, int nrow, int offset, int contrastBoost) {
    double teleline[3000];
    double wedge[16];
    rgparam regr[30];
    int n;
    int k;
    int mtelestart, telestart;
    int channel = -1;
    float max;

	// Extract telemetry data, for a single pixel row
    for (n = 0; n < nrow; n++) {
		int i;

		teleline[n] = 0.0;
		// Average the 40 center pixels from the telemetry block
		for (i = 3; i < 43; i++) {
			teleline[n] += prow[n][i + offset + CH_WIDTH];
		}
		// Compute the average
		teleline[n] /= 40.0;
    }

	// A good minimum amount of pixels to find the telemetry start
    if (nrow < 192) {
		fprintf(stderr, ERR_TELE_ROW);
		return(0);
    }

	// Find the biggest contrast in the telemetry
    max = 0.0;
    mtelestart = 0;

	// Only check the center of the image, where the signal is most likely strongest
    for (n = nrow / 3 - 64; n < 2 * nrow / 3 - 64; n++) {
		float df;

		// Calculate the contrast, in other words
		// (sum 4px below) / (sum 4px above)
		df = (teleline[n - 4] + teleline[n - 3] + teleline[n - 2] +
			  teleline[n - 1]) / (teleline[n] + teleline[n + 1] +
			  teleline[n + 2] + teleline[n + 3]);
		// Find the biggest contrast
		if (df > max) {
			mtelestart = n;
			max = df;
		}
    }

	// Calculate relative offset
    telestart = (mtelestart - 64) % 128;

	// If we cannot find the start of the telemetry or if there is not enough of it
    if (mtelestart < 0 || nrow < telestart + 128) {
		fprintf(stderr, ERR_TELE_ROW);
		return(0);
    }

	/* Compute wedges and regression
	 *
	 * This loop loops every 128 pixels after the relative telemetry start.
	 * Gets the values of where the wedges should be and then feeds into a 
	 * regression algorithm which calculates the amount of noise on the
	 * telemetry.
	 * 
	 * It then finds the part of the telemetry data with the least noise and
	 * turns it into digital values.
	 */
    for (n = telestart, k = 0; n < nrow - 128; n += 128, k++) {
		int j;

		// Loop through the 16 wedges
		for (j = 0; j < 16; j++) {
			int i;

			wedge[j] = 0.0;
			// Center 6 pixels
			for (i = 1; i < 7; i++){
				wedge[j] += teleline[(j * 8) + n + i];
			}
			// Average
			wedge[j] /= 6;
		}

		rgcomp(wedge, &(regr[k]));

		if (k == nrow / 256) {
			int i, l;

			// Telemetry calibration
			for (j = 0; j < 16; j++) {
				tele[j] = rgcal(wedge[j], &(regr[k]));
			}

			// Channel ID
			for (j = 0, max = 10000.0, channel = -1; j < 6; j++) {
				float df;

				df = wedge[15] - wedge[j];
				df = df * df;
				if (df < max) {
					channel = j;
					max = df;
				}
			}

			/* Cs computation */
			for (Cs = 0.0, i = 0, j = n; j < n + 128; j++) {
				double csline;

				for (csline = 0.0, l = 3; l < 43; l++)
					csline += prow[n][l + offset -SPC_WIDTH];
				
				csline /= 40.0;
				if (csline > 50.0) {
					Cs += csline;
					i++;
				}
			}
			Cs /= i;
			Cs = rgcal(Cs, &(regr[k]));
		}
    }
    nbtele = k;

	// Image contrast
	if(contrastBoost) equalise(prow, nrow, offset, telestart, regr);

    return(channel + 1);
}

// --- Temperature Calibration --- //
extern int satnum;
#include "satcal.h"

typedef struct {
    double Nbb;
    double Cs;
    double Cb;
    int ch;
} tempparam;

/* temperature compensation for IR channel */
static void tempcomp(double t[16], int ch, tempparam * tpr) {
    double Tbb, T[4];
    double C;
    int n;

    tpr -> ch = ch - 4;

    /* compute equivalent T black body  */
    for (n = 0; n < 4; n++) {
		float d0, d1, d2;

		C = t[9 + n] * 4.0;
		d0 = satcal[satnum].d[n][0];
		d1 = satcal[satnum].d[n][1];
		d2 = satcal[satnum].d[n][2];
		T[n] = d0;
		T[n] += d1 * C;
		C = C * C;
		T[n] += d2 * C;
    }
    Tbb = (T[0] + T[1] + T[2] + T[3]) / 4.0;
    Tbb = satcal[satnum].rad[tpr->ch].A + satcal[satnum].rad[tpr->ch].B * Tbb;

    /* compute radiance Black body */
    C = satcal[satnum].rad[tpr->ch].vc;
    tpr->Nbb = c1 * C * C * C / (expm1(c2 * C / Tbb));

    /* store Count Blackbody and space */
    tpr->Cs = Cs * 4.0;
    tpr->Cb = t[14] * 4.0;
}

static double tempcal(float Ce, tempparam * rgpr) {
    double Nl, Nc, Ns, Ne;
    double T, vc;

    Ns = satcal[satnum].cor[rgpr->ch].Ns;
    Nl = Ns + (rgpr->Nbb - Ns) * (rgpr->Cs - Ce * 4.0) / (rgpr->Cs - rgpr->Cb);
    Nc = satcal[satnum].cor[rgpr->ch].b[0] +
		 satcal[satnum].cor[rgpr->ch].b[1] * Nl +
		 satcal[satnum].cor[rgpr->ch].b[2] * Nl * Nl;

    Ne = Nl + Nc;

    vc = satcal[satnum].rad[rgpr->ch].vc;
    T = c2 * vc / log1p(c1 * vc * vc * vc / Ne);
    T = (T - satcal[satnum].rad[rgpr->ch].A) / satcal[satnum].rad[rgpr->ch].B;

    /* rescale to range 0-255 for -60 +40 'C */
    T = (T - 273.15 + 60.0) / 100.0 * 256.0;

    return(T);
}

void Temperature(float **prow, int nrow, int channel, int offset) {
    tempparam temp;
    int n;

    printf("Temperature... ");
    fflush(stdout);

    tempcomp(tele, channel, &temp);

    for (n = 0; n < nrow; n++) {
		float *pixelv;
		int i;

		pixelv = prow[n];
		for (i = 0; i < CH_WIDTH; i++) {
			float pv;

			pv = tempcal(pixelv[i + offset], &temp);

			pv = CLIP(pv, 0, 255);
			pixelv[i + offset] = pv;
		}
    }
    printf("Done\n");
}