/*  calcc.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2013, 2014, 2016, 2019, 2023 Warren Pratt, NR0V

This program 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, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

The author can be reached by email at

warren@wpratt.com

*/

#define _CRT_SECURE_NO_WARNINGS
#include "comm.h"

void size_calcc (CALCC a)
{   // for change in ints or spi
    int i;
    a->nsamps = a->ints * a->spi;

    a->tsamps = a->nsamps + a->npsamps;
    a->env_TX = (double*)malloc0(a->nsamps * sizeof(double));
    a->env_RX = (double*)malloc0(a->nsamps * sizeof(double));
    a->x = (double*)malloc0(a->tsamps * sizeof(double));
    a->ym = (double*)malloc0(a->tsamps * sizeof(double));
    a->yc = (double*)malloc0(a->tsamps * sizeof(double));
    a->ys = (double*)malloc0(a->tsamps * sizeof(double));
    a->cat = (double*)malloc0(4 * a->nsamps * sizeof(double));

    a->t    = (double *) malloc0 ((a->ints + 1) * sizeof(double));
    a->tmap = (double *) malloc0 ((a->ints + 1) * sizeof(double));
    for (i = 0; i < a->ints + 1; i++)
        a->t[i] = (double)i / (double)a->ints;

    a->cm = (double *) malloc0 (a->ints * 4 * sizeof(double));
    a->cc = (double *) malloc0 (a->ints * 4 * sizeof(double));
    a->cs = (double *) malloc0 (a->ints * 4 * sizeof(double));
    a->cm_old = (double *) malloc0 (a->ints * 4 * sizeof (double));

    a->rxs = (double *) malloc0 (a->nsamps * sizeof (complex));
    a->txs = (double *) malloc0 (a->nsamps * sizeof (complex));

    a->ccbld = create_builder(a->nsamps + a->npsamps, a->ints);

    a->ctrl.cpi = (int *) malloc0 (a->ints * sizeof (int));
    a->ctrl.sindex = (int *) malloc0 (a->ints * sizeof (int));
    a->ctrl.sbase = (int *) malloc0 (a->ints * sizeof (int));

    for (i = 0; i < a->ints; i++)
    {
        a->ctrl.cpi[i] = 0;
        a->ctrl.sindex[i] = 0;
        a->ctrl.sbase[i] = i * a->spi;
    }

    a->disp.x  = (double *) malloc0 (a->nsamps * sizeof (double));
    a->disp.ym = (double *) malloc0 (a->nsamps * sizeof (double));
    a->disp.yc = (double *) malloc0 (a->nsamps * sizeof (double));
    a->disp.ys = (double *) malloc0 (a->nsamps * sizeof (double));
    a->disp.cm = (double *) malloc0 (a->ints * 4 * sizeof(double));
    a->disp.cc = (double *) malloc0 (a->ints * 4 * sizeof(double));
    a->disp.cs = (double *) malloc0 (a->ints * 4 * sizeof(double));

    a->util.pm = (double *) malloc0 (4 * a->util.ints * sizeof(double));
    a->util.pc = (double *) malloc0 (4 * a->util.ints * sizeof(double));
    a->util.ps = (double *) malloc0 (4 * a->util.ints * sizeof(double));
}

void desize_calcc (CALCC a)
{
    _aligned_free(a->util.pm);
    _aligned_free(a->util.pc);
    _aligned_free(a->util.ps);

    _aligned_free (a->disp.cs);
    _aligned_free (a->disp.cc);
    _aligned_free (a->disp.cm);
    _aligned_free (a->disp.ys);
    _aligned_free (a->disp.yc);
    _aligned_free (a->disp.ym);
    _aligned_free (a->disp.x);

    _aligned_free (a->ctrl.sbase);
    _aligned_free (a->ctrl.sindex);
    _aligned_free (a->ctrl.cpi);
    destroy_builder(a->ccbld);
    _aligned_free (a->rxs);
    _aligned_free (a->txs);
    _aligned_free (a->cm_old);
    _aligned_free (a->cm);
    _aligned_free (a->cc);
    _aligned_free (a->cs);
    _aligned_free (a->tmap);
    _aligned_free (a->t);

    _aligned_free(a->cat);
    _aligned_free(a->x);
    _aligned_free(a->ym);
    _aligned_free(a->yc);
    _aligned_free(a->ys);
    _aligned_free(a->env_TX);
    _aligned_free(a->env_RX);
}

CALCC create_calcc (int channel, int runcal, int size, int rate, int ints, int spi, double hw_scale,
    double moxdelay, double loopdelay, double ptol, int mox, int solidmox, int pin, int map, int stbl,
    int npsamps, double alpha)
{
    CALCC a = (CALCC) malloc0 (sizeof (calcc));
    a->channel = channel;
    a->runcal = runcal;
    a->size = size;
    a->rate = rate;
    a->ints = ints;
    a->spi = spi;
    a->hw_scale = hw_scale;
    a->ctrl.moxdelay = moxdelay;
    a->ctrl.loopdelay = loopdelay;
    a->ptol = ptol;
    a->mox = mox;
    a->solidmox = solidmox;
    a->pin = pin;
    a->map = map;
    a->stbl = stbl;
    a->npsamps = npsamps;
    a->alpha = alpha;

    a->info  = (int *) malloc0 (16 * sizeof (int));
    a->binfo = (int *) malloc0 (16 * sizeof (int));

    a->ctrl.state = 0;
    a->ctrl.reset = 0;
    a->ctrl.automode = 0;
    a->ctrl.mancal = 0;
    a->ctrl.turnon = 0;
    a->ctrl.moxsamps = (int)(a->rate * a->ctrl.moxdelay);
    a->ctrl.moxcount = 0;
    a->ctrl.count = 0;
    a->ctrl.full_ints = 0;
    a->ctrl.calcinprogress = 0;
    a->ctrl.calcdone = 0;
    a->ctrl.waitsamps = (int)(a->rate * a->ctrl.loopdelay);
    a->ctrl.waitcount = 0;
    a->ctrl.running = 0;
    a->ctrl.current_state = 0;
    InitializeCriticalSectionAndSpinCount (&txa[a->channel].calcc.cs_update, 2500);
    InitializeCriticalSectionAndSpinCount (&a->ctrl.cs_SafeToEnd, 2500);
    a->rxdelay = create_delay (
        1,                                          // run
        0,                                          // size             [stuff later]
        0,                                          // input buffer     [stuff later]
        0,                                          // output buffer    [stuff later]
        a->rate,                                    // sample rate
        20.0e-09,                                   // delta (delay stepsize)
        0.0);                                       // delay
    a->txdelay = create_delay (
        1,                                          // run
        0,                                          // size             [stuff later]
        0,                                          // input buffer     [stuff later]
        0,                                          // output buffer    [stuff later]
        a->rate,                                    // sample rate
        20.0e-09,                                   // delta (delay stepsize)
        0.0);                                       // delay

    InitializeCriticalSectionAndSpinCount (&a->disp.cs_disp, 2500);
    a->util.ints = a->ints;
    a->util.channel = a->channel;

    size_calcc (a);

    a->temprx = (double*)malloc0(2048 * sizeof(complex));                                                       // remove later
    a->temptx = (double*)malloc0(2048 * sizeof(complex));                                                       // remove later

    // correction save and restore threads
    InterlockedBitTestAndReset(&a->savecorr_bypass, 0);
    a->Sem_SaveCorr = CreateSemaphore(0, 0, 1, 0);
    _beginthread(PSSaveCorrection, 0, (void*)a);
    InterlockedBitTestAndReset(&a->restcorr_bypass, 0);
    a->Sem_RestCorr = CreateSemaphore(0, 0, 1, 0);
    _beginthread(PSRestoreCorrection, 0, (void*)a);
    InterlockedBitTestAndReset(&a->calccorr_bypass, 0);
    a->Sem_CalcCorr = CreateSemaphore(0, 0, 1, 0);
    _beginthread(doPSCalcCorrection, 0, (void*)a);
    InterlockedBitTestAndReset(&a->turnoff_bypass, 0);
    a->Sem_TurnOff = CreateSemaphore(0, 0, 1, 0);
    _beginthread(doPSTurnoff, 0, (void*)a);

    return a;
}

void destroy_calcc (CALCC a)
{
    // correction save and restore threads
    InterlockedBitTestAndReset(&txa[a->channel].iqc.p1->busy, 0);
    Sleep(10);
    InterlockedBitTestAndSet(&a->savecorr_bypass, 0);
    ReleaseSemaphore(a->Sem_SaveCorr, 1, 0);
    while (InterlockedAnd(&a->savecorr_bypass, 0xffffffff)) Sleep(1);
    CloseHandle(a->Sem_SaveCorr);
    InterlockedBitTestAndSet(&a->restcorr_bypass, 0);
    ReleaseSemaphore(a->Sem_RestCorr, 1, 0);
    while (InterlockedAnd(&a->restcorr_bypass, 0xffffffff)) Sleep(1);
    CloseHandle(a->Sem_RestCorr);
    InterlockedBitTestAndSet(&a->calccorr_bypass, 0);
    ReleaseSemaphore(a->Sem_CalcCorr, 1, 0);
    while (InterlockedAnd(&a->calccorr_bypass, 0xffffffff)) Sleep(1);
    CloseHandle(a->Sem_CalcCorr);
    InterlockedBitTestAndSet(&a->turnoff_bypass, 0);
    ReleaseSemaphore(a->Sem_TurnOff, 1, 0);
    while (InterlockedAnd(&a->turnoff_bypass, 0xffffffff)) Sleep(1);
    CloseHandle(a->Sem_TurnOff);

    _aligned_free (a->temptx);                                                                                      // remove later
    _aligned_free (a->temprx);                                                                                      // remove later
    desize_calcc (a);
    DeleteCriticalSection (&a->disp.cs_disp);
    destroy_delay (a->txdelay);
    destroy_delay (a->rxdelay);
    DeleteCriticalSection (&a->ctrl.cs_SafeToEnd);
    DeleteCriticalSection (&txa[a->channel].calcc.cs_update);
    _aligned_free (a->binfo);
    _aligned_free (a->info);

    _aligned_free (a);
}

void flush_calcc (CALCC a)
{
    flush_delay (a->rxdelay);
    flush_delay (a->txdelay);
}

void scheck(CALCC a)
{
    int i, j, k;
    double v, dx, out, x, xold;
    int intm1 = a->ints - 1;
    const double diff_thresh = 0.05;
    a->binfo[6] = 0x0000;

    for (i = 0; i < 4 * a->ints; i++)
    {
        if (isnan (a->cm[i])) a->binfo[6] |= 0x0001;
        if (isnan (a->cc[i])) a->binfo[6] |= 0x0001;
        if (isnan (a->cs[i])) a->binfo[6] |= 0x0001;
    }

    for (i = 0; i < a->ints; i++)
        if ((a->cm[4 * i + 0] == 0.0) && (a->cm[4 * i + 1] == 0.0) &&
            (a->cm[4 * i + 2] == 0.0) && (a->cm[4 * i + 3] == 0.0)) a->binfo[6] |= 0x0002;

    for (i = 0; i < a->ints; i++)
    {
        for (j = 0; j < 4; j++)
        {
            k = 4 * i + j;
            v = (double)k / (4.0 * (double)a->ints);
            dx = (a->t[i + 1] - a->t[i]) * (double)j / 4.0;
            out = v * (a->cm[4 * i + 0] + dx * (a->cm[4 * i + 1] + dx * (a->cm[4 * i + 2] + dx * a->cm[4 * i + 3])));
            if (out > 1.0)
                a->binfo[6] |= 0x0004;
            if (out < 0.0) a->binfo[6] |= 0x0010;
        }
    }

    dx = a->t[a->ints] - a->t[intm1];
    x = a->cm[4 * intm1 + 0] + dx * (a->cm[4 * intm1 + 1] + dx * (a->cm[4 * intm1 + 2] + dx * a->cm[4 * intm1 + 3]));

    if (x > 1.07)   // VALUE
        a->binfo[6] |= 0x0008;
    if (x < 0.0) a->binfo[6] |= 0x0020;

    for (i = 4; i < a->ints; i++)
        if (fabs (a->cm[4 * i + 0] - a->cm_old[4 * i + 0]) > diff_thresh) a->binfo[6] |= 0x0040;
    xold = a->cm_old[4 * intm1 + 0] + dx * (a->cm_old[4 * intm1 + 1] + dx * (a->cm_old[4 * intm1 + 2] + dx * a->cm_old[4 * intm1 + 3]));
    if (fabs (x - xold) > diff_thresh) a->binfo[6] |= 0x0040;
    memcpy (a->cm_old, a->cm, a->ints * 4 * sizeof(double));
}

void rxscheck (int rints, double* tvec, double* coef, int* info)
{
    int i, j, k;
    int rintsm1 = rints - 1;
    double v, dx, out;
    *info = 0x0000;
    for (i = 0; i < 4 * rints; i++)
        if (isnan (coef[i])) *info |= 0x0001;
    for (i = 0; i < rints; i++)
        if ((coef[4 * i + 0] == 0.0) && (coef[4 * i + 1] == 0.0) && (coef[4 * i + 2] == 0.0) && (coef[4 * i + 3] == 0.0))
            *info |= 0x0002;
    for (i = 0; i < rints; i++)
    {
        for (j = 0; j < 4; j++)
        {
            k = 4 * i + j;
            v = (double)k / (4.0 * (double)rints);
            dx = (tvec[i + 1] - tvec[i]) * (double)j / 4.0;
            out = v * (coef[4 * i + 0] + dx * (coef[4 * i + 1] + dx * (coef[4 * i + 2] + dx * coef[4 * i + 3])));
            if (out > 1.0)  // potentially use hw_scale here
                *info |= 0x0004;
            if (out < 0.0) *info |= 0x0010;
        }
    }
    dx = tvec[rints] - tvec[rints - 1];
    out = coef[4 * rintsm1 + 0] + dx * (coef[4 * rintsm1 + 1] + dx * (coef[4 * rintsm1 + 2] + dx * coef[4 * rintsm1 + 3]));
    if (out > 1.07) *info |= 0x0008;
    if (out < 0.00) *info |= 0x0020;
}

void calc (CALCC a)
{
    int i;
    double norm;
    for (i = 0; i < a->nsamps; i++)
    {
        a->env_TX[i] = sqrt (a->txs[2 * i + 0] * a->txs[2 * i + 0] + a->txs[2 * i + 1] * a->txs[2 * i + 1]);
        a->env_RX[i] = sqrt (a->rxs[2 * i + 0] * a->rxs[2 * i + 0] + a->rxs[2 * i + 1] * a->rxs[2 * i + 1]);
    }
    {
        int rints, ix;
        double dx;
        double tvec[3];
        double txrxcoefs[4 * 2];
        double rx_scale;
        if (a->ints < 16) rints = 1;
        else              rints = 2;
        ix = rints - 1;
        for (i = 0; i <= rints; i++)
            tvec[i] = (double)i / (double)rints / a->hw_scale;
        dx = tvec[rints] - tvec[rints - 1];
        xbuilder(a->ccbld, a->nsamps, a->env_TX, a->env_RX, rints, tvec, &(a->binfo[0]), txrxcoefs, a->ptol);
        rxscheck (rints, tvec, txrxcoefs, &a->binfo[7]);
        if ((a->binfo[0] == 0) && (a->binfo[7] == 0))
            rx_scale = 1.0 / (txrxcoefs[4 * ix + 0] + dx * (txrxcoefs[4 * ix + 1] + dx * (txrxcoefs[4 * ix + 2] + dx * txrxcoefs[4 * ix + 3])));
        else
        {
            a->scOK = 0;
            goto cleanup;
        }
        if (a->stbl && _InterlockedAnd (&a->ctrl.running, 1))
            a->rx_scale = a->alpha * a->rx_scale + (1.0 - a->alpha) * rx_scale;
        else
            a->rx_scale = rx_scale;
    }

    a->binfo[4] = (int)(256.0 * (a->hw_scale / a->rx_scale));
    a->binfo[5]++;

    if (a->pin) // regress
    {
        const double slope = 0.001;
        double max_rx;
        for (i = 0; i < a->nsamps; i++)
        {
            max_rx = (1.0 - slope + slope * a->hw_scale * a->env_TX[i]) / a->rx_scale;
            if (a->env_RX[i] > max_rx)
                a->env_RX[i] = max_rx;
        }
    }

    for (i = 0; i < a->nsamps; i++)
    {
        norm = a->env_TX[i] * a->env_RX[i];
        a->x[i]  = a->rx_scale * a->env_RX[i];
        a->ym[i] = (a->hw_scale * a->env_TX[i]) / (a->rx_scale * a->env_RX[i]);
        a->yc[i] = (+ a->txs[2 * i + 0] * a->rxs[2 * i + 0] + a->txs[2 * i + 1] * a->rxs[2 * i + 1]) / norm;
        a->ys[i] = (- a->txs[2 * i + 0] * a->rxs[2 * i + 1] + a->txs[2 * i + 1] * a->rxs[2 * i + 0]) / norm;
        if (a->stbl && _InterlockedAnd (&a->ctrl.running, 1) && a->scOK)
        {
            int k;
            double dx, ymo, yco, yso;
            if ((k = (int)(a->x[i] * a->ints)) > a->ints - 1) k = a->ints - 1;
            dx = a->x[i] - a->t[k];
            ymo = a->cm[4 * k + 0] + dx * (a->cm[4 * k + 1] + dx * (a->cm[4 * k + 2] + dx * a->cm[4 * k + 3]));
            yco = a->cc[4 * k + 0] + dx * (a->cc[4 * k + 1] + dx * (a->cc[4 * k + 2] + dx * a->cc[4 * k + 3]));
            yso = a->cs[4 * k + 0] + dx * (a->cs[4 * k + 1] + dx * (a->cs[4 * k + 2] + dx * a->cs[4 * k + 3]));
            a->ym[i] = a->alpha * ymo + (1.0 - a->alpha) * a->ym[i];
            a->yc[i] = a->alpha * yco + (1.0 - a->alpha) * a->yc[i];
            a->ys[i] = a->alpha * yso + (1.0 - a->alpha) * a->ys[i];
        }
    }

    if (a->pin) // pin
    {
        const double mval = 1.0e+00 - 1.0e-10;
        double cval, sval;
        for (i = 0; i < a->nsamps; i++)
        {
            a->cat[4 * i + 0] = a->x[i];
            a->cat[4 * i + 1] = a->ym[i];
            a->cat[4 * i + 2] = a->yc[i];
            a->cat[4 * i + 3] = a->ys[i];
        }
        qsort(a->cat, a->nsamps, 4 * sizeof(double), fcompare);
        for (i = 0; i < a->nsamps; i++)
        {
            a->x[i]  = a->cat[4 * i + 0];
            a->ym[i] = a->cat[4 * i + 1];
            a->yc[i] = a->cat[4 * i + 2];
            a->ys[i] = a->cat[4 * i + 3];
        }
        cval = 0.0;
        sval = 0.0;
        for (i = a->nsamps - 1; i > a->nsamps - 17; i--)
        {
            cval += a->yc[i];
            sval += a->ys[i];
        }
        cval /= 16.0;
        sval /= 16.0;
        for (i = a->nsamps; i < a->tsamps; i++)
        {
            a->x[i]  = mval;
            a->ym[i] = mval;
            a->yc[i] = cval;
            a->ys[i] = sval;
        }
        xbuilder(a->ccbld, a->tsamps, a->x, a->ym, a->ints, a->t, &(a->binfo[1]), a->cm, a->ptol);
        xbuilder(a->ccbld, a->tsamps, a->x, a->yc, a->ints, a->t, &(a->binfo[2]), a->cc, a->ptol);
        xbuilder(a->ccbld, a->tsamps, a->x, a->ys, a->ints, a->t, &(a->binfo[3]), a->cs, a->ptol);
    }
    else
    {
        xbuilder(a->ccbld, a->nsamps, a->x, a->ym, a->ints, a->t, &(a->binfo[1]), a->cm, a->ptol);
        xbuilder(a->ccbld, a->nsamps, a->x, a->yc, a->ints, a->t, &(a->binfo[2]), a->cc, a->ptol);
        xbuilder(a->ccbld, a->nsamps, a->x, a->ys, a->ints, a->t, &(a->binfo[3]), a->cs, a->ptol);
    }

    if (a->pin) // tune
    {
        int k = a->ints - 1;
        double dx = a->t[a->ints] - a->t[a->ints - 1];
        double sf = 1.0 / (a->cm[4 * k + 0] + dx * (a->cm[4 * k + 1] + dx * (a->cm[4 * k + 2] + dx * a->cm[4 * k + 3])));
        for (i = 0; i < 4 * a->ints; i++)
            a->cm[i] *= sf;
    }

    scheck (a);
    a->scOK = ((a->binfo[0] == 0) && (a->binfo[1] == 0) && (a->binfo[2] == 0) && (a->binfo[3] == 0) && (a->binfo[6] == 0));

    if (a->scOK)    // map calc
    {
        for (i = 0; i < a->ints; i++)
            a->tmap[i] = a->cm[4 * i] * a->t[i];
        a->tmap[a->ints] = 1.0;
        a->convex = ((a->tmap[a->ints] - a->tmap[a->ints - 1]) > (a->t[a->ints] - a->t[a->ints - 1]));
    }

    EnterCriticalSection (&a->disp.cs_disp);
    memcpy(a->disp.x, a->x,  a->nsamps * sizeof (double));
    memcpy(a->disp.ym, a->ym, a->nsamps * sizeof (double));
    memcpy(a->disp.yc, a->yc, a->nsamps * sizeof (double));
    memcpy(a->disp.ys, a->ys, a->nsamps * sizeof (double));
    if (a->scOK)
    {
        memcpy(a->disp.cm, a->cm, a->ints * 4 * sizeof (double));
        memcpy(a->disp.cc, a->cc, a->ints * 4 * sizeof (double));
        memcpy(a->disp.cs, a->cs, a->ints * 4 * sizeof (double));
    }
    else
    {
        memset(a->disp.cm, 0, a->ints * 4 * sizeof (double));
        memset(a->disp.cc, 0, a->ints * 4 * sizeof (double));
        memset(a->disp.cs, 0, a->ints * 4 * sizeof (double));
    }
    LeaveCriticalSection (&a->disp.cs_disp);
cleanup:
    return;
}

void __cdecl doPSCalcCorrection (void *arg)
{
    CALCC a = (CALCC)arg;
    while (!InterlockedAnd(&a->calccorr_bypass, 0xffffffff))
    {
        WaitForSingleObject(a->Sem_CalcCorr, INFINITE);
        if (!InterlockedAnd(&a->calccorr_bypass, 0xffffffff))
        {
            calc(a);
            if (a->scOK)
            {
                EnterCriticalSection (&a->ctrl.cs_SafeToEnd);
                if (!InterlockedBitTestAndSet(&a->ctrl.running, 0))
                    SetTXAiqcStart(a->channel, a->cm, a->cc, a->cs);
                else
                    SetTXAiqcSwap(a->channel, a->cm, a->cc, a->cs);
                LeaveCriticalSection(&a->ctrl.cs_SafeToEnd);
            }
            InterlockedBitTestAndSet(&a->ctrl.calcdone, 0);
        }
    }
    InterlockedBitTestAndReset(&a->calccorr_bypass, 0);
}

void __cdecl doPSTurnoff (void *arg)
{
    CALCC a = (CALCC)arg;
    while (!InterlockedAnd(&a->turnoff_bypass, 0xffffffff))
    {
        WaitForSingleObject(a->Sem_TurnOff, INFINITE);
        if (!InterlockedAnd(&a->turnoff_bypass, 0xffffffff))
        {
            EnterCriticalSection(&a->ctrl.cs_SafeToEnd);
            SetTXAiqcEnd(a->channel);
            LeaveCriticalSection(&a->ctrl.cs_SafeToEnd);
        }
    }
    InterlockedBitTestAndReset(&a->turnoff_bypass, 0);
}

enum _calcc_state
{
    LRESET,
    LWAIT,
    LMOXDELAY,
    LSETUP,
    LCOLLECT,
    MOXCHECK,
    LCALC,
    LDELAY,
    LSTAYON,
    LTURNON
};

void __cdecl PSSaveCorrection (void *pargs)
{
    int i, k;
    CALCC a = (CALCC)pargs;
    while (!InterlockedAnd(&a->savecorr_bypass, 0xffffffff))
    {
        WaitForSingleObject(a->Sem_SaveCorr, INFINITE);
        if (!InterlockedAnd(&a->savecorr_bypass, 0xffffffff))
        {
            FILE* file = fopen(a->util.savefile, "w");
            GetTXAiqcValues(a->util.channel, a->util.pm, a->util.pc, a->util.ps);
            for (i = 0; i < a->util.ints; i++)
            {
                for (k = 0; k < 4; k++)
                    fprintf(file, "%.17e\t", a->util.pm[4 * i + k]);
                fprintf(file, "\n");
                for (k = 0; k < 4; k++)
                    fprintf(file, "%.17e\t", a->util.pc[4 * i + k]);
                fprintf(file, "\n");
                for (k = 0; k < 4; k++)
                    fprintf(file, "%.17e\t", a->util.ps[4 * i + k]);
                fprintf(file, "\n\n");
            }
            fflush(file);
            fclose(file);
        }
    }
    InterlockedBitTestAndReset(&a->savecorr_bypass, 0);
}

void __cdecl PSRestoreCorrection(void *pargs)
{
    int i, k;
    CALCC a = (CALCC)pargs;
    while (!InterlockedAnd(&a->restcorr_bypass, 0xffffffff))
    {
        WaitForSingleObject(a->Sem_RestCorr, INFINITE);
        if (!InterlockedAnd(&a->restcorr_bypass, 0xffffffff))
        {
            FILE* file = fopen(a->util.restfile, "r");
            for (i = 0; i < a->util.ints; i++)
            {
                for (k = 0; k < 4; k++)
                    fscanf(file, "%le", &(a->util.pm[4 * i + k]));
                for (k = 0; k < 4; k++)
                    fscanf(file, "%le", &(a->util.pc[4 * i + k]));
                for (k = 0; k < 4; k++)
                    fscanf(file, "%le", &(a->util.ps[4 * i + k]));
            }
            fclose(file);
            if (!InterlockedBitTestAndSet(&a->ctrl.running, 0))
                SetTXAiqcStart(a->channel, a->util.pm, a->util.pc, a->util.ps);
            else
                SetTXAiqcSwap(a->channel, a->util.pm, a->util.pc, a->util.ps);
        }
    }
    InterlockedBitTestAndReset(&a->restcorr_bypass, 0);
}


/********************************************************************************************************
*                                                                                                       *
*                                         Public Functions                                              *
*                                                                                                       *
********************************************************************************************************/

PORT
void pscc (int channel, int size, double* tx, double* rx)
{
    int i, n, m;
    double env;
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    if (a->runcal)
    {
        a->size = size;
        if (InterlockedAnd (&a->mox, 1) && (a->txdelay->tdelay != 0.0 || a->rxdelay->tdelay != 0.0))
        {
            SetDelayBuffs (a->rxdelay, a->size, rx, rx);
            xdelay (a->rxdelay);
            SetDelayBuffs (a->txdelay, a->size, tx, tx);
            xdelay (a->txdelay);
        }
        a->info[15] = a->ctrl.state;

        switch (a->ctrl.state)
        {
            case LRESET:
                InterlockedExchange (&a->ctrl.current_state, LRESET);
                a->ctrl.reset = 0;
                if (!a->ctrl.turnon)
                    if (InterlockedBitTestAndReset(&a->ctrl.running, 0))
                        ReleaseSemaphore(a->Sem_TurnOff, 1, 0);
                a->info[14] = 0;
                a->ctrl.env_maxtx = 0.0;
                a->ctrl.bs_count = 0;
                if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (a->ctrl.automode || a->ctrl.mancal)
                    a->ctrl.state = LWAIT;
                break;
            case LWAIT:
                InterlockedExchange (&a->ctrl.current_state, LWAIT);
                a->ctrl.mancal = 0;
                a->ctrl.moxcount = 0;
                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (InterlockedAnd (&a->mox, 1))
                {
                    a->ctrl.state = LMOXDELAY;
                    InterlockedBitTestAndSet (&a->solidmox, 0);
                }
                break;
            case LMOXDELAY:
                InterlockedExchange (&a->ctrl.current_state, LMOXDELAY);
                a->ctrl.moxcount += a->size;
                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (!InterlockedAnd (&a->mox, 1) || !InterlockedAnd (&a->solidmox, 1))
                    a->ctrl.state = LWAIT;
                else if ((a->ctrl.moxcount - a->size) >= a->ctrl.moxsamps)
                    a->ctrl.state = LSETUP;
                break;
            case LSETUP:
                InterlockedExchange (&a->ctrl.current_state, LSETUP);
                a->ctrl.count = 0;
                for (i = 0; i < a->ints; i++)
                {
                    a->ctrl.cpi[i] = 0;
                    a->ctrl.sindex[i] = 0;
                }
                a->ctrl.full_ints = 0;
                a->ctrl.waitcount = 0;

                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (InterlockedAnd (&a->mox, 1) && InterlockedAnd (&a->solidmox, 1))
                {
                    a->ctrl.state = LCOLLECT;
                    SetTXAiqcDogCount (channel, a->info[13] = 0);
                }
                else
                    a->ctrl.state = LWAIT;
                break;
            case LCOLLECT:
                InterlockedExchange (&a->ctrl.current_state, LCOLLECT);
                for (i = 0; i < a->size; i++)
                {
                    env = sqrt(tx[2 * i + 0] * tx[2 * i + 0] + tx[2 * i + 1] * tx[2 * i + 1]);
                    if (env > a->ctrl.env_maxtx)
                        a->ctrl.env_maxtx = env;
                    if ((env *= a->hw_scale) <= 1.0)
                    {
                        if (env == 1.0)
                            n = a->ints - 1;
                        else if (a->map && _InterlockedAnd (&a->ctrl.running, 1) && a->convex)
                        {
                            int nmin = 0;
                            int nmax = a->ints;

                            while (nmax - nmin > 1)
                            {
                                n = (nmin + nmax) / 2;
                                if (env >= a->tmap[n])
                                    nmin = n;
                                else
                                    nmax = n;
                            }
                            if (env < a->tmap[n]) n--;
                        }
                        else
                            n = (int)(env * (double)a->ints);
                        m = a->ctrl.sbase[n] + a->ctrl.sindex[n];
                        a->txs[2 * m + 0] = tx[2 * i + 0];
                        a->txs[2 * m + 1] = tx[2 * i + 1];
                        a->rxs[2 * m + 0] = rx[2 * i + 0];
                        a->rxs[2 * m + 1] = rx[2 * i + 1];
                        if (++a->ctrl.sindex[n] == a->spi) a->ctrl.sindex[n] = 0;
                        if (a->ctrl.cpi[n] != a->spi)
                            if (++a->ctrl.cpi[n] == a->spi) a->ctrl.full_ints++;
                        ++a->ctrl.count;
                    }
                }
                GetTXAiqcDogCount (channel, &a->info[13]);
                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (!InterlockedAnd (&a->mox, 1) || !InterlockedAnd (&a->solidmox, 1))
                    a->ctrl.state = LWAIT;
                else if (a->ctrl.full_ints == a->ints)
                    a->ctrl.state = MOXCHECK;
                else if (a->info[13] >= 6)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.count >= 4 * a->rate)
                {
                    a->ctrl.count = 0;
                    for (i = 0; i < a->ints; i++)
                    {
                        a->ctrl.cpi[i] = 0;
                        a->ctrl.sindex[i] = 0;
                    }
                    a->ctrl.full_ints = 0;
                }
                break;
            case MOXCHECK:
                InterlockedExchange (&a->ctrl.current_state, MOXCHECK);
                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if (!InterlockedAnd (&a->mox, 1) || !InterlockedAnd (&a->solidmox, 1))
                    a->ctrl.state = LWAIT;
                else
                    a->ctrl.state = LCALC;
                break;
            case LCALC:
                InterlockedExchange (&a->ctrl.current_state, LCALC);
                if (!a->ctrl.calcinprogress)
                {
                    a->ctrl.calcinprogress = 1;
                    ReleaseSemaphore(a->Sem_CalcCorr, 1, 0);
                }

                if (InterlockedBitTestAndReset(&a->ctrl.calcdone, 0))
                {
                    memcpy (a->info, a->binfo, 8 * sizeof (int));
                    a->info[14] = _InterlockedAnd (&a->ctrl.running, 1);
                    a->ctrl.calcinprogress = 0;
                    if (a->ctrl.reset)
                        a->ctrl.state = LRESET;
                    else if (a->ctrl.turnon)
                        a->ctrl.state = LTURNON;
                    else if (a->scOK)
                    {
                        a->ctrl.bs_count = 0;
                        a->ctrl.state = LDELAY;
                    }
                    else if (++(a->ctrl.bs_count) >= 2)
                        a->ctrl.state = LRESET;
                    else if (InterlockedAnd (&a->mox, 1) && InterlockedAnd (&a->solidmox, 1))
                        a->ctrl.state = LSETUP;
                    else a->ctrl.state = LWAIT;
                }
                break;
            case LDELAY:
                InterlockedExchange (&a->ctrl.current_state, LDELAY);
                a->ctrl.waitcount += a->size;
                if (a->ctrl.reset)
                    a->ctrl.state = LRESET;
                else if (a->ctrl.turnon)
                    a->ctrl.state = LTURNON;
                else if ((a->ctrl.waitcount - a->size) >= a->ctrl.waitsamps)
                {
                    if (a->ctrl.automode)
                    {
                        if (InterlockedAnd (&a->mox, 1) && InterlockedAnd (&a->solidmox, 1))
                            a->ctrl.state = LSETUP;
                        else
                            a->ctrl.state = LWAIT;
                    }
                    else
                        a->ctrl.state = LSTAYON;
                }
                break;
            case LSTAYON:
                InterlockedExchange (&a->ctrl.current_state, LSTAYON);
                if (a->ctrl.reset || a->ctrl.automode || a->ctrl.mancal)
                    a->ctrl.state = LRESET;
                break;
            case LTURNON:
                InterlockedExchange (&a->ctrl.current_state, LTURNON);
                a->ctrl.turnon = 0;
                a->ctrl.automode = 0;
                a->info[14] = 1;
                a->ctrl.state = LSTAYON;
                break;
        }
    }
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void psccF (int channel, int size, float *Itxbuff, float *Qtxbuff, float *Irxbuff, float *Qrxbuff, int mox, int solidmox)
{
    int i;
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    // a->mox = mox;
    // a->solidmox = solidmox;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
    for (i = 0; i < size; i++)
    {
        a->temptx[2 * i + 0] = (double)Itxbuff[i];
        a->temptx[2 * i + 1] = (double)Qtxbuff[i];
        a->temprx[2 * i + 0] = (double)Irxbuff[i];
        a->temprx[2 * i + 1] = (double)Qrxbuff[i];
    }
    pscc (channel, size, a->temptx, a->temprx);
}

PORT
void PSSaveCorr (int channel, char* filename)
{
    CALCC a;
    int i = 0;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    while (a->util.savefile[i++] = *filename++);
    ReleaseSemaphore(a->Sem_SaveCorr, 1, 0);
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void PSRestoreCorr (int channel, char* filename)
{
    CALCC a;
    int i = 0;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    while (a->util.restfile[i++] = *filename++);
    a->ctrl.turnon = 1;
    ReleaseSemaphore(a->Sem_RestCorr, 1, 0);
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

/********************************************************************************************************
*                                                                                                       *
*                                             Properties                                                *
*                                                                                                       *
********************************************************************************************************/

PORT
void SetPSRunCal (int channel, int run)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->runcal = run;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSMox (int channel, int mox)
{
    CALCC a = txa[channel].calcc.p;;
    if (mox)
        InterlockedBitTestAndSet (&a->mox, 0);
    else
    {
        InterlockedBitTestAndReset (&a->mox, 0);
        InterlockedBitTestAndReset (&a->solidmox, 0);
    }
}

PORT
void GetPSInfo (int channel, int *info)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    memcpy (info, a->info, 16 * sizeof(int));
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSReset (int channel, int reset)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->ctrl.reset = reset;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSMancal (int channel, int mancal)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->ctrl.mancal = mancal;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSAutomode (int channel, int automode)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->ctrl.automode = automode;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSTurnon (int channel, int turnon)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->ctrl.turnon = turnon;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSControl (int channel, int reset, int mancal, int automode, int turnon)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->ctrl.reset = reset;
    a->ctrl.mancal = mancal;
    a->ctrl.automode = automode;
    a->ctrl.turnon = turnon;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSLoopDelay (int channel, double delay)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->ctrl.loopdelay = delay;
    a->ctrl.waitsamps = (int)(a->rate * a->ctrl.loopdelay);
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSMoxDelay (int channel, double delay)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->ctrl.moxdelay = delay;
    a->ctrl.moxsamps = (int)(a->rate * a->ctrl.moxdelay);
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
double SetPSTXDelay (int channel, double delay)
{
    CALCC a;
    double adelay;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->txdel = delay;
    if (a->txdel >= 0.0)
    {
        adelay = SetDelayValue (a->txdelay, a->txdel);
        SetDelayValue (a->rxdelay, 0.0);
    }
    else
    {
        adelay = -SetDelayValue (a->rxdelay, -a->txdel);
        SetDelayValue (a->txdelay, 0.0);
    }
    //adelay = SetDelayValue (a->txdelay, a->txdel);
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
    return adelay;
}

PORT
void SetPSHWPeak (int channel, double peak)
{
    CALCC a;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a = txa[channel].calcc.p;
    a->hw_scale = 1.0 / peak;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void GetPSHWPeak (int channel, double* peak)
{
EnterCriticalSection (&txa[channel].calcc.cs_update);
*peak = 1.0 / txa[channel].calcc.p->hw_scale;
LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void GetPSMaxTX (int channel, double* maxtx)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    *maxtx = txa[channel].calcc.p->ctrl.env_maxtx;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSPtol (int channel, double ptol)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->ptol = ptol;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void GetPSDisp (int channel, double* x, double* ym, double* yc, double* ys, double* cm, double* cc, double* cs)
{
    CALCC a = txa[channel].calcc.p;
    EnterCriticalSection (&a->disp.cs_disp);
    memcpy (x,  a->disp.x,  a->nsamps * sizeof (double));
    memcpy (ym, a->disp.ym, a->nsamps * sizeof (double));
    memcpy (yc, a->disp.yc, a->nsamps * sizeof (double));
    memcpy (ys, a->disp.ys, a->nsamps * sizeof (double));
    memcpy (cm, a->disp.cm, a->ints * 4 * sizeof (double));
    memcpy (cc, a->disp.cc, a->ints * 4 * sizeof (double));
    memcpy (cs, a->disp.cs, a->ints * 4 * sizeof (double));
    LeaveCriticalSection (&a->disp.cs_disp);
}

PORT
void SetPSFeedbackRate (int channel, int rate)
{
    CALCC a = txa[channel].calcc.p;
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    a->rate = rate;
    a->ctrl.moxsamps = (int)(a->rate * a->ctrl.moxdelay);
    a->ctrl.waitsamps = (int)(a->rate * a->ctrl.loopdelay);
    destroy_delay (a->txdelay);
    destroy_delay (a->rxdelay);
    a->rxdelay = create_delay (
        1,                                          // run
        0,                                          // size             [stuff later]
        0,                                          // input buffer     [stuff later]
        0,                                          // output buffer    [stuff later]
        a->rate,                                    // sample rate
        20.0e-09,                                   // delta (delay stepsize)
        0.0);                                       // delay
    a->txdelay = create_delay (
        1,                                          // run
        0,                                          // size             [stuff later]
        0,                                          // input buffer     [stuff later]
        0,                                          // output buffer    [stuff later]
        a->rate,                                    // sample rate
        20.0e-09,                                   // delta (delay stepsize)
        a->txdel);                                  // delay
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSPinMode (int channel, int pin)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->pin = pin;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSMapMode (int channel, int map)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->map = map;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

PORT
void SetPSStabilize (int channel, int stbl)
{
    EnterCriticalSection (&txa[channel].calcc.cs_update);
    txa[channel].calcc.p->stbl = stbl;
    LeaveCriticalSection (&txa[channel].calcc.cs_update);
}

void ForceShutDown (CALCC a, IQC b, int timeout)
{
    a->runcal = 0;                                      // close pscc() gate
    InterlockedBitTestAndReset (&b->run, 0);            // close xiqc() gate
    Sleep (timeout);                                    // wait for anything possibly running to clear
    a->ctrl.state = LRESET;                             // set next_state
    InterlockedBitTestAndReset (&a->ctrl.running, 0);   // set running = 0
    InterlockedBitTestAndReset (&b->busy, 0);           // set busy = 0 so turnoff thread can finish & terminate
    InterlockedBitTestAndReset (&a->ctrl.calcdone, 0);
    a->info[14] = 0;
    a->ctrl.env_maxtx = 0.0;
    a->ctrl.bs_count = 0;
}

PORT
void SetPSIntsAndSpi (int channel, int ints, int spi)
{
    CALCC a = txa[channel].calcc.p;
    IQC   b = txa[channel].iqc.p1;
    if (b->ints != ints || b->dog.spi != spi || a->ints != ints || a->spi != spi)
    {
        // SHUT-DOWN
        const int timeout = 50;
        int runcal   = a->runcal;
        int mancal   = a->ctrl.mancal;
        int automode = a->ctrl.automode;
        int turnon   = a->ctrl.turnon;
        int count = 0;
        SetPSControl (a->channel, 1, 0, 0, 0);
        while (count++ < timeout && (LRESET != _InterlockedAnd (&a->ctrl.current_state, 0xFFFFFFFF)
            || _InterlockedAnd (&a->ctrl.running, 1) || _InterlockedAnd (&b->run, 1)))
            Sleep (1);                      // wait for normal shutdown (when samples are flowing)
        if (LRESET != _InterlockedAnd (&a->ctrl.current_state, 0xFFFFFFFF)
            || _InterlockedAnd (&a->ctrl.running, 1) || _InterlockedAnd (&b->run, 1))
            ForceShutDown(a, b, timeout);   // apparently no sammples flowing; force shutdown.
        // MAKE CHANGES
        desize_iqc (b);
        desize_calcc (a);
        b->ints = ints;
        b->dog.spi = spi;
        a->ints = ints;
        a->spi = spi;
        size_calcc (a);
        size_iqc (b);
        // START-UP
        SetPSControl (a->channel, 1, mancal, automode, turnon);
        a->runcal = runcal;
    }
}
