// Cavity decay constant computation using newmat10 or newmat11
// Mostly ported from Garth Brown LCLS-II Gun and LBNL team Apex Gun

#include <iostream>
#include <math.h>
#include <stdio.h>

#include "../../newmat10/newmat.h"
#include "../../newmat10/newmatap.h"
#include "../../newmat10/newmatio.h"
#include "../../newmat10/precisio.h"

#include <aSubRecord.h>
#include <recGbl.h>
#include <alarm.h>
#include <dbAccess.h>

#include <registryFunction.h>
#include <epicsExport.h>

using namespace std;
using namespace NEWMAT;

Matrix PseudoInverse(const Matrix & m)
{
    Matrix result;
    // int rows = m.Nrows();
    unsigned int cols = m.Ncols();
    // calculate SVD decomposition
    Matrix U,V;
    DiagonalMatrix D;
    try
    {
	SVD(m,D,U,V);
    }
    catch(Exception)
    {
	cout<<Exception::what() <<endl;
    }

    Matrix Dinv(cols,cols);
    Dinv = 0;
    for (unsigned int i=0; i<cols; i++) {
	if ( fabs(D(i+1,i+1)) < FloatingPointPrecision::Epsilon() )
	    Dinv(i+1,i+1) = 0;
	else
	    Dinv(i+1,i+1) = 1/D(i+1,i+1);
    }

    result = V * Dinv * U.t();
    result.Release();
    return result;
}

static long
asub_decay(aSubRecord *prec)
{
    short debug = (prec->tpro > 1) ? 1 : 0;

    /* Inputs */
    short pulse_stat = *(short *)prec->a,
	  offset     = *(short *)prec->b; // User's offset to calculated 'start'
					  // waveform element


    double  *wf_i = (double *)prec->c, // Full cavity probe
	    *wf_q = (double *)prec->d, // I/Q waveforms used for analysis 
	    *wf_t = (double *)prec->g, // Time and amplitude waveforms
	    *wf_a = (double *)prec->h, // used for displays
	    pulse_length = *(double *)prec->e/1000, // Convert ms to s
	    samp_period  = *(double *)prec->f; // Waveform period between samples

    // Outputs
    short *valid = (short *)prec->valf,
	  *startval = (short *)prec->valg,
	  *wf_length = (short *)prec->vall; // Length of waveform used in analysis

    double  *bw   = (double *)prec->vala,
	    *df   = (double *)prec->valb,
	    *ampl = (double *)prec->valc,
	    *std  = (double *)prec->vald,
	    *ql   = (double *)prec->vale,
	    *wf_i_used = (double *)prec->vali, // Portion of I/Q waveforms
	    *wf_q_used = (double *)prec->valj, // used in analysis
	    *wf_t_used = (double *)prec->valk,
	    *wf_a_used = (double *)prec->valm;


    // Output values from previous iteration;
    // may use in future
    // double  *bw_l   = (double *)prec->ovla,
    //	    *df_l   = (double *)prec->ovlb,
    //	    *ampl_l = (double *)prec->ovlc,
    //	    *std_l  = (double *)prec->ovld,
    //	    *ql_l   = (double *)prec->ovle;
    //    short *valid_l    = (short *)prec->ovlf,
    //	  *startval_l = (short *)prec->ovlg;

    short start;
    double invdt = 1/samp_period;

    double tdiff1, tdiff2;

    size_t len;
    epicsTimeStamp timei, timeq, timea;
    epicsEnum16  sevri, sevrq;

    *valid = 0; // Initialize to not valid

    if ( prec->nec != prec->ned ) {
	if ( debug ) {
	    printf("asub_decay %s: I/Q waveform lengths do not match "
		"I is %i and Q is %i\n", prec->name,
		prec->nec, prec->ned);
	}
	void(recGblSetSevr(prec, CALC_ALARM, INVALID_ALARM));
	return -1;
    }
    else {
	len = prec->nec;
    }

    
    if (pulse_stat == 2) { // If pulsing
	if ( debug ) {
	    printf("asub_decay %s: in pulse mode\n", prec->name);
	}
	*valid = 1;
    }
    else {
	if ( debug ) {
	    printf("asub_decay %s: not in pulse mode. Exiting\n", prec->name);
	}
	*valid = 0;
	void(recGblSetSevr(prec, STATE_ALARM, INVALID_ALARM));
	// Do not process outputs
	return -1;
    }

    // Check I and Q data are valid
    dbGetSevr(&prec->inpc, &sevri);
    dbGetSevr(&prec->inpd, &sevrq);
    if ( (sevri == INVALID_ALARM) || (sevrq == INVALID_ALARM)) {
	if ( debug ) {
	    printf("asub_decay %s: I %i and/or Q %i data invalid\n", prec->name, sevri, sevrq);
	}
	*valid = 0;
	void(recGblSetSevr(prec, CALC_ALARM, INVALID_ALARM));
	// Do not process outputs
	return -1;
    }

    // Check I, Q, amplitude data synchronous
    // (amplitude only used for user displays)
    // (don't bother with time because it changes so rarely)
    dbGetTimeStamp(&prec->inpc, &timei);
    dbGetTimeStamp(&prec->inpd, &timeq);
    dbGetTimeStamp(&prec->inph, &timea);
    tdiff1 =epicsTimeDiffInSeconds(&timei,&timeq);
    tdiff2 =epicsTimeDiffInSeconds(&timei,&timea);
    if ( (0.0 != tdiff1) || (0.0 != tdiff1) ) {
	if ( debug ) {
	    printf("asub_decay %s: I, Q, amplitude timestamps do not match: deltas %.2f s %.2f s\n",
	    prec->name, tdiff1, tdiff2);
	}
	// Do not process outputs
	return -1;
    }
  
    double start_f = (pulse_length/samp_period);
    start = *startval = offset+(short)ceil(start_f);
    if ( debug ) {
	printf("asub_decay %s: len %i offset %i start on %ith element %f invdt %fHz "
	    "pulse length %fs samp period %1.3es\n", 
	    prec->name, (int)len, offset, start, start_f, invdt, pulse_length, samp_period);
    }

#define D_REAL(i) (wf_i[i])
#define D_IMAG(i) (wf_q[i])

	unsigned int length = len - start;
	Matrix X(2*length-1,2);
	Matrix Y(2*length-1,1);
	double energy_hat = 0.0;
	unsigned int i = 0;
	for (i=0; i<length-1; i++) {
		int j = start+i;
		double xr1 = D_REAL(j);
		double xi1 = D_IMAG(j);
		double xr2 = D_REAL(j+1);
		double xi2 = D_IMAG(j+1);

		X(i+1,1) = 0.5*(xi1 + xi2);
		X(length+i+1,1)=0.5*(xr1 + xr2);
		X(i+1,2) = 0.5*(xr1 + xr2);
		X(length+i+1,2)=-0.5*(xi1+xi2);
		Y(i+1,1) = -xi1 + xi2;
		Y(length+i+1,1) = -xr1 + xr2;

		energy_hat = energy_hat + xr1*xr1+xi1*xi1;
		wf_i_used[i] = xr1;
		wf_q_used[i] = xi1;
		wf_t_used[i] = wf_t[j];
		wf_a_used[i] = wf_a[j];

	}
	*wf_length = prec->nevi = prec->nevj = prec->nevk = prec->nevm = length - 1;
	Matrix A = PseudoInverse(X) * Y;
	double ar = -A(1,1);
	double ai = A(2,1);
	ai = (ai * invdt) / (2 * M_PI);
	ar = (ar * invdt) / (2 * M_PI);
	double arr = A(1,1);
	double aii = A(2,1);
	double residue = 0.0;
	for (unsigned int i=0; i<2*length-1;i++) {
		double x1 = X(i+1,1);
		double x2 = X(i+1,2);
		double y = Y(i+1,1);
		double r = (y-(x1*arr)-(x2*aii));
		residue = residue + r*r;
	}
	double stdev = sqrt(residue/(2*length));
	*bw   = ar;
	*df   = ai;
	*ampl = energy_hat;
	*std  = stdev;
        *ql   = 1300e6 / 2 / *bw; // From rf_controls.py

	if ( debug ) {
	    printf("%s decay_asub: Bandwidth %8.1f Hz  Detune %8.1f Hz  "
		"Signal strength %7f  Stdev %5.3f Qloaded %2.3e\n", 
		prec->name, ar, ai, energy_hat, stdev, *ql);
	}

	return 0;
}

extern "C" {
epicsRegisterFunction(asub_decay);
}
