/* 
 *  This file is part of Aptdec.
 *  Copyright (c) 2004-2009 Thierry Leconte (F4DWV), Xerbo (xerbo@protonmail.com) 2019-20222
 *
 *  Aptdec is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

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

#include "apt.h"
#include "filter.h"

// In case your C compiler is so old that Pi hadn't been invented yet
#ifndef M_PI
#define M_PI 3.141592653589793
#endif

// Block sizes
#define BLKAMP 32768
#define BLKIN 32768

#define Fc 2400.0
#define DFc 50.0
#define PixelLine 2080
#define Fp (2 * PixelLine)
#define RSMULT 15
#define Fi (Fp * RSMULT)

static double Fe;

static double offset = 0.0;
static double FreqLine = 1.0;

static double FreqOsc;
static double K1, K2;

// Check the sample rate and calculate some constants
int apt_init(double F) {
	if(F > Fi) return 1;
	if(F < Fp) return -1;
	Fe = F;

	K1 = DFc / Fe;
	K2 = K1 * K1 / 2.0;
	// Number of samples per cycle
	FreqOsc = Fc / Fe;

	return 0;
}

/* Fast phase estimator
 * Calculates the phase angle of a signal from a IQ sample
 */
static inline double Phase(double I, double Q) {
	double angle, r;
	int s;

	if(I == 0.0 && Q == 0.0) return 0.0;

	if (Q < 0) {
		s = -1;
		Q = -Q;
	} else {
		s = 1;
	}

	if (I >= 0) {
		r = (I - Q) / (I + Q);
		angle = 0.25 - 0.25 * r;
	} else {
		r = (I + Q) / (Q - I);
		angle = 0.75 - 0.25 * r;
	}

	if(s > 0){
		return angle;
	}else{
		return -angle;
	}
}

/* Phase locked loop
 * https://arachnoid.com/phase_locked_loop/
 * Model of this filter here https://www.desmos.com/calculator/m0uadgkoee
 */
static double pll(double I, double Q) {
	// PLL coefficient
	static double PhaseOsc = 0.0;
	double Io, Qo;
	double Ip, Qp;
	double DPhi;

	// Quadrature oscillator / reference
	Io = cos(PhaseOsc);
	Qo = sin(PhaseOsc);

	// Phase detector
	Ip = I * Io + Q * Qo;
	Qp = Q * Io - I * Qo;
	DPhi = Phase(Ip, Qp);

	// Loop filter
	PhaseOsc += 2.0 * M_PI * (K1 * DPhi + FreqOsc);
	if (PhaseOsc > M_PI)
		PhaseOsc -= 2.0 * M_PI;
	if (PhaseOsc <= -M_PI)
		PhaseOsc += 2.0 * M_PI;

	FreqOsc += K2 * DPhi;
	if (FreqOsc > ((Fc + DFc) / Fe))
		FreqOsc = (Fc + DFc) / Fe;
	if (FreqOsc < ((Fc - DFc) / Fe))
		FreqOsc = (Fc - DFc) / Fe;

	return Ip;
}

// Convert samples into pixels
static int getamp(double *ampbuff, int count, apt_getsamples_t getsamples, void *context) {
	static float inbuff[BLKIN];
	static int idxin = 0;
	static int nin = 0;

	for (int n = 0; n < count; n++) {
		double I, Q;

		// Get some more samples when needed
		if (nin < IQFilterLen * 2 + 2) {
			// Number of samples read
			int res;
			memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float));
			idxin = 0;

			// Read some samples
			res = getsamples(context, &(inbuff[nin]), BLKIN - nin);
			nin += res;

			// Make sure there is enough samples to continue
			if (nin < IQFilterLen * 2 + 2)
				return n;
		}

		// Process read samples into a brightness value
		iqfir(&inbuff[idxin], iqfilter, IQFilterLen, &I, &Q);
		ampbuff[n] = pll(I, Q);

		// Increment current sample
		idxin++;
		nin--;
	}

	return count;
}

// Sub-pixel offsetting + FIR compensation
int getpixelv(float *pvbuff, int count, apt_getsamples_t getsamples, void *context) {
	// Amplitude buffer
	static double ampbuff[BLKAMP];
	static int nam = 0;
	static int idxam = 0;

	double mult;

	// Gaussian resampling factor
	mult = (double) Fi / Fe * FreqLine;
	int m = (int)(RSFilterLen / mult + 1);

	for (int n = 0; n < count; n++) {
		int shift;

		if (nam < m) {
			int res;
			memmove(ampbuff, &(ampbuff[idxam]), nam * sizeof(double));
			idxam = 0;
			res = getamp(&(ampbuff[nam]), BLKAMP - nam, getsamples, context);
			nam += res;
			if (nam < m)
				return n;
		}

		// Gaussian FIR compensation filter
		pvbuff[n] = (float)(rsfir(&(ampbuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 256.0);

		shift = ((int) floor((RSMULT - offset) / mult)) + 1;
		offset = shift * mult + offset - RSMULT;

		idxam += shift;
		nam -= shift;
	}

	return count;
}

// Get an entire row of pixels, aligned with sync markers
int apt_getpixelrow(float *pixelv, int nrow, int *zenith, int reset, apt_getsamples_t getsamples, void *context) {
	static float pixels[PixelLine + SyncFilterLen];
	static int npv;
	static int synced = 0;
	static double max = 0.0;
	static double minDoppler = 1000000000, previous = 0;

	if(reset) synced = 0;

	double corr, ecorr, lcorr;
	int res;

	// Move the row buffer into the the image buffer
	if (npv > 0)
		memmove(pixelv, pixels, npv * sizeof(float));

	// Get the sync line
	if (npv < SyncFilterLen + 2) {
		res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv, getsamples, context);
		npv += res;
		if (npv < SyncFilterLen + 2)
			return 0;
	}

	// Calculate the frequency offset
	ecorr = fir(pixelv, Sync, SyncFilterLen);
	corr = fir(&pixelv[1], Sync, SyncFilterLen - 1);
	lcorr = fir(&pixelv[2], Sync, SyncFilterLen - 2);
	FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0);

	double val = fabs(lcorr - ecorr)*0.25 + previous*0.75;
	if(val < minDoppler && nrow > 10){
		minDoppler = val;
		*zenith = nrow;
	}
	previous = fabs(lcorr - ecorr);

	// The point in which the pixel offset is recalculated
	if (corr < 0.75 * max) {
		synced = 0;
		FreqLine = 1.0;
	}
	max = corr;

	if (synced < 8) {
		int mshift;
        static int lastmshift;

		if (npv < PixelLine + SyncFilterLen) {
			res = getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv, getsamples, context);
			npv += res;
			if (npv < PixelLine + SyncFilterLen)
				return 0;
		}

		// Test every possible position until we get the best result
		mshift = 0;
		for (int shift = 0; shift < PixelLine; shift++) {
			double corr;

			corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
			if (corr > max) {
				mshift = shift;
				max = corr;
			}
		}

		// Stop rows dissapearing into the void
		int mshiftOrig = mshift;
		if(abs(lastmshift-mshift) > 3 && nrow != 0){
			mshift = 0;
		}
		lastmshift = mshiftOrig;

		// If we are already as aligned as we can get, just continue
		if (mshift == 0) {
			synced++;
		} else {
			memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float));
			npv -= mshift;
			synced = 0;
			FreqLine = 1.0;
		}
	}

	// Get the rest of this row
	if (npv < PixelLine) {
		res = getpixelv(&(pixelv[npv]), PixelLine - npv, getsamples, context);
		npv += res;
		if (npv < PixelLine)
			return 0;
	}

	// Move the sync lines into the output buffer with the calculated offset
	if (npv == PixelLine) {
		npv = 0;
	} else {
		memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float));
		npv -= PixelLine;
	}

	return 1;
}