/*  div.c

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

Copyright (C) 2014 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

*/

#include "comm.h"

#define MAX_NR  (8)     // maximum number of receivers to mix

MDIV create_div (int run, int nr, int size, double **in, double *out)
{
    int i;
    MDIV a = (MDIV) malloc0 (sizeof (mdiv));
    a->run = run;
    a->nr = nr;
    a->size = size;
    a->out = out;
    a->in = (double **) malloc0 ( MAX_NR * sizeof (double *));
    if (in != 0)
        for (i = 0; i < nr; i++) a->in[i] = in[i];
    a->Irotate = (double *) malloc0 (MAX_NR * sizeof (double));
    a->Qrotate = (double *) malloc0 (MAX_NR * sizeof (double));
    InitializeCriticalSectionAndSpinCount (&a->cs_update, 2500);
    for (i = 0; i < 4; i++)                                                                                 ///////////// legacy interface - remove
        a->legacy[i] = (double *) malloc0 (2048 * sizeof (complex));                                        ///////////// legacy interface - remove
    return a;
}

void destroy_div (MDIV a)
{
    int i;                                                                                                  ///////////// legacy interface - remove
    DeleteCriticalSection (&a->cs_update);
    for (i = 0; i < 4; i++)                                                                                 ///////////// legacy interface - remove
        _aligned_free (a->legacy[i]);                                                                       ///////////// legacy interface - remove
    _aligned_free (a->Qrotate);
    _aligned_free (a->Irotate);
    _aligned_free (a->in);
    _aligned_free (a);
}

void flush_div (MDIV a)
{

}

void xdiv (MDIV a)
{
    if (a->run)
    {
        EnterCriticalSection (&a->cs_update);
        if (a->output != a->nr)
        {
            if (a->out != a->in[a->output])
                memcpy (a->out, a->in[a->output], a->size * sizeof (complex));
        }
        else
        {
            int i, j;
            double I, Q;
            memset (a->out, 0, a->size * sizeof (complex));
            for (i = 0; i < a->nr; i++)
                for (j = 0; j < a->size; j++)
                {
                    I = a->in[i][2 * j + 0];
                    Q = a->in[i][2 * j + 1];
                    a->out[2 * j + 0] += a->Irotate[i] * I - a->Qrotate[i] * Q;
                    a->out[2 * j + 1] += a->Irotate[i] * Q + a->Qrotate[i] * I;
                }
        }
        LeaveCriticalSection (&a->cs_update);
    }
    else
        memcpy (a->out, a->in[0], a->size * sizeof (complex));
}


/********************************************************************************************************
*                                                                                                       *
*                                       CALLS FOR EXTERNAL USE                                          *
*                                                                                                       *
********************************************************************************************************/

#define MAX_EXT_DIVS    (2)                         // maximum number of DIVs called from outside wdsp
__declspec (align (16)) MDIV pdiv[MAX_EXT_DIVS];    // array of pointers for DIVs used EXTERNAL to wdsp

PORT
void create_divEXT (int id, int run, int nr, int size)
{
    pdiv[id] = create_div (run, nr, size, 0, 0);
}

PORT
void destroy_divEXT (int id)
{
    destroy_div (pdiv[id]);
}

PORT
void flush_divEXT (int id)
{
    flush_div (pdiv[id]);
}

PORT
void xdivEXT (int id, int nsamples, double **in, double *out)
{
    int i;
    MDIV a = pdiv[id];
    a->size = nsamples;
    a->out = out;
    for (i = 0; i < a->nr; i++) a->in[i] = in[i];
    xdiv (a);
}

// 0 - does nothing; 1 - operates
PORT
void SetEXTDIVRun (int id, int run)
{
    MDIV a = pdiv[id];
    EnterCriticalSection (&a->cs_update);
    a->run = run;
    LeaveCriticalSection (&a->cs_update);
}

// size of data buffer in complex samples
PORT
void SetEXTDIVBuffsize (int id, int size)
{
    MDIV a = pdiv[id];
    EnterCriticalSection (&a->cs_update);
    a->size = size;
    LeaveCriticalSection (&a->cs_update);
}

// number of receivers being used for diversity
PORT
void SetEXTDIVNr (int id, int nr)
{
    MDIV a = pdiv[id];
    EnterCriticalSection (&a->cs_update);
    a->nr = nr;
    LeaveCriticalSection (&a->cs_update);
}

// number of which receiver to output
//  if output==nr, mixing occurs
PORT
void SetEXTDIVOutput (int id, int output)
{
    MDIV a = pdiv[id];
    EnterCriticalSection (&a->cs_update);
    a->output = output;
    LeaveCriticalSection (&a->cs_update);
}

// I and Q "rotate" multipliers for each receiver
//  can be set to 1.0 and 0.0 for "reference receiver"
PORT
void SetEXTDIVRotate (int id, int nr, double *Irotate, double *Qrotate)
{
    MDIV a = pdiv[id];
    EnterCriticalSection (&a->cs_update);
    memcpy (a->Irotate, Irotate, nr * sizeof (double));
    memcpy (a->Qrotate, Qrotate, nr * sizeof (double));
    LeaveCriticalSection (&a->cs_update);
}

/********************************************************************************************************
*                                                                                                       *
*                                     LEGACY INTERFACE - REMOVE                                         *
*                                                                                                       *
********************************************************************************************************/
PORT
void xdivEXTF (int id, int size, float **input, float *Iout, float *Qout)
{
    int i, j;
    MDIV a = pdiv[id];
    if (a->run)
    {
        a->size = size;
        for (i = 0; i < a->nr; i++)
        {
            for (j = 0; j < a->size; j++)
            {
                a->legacy[i][2 * j + 0] = (double)input[2 * i + 0][j];
                a->legacy[i][2 * j + 1] = (double)input[2 * i + 1][j];
            }
            a->in[i] = a->legacy[i];
        }
        a->out = a->legacy[3];
        xdiv (a);
        for (j = 0; j < a->size; j++)
        {
            Iout[j] = (float)a->legacy[3][2 * j + 0];
            Qout[j] = (float)a->legacy[3][2 * j + 1];
        }
    }
}
