//
//  matcMath.cpp
//  fpComplex
//
//  Created by Robert Delaney on 2/2/17.
//
//

#include "matcMath.hpp"
#include "matcConv.hpp"
#include "matfp.hpp"

// swap row i with row j
static void matcSwapRows(matc& x, INT32 i, INT32 j)
{
    fpComplex 		temp;
    INT32           k;
    
    for(k=0;k<x.nc;k++)
    {
        temp = x.array[i][k];
        x.array[i][k] = x.array[j][k];
        x.array[j][k] = temp;
    }
    
}/* matcSwapRows */


static bool doGauss(matc& x, INT32& swapParity, INT32 ii, INT32 jj)
{
    fpComplex   temp, temp1;
    INT32       i, j, k;
    bool        isZero;
    
    if(ii<0 || jj<0 || ii>=x.nr || jj>=x.nc)
        return false;
    
    k = 0;
    
    // is pivot zero?
    if(!x.array[ii][jj].re && !x.array[ii][jj].im)
    {
        // must swap rows
        // find non-zero element below pivot in pivot column
        isZero = true;
        for(i=ii+1;i<x.nr;++i)
        {
            if(x.array[i][jj].re!=0 || x.array[i][jj].im!=0)
            {
                isZero = false;
                k = i;
                break;
            }
        }
        
        if(isZero)
            return false;
        
        // have non-zero element in row k, so swap rows ii with k
        matcSwapRows(x, ii, k);
        swapParity = -swapParity;
    }
    
    // now we do the Gauss Elim. operation
    // pivot element xt[ii][jj] is not zero
    
    // now for every k not ii subtract row ii multiplied by xt[k][jj] from row k provided x[k][jj] != 0
    for(k=0;k<x.nr;++k)
    {
        if(k!=ii && (x.array[k][jj].re!=0 || x.array[k][jj].im!=0))
        {
            //temp = x[k][jj] / x[ii][jj];
            //DivCC(temp, x[k][jj], x[ii][jj]);
            temp = x.array[k][jj] / x.array[ii][jj];
            for(j=0;j<x.nc;++j)
            {
                //MulCC(temp1, x[ii][j], temp);
                temp1 = x.array[ii][j] * temp;
                //SubCC(x[k][j], x[k][j], temp1);
                x.array[k][j] = x.array[k][j] - temp1;
            }
        }
    }
    return true;
    
}/* doGauss */


static bool doReduce(matc& x, INT32 ii, INT32 jj)
{
    fpComplex      temp, temp1;
    INT32          i, j, k;
    bool           isZero;
    
    if(ii<0 || jj<0 || ii>=x.nr || jj>=x.nc)
        return false;
    
    k = 0; // to avoid incorrect warning
    // is pivot zero?
    if(x.array[ii][jj].re==0 && x.array[ii][jj].im==0)
    {
        // must swap rows
        // find non-zero element below pivot in pivot column
        isZero = true;
        for(i=ii+1;i<x.nr;++i)
        {
            if(x.array[i][jj].re!=0 || x.array[i][jj].im!=0)
            {
                isZero = false;
                k = i;
                break;
            }
        }
        
        if(isZero)
            return false;
        
        // have non-zero element in row k, so swap rows ii with k
        matcSwapRows(x, ii, k);
    }
    // now we do the reduce operation
    // pivot element xt.array[ii][jj] is not zero
    // divide each element of row ii by xt.array[ii][jj] making xt.array[ii][jj] = 1
    //temp.Re = x[ii][jj].Re;
    //temp.Im = x[ii][jj].Im;
    temp = x.array[ii][jj];
    for(j=0;j<x.nc;++j)
    {
        //DivCC(x[ii][j], x[ii][j], temp);
        x.array[ii][j] = x.array[ii][j] / temp;
    }
    
    // now for every k not ii subtract row ii multiplied by xt[k][jj] from row k provided x[k][jj] != 0
    for(k=0;k<x.nr;++k)
    {
        if(k!=ii && (x.array[k][jj].re!=0 || x.array[k][jj].im!=0))
        {
            //temp.Re = x[k][jj].Re;
            //temp.Im = x[k][jj].Im;
            temp = x.array[k][jj];
            for(j=0;j<x.nc;++j)
            {
                //MulCC(temp1, x[ii][j], temp);
                //SubCC(x[k][j], x[k][j], temp1);
                temp1 = x.array[ii][j] * temp;
                x.array[k][j] = x.array[k][j] - temp1;
            }
        }
    }
    return true;
    
}/* doReduce */


void add(matc& z, const matc& x, const matc& y)
{
    matc		zt;
    INT32		i, j;
    
    if(x.nr!=y.nr || x.nc!=y.nc)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] + y.array[i][j];
    
    z = zt;
    
}/* add */

void add(matc& z, const matc& x, const matfp& y)
{
    matc		zt;
    INT32		i, j;
    
    if(x.nr!=y.nr || x.nc!=y.nc)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] + y.array[i][j];
    
    z = zt;
    
}/* add */

void add(matc& z, const matfp& x, const matc& y)
{
    add(z, y, x);
    
}/* add */


void sub(matc& z, const matc& x, const matc& y)
{
    matc		zt;
    INT32		i, j;
    
    if(x.nr!=y.nr || x.nc!=y.nc)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] - y.array[i][j];
    
    z = zt;
    
}/* sub */

void sub(matc& z, const matc& x, const matfp& y)
{
    matc		zt;
    INT32		i, j;
    
    if(x.nr!=y.nr || x.nc!=y.nc)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] - y.array[i][j];
    
    z = zt;
    
}/* sub */

void sub(matc& z, const matfp& x, const matc& y)
{
    matc		zt;
    INT32		i, j;
    
    if(x.nr!=y.nr || x.nc!=y.nc)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] - y.array[i][j];
    
    z = zt;
    
}/* sub */


void mul(matc& z, const matc& x, const matc& y)
{
    matc		zt;
    INT32		i, j, k;
    
    if(x.nc!=y.nr)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
        {
            zt.array[i][j].re = 0;
            zt.array[i][j].im = 0;
            for(k=0;k<x.nc;k++)
                zt.array[i][j] += x.array[i][k] * y.array[k][j];
        }
    
    z = zt;
    
}/* mul */

void mul(matc& z, const matc& x, const matfp& y)
{
    matc		zt;
    INT32		i, j, k;
    
    if(x.nc!=y.nr)
        return;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
        {
            zt.array[i][j].re = 0;
            zt.array[i][j].im = 0;
            for(k=0;k<x.nc;k++)
                zt.array[i][j] += x.array[i][k] * y.array[k][j];
        }
    
    z = zt;
    
}/* mul */

void mul(matc& z, const matfp& x, const matc& y)
{
    mul(z, y, x);
    
}/* mul */


void mul(matc& z, const matc& x, const fp& y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = y * x.array[i][j];
    
    z = zt;
    
}/* mul */

void div(matc& z, const matc& x, const fp& y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] / y;
    
    z = zt;
    
}/* div */

void mul(matc& z, const fp& x, const matc& y)
{
    mul(z, y, x);
    
}/* mul */

void mul(matc& z, const matc& x, const mb&  y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = y * x.array[i][j];
    
    z = zt;
    
}/* mul */

void div(matc& z, const matc& x, const mb& y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] / y;
    
    z = zt;
    
}/* div */

void mul(matc& z, const mb& x, const matc& y)
{
    mul(z, y, x);
    
}/* mul */


void mul(matc& z, const matc& x, double y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = y * x.array[i][j];
    
    z = zt;
    
}/* mul */

void div(matc& z, const matc& x, double y)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[i][j] / y;
    
    z = zt;
    
}/* div */

void mul(matc& z, double x, const matc& y)
{
    mul(z, y, x);
    
}/* mul */

// creates unit matrix of size n
void matcUnit(matc& z, INT32 n)
{
    INT32		i, j;
    
    init(z, n, n);
    
    for(i=0;i<z.nr;i++)
        for(j=0;j<z.nc;j++)
        {
            if(i==j)
                z.array[i][j] = 1.;
            else
                z.array[i][j] = 0.;
        }
    
}/* matcUnit */

matc matcUnit(INT32 n)
{
    matc		z;
    
    matcUnit(z, n);
    
    return z;
    
}/* matcUnit */

void transpose(matc& z, const matc& x)
{
    matc		zt;
    INT32		i, j;
    
    init(zt, x.nr, x.nc);
    
    for(i=0;i<zt.nr;i++)
        for(j=0;j<zt.nc;j++)
            zt.array[i][j] = x.array[j][i];
    
    z = zt;
    
}/* transpose */


matc transpose(const matc& x)
{
    matc		z;
    
    transpose(z, x);
    
    return z;
    
}/* transpose */

// x must be square
bool trace(fpComplex& z, const matc& x)
{
    fpComplex	zt;
    INT32		i;
    
    if(x.nr != x.nc)
    {
        z.re = 0.;
        z.im = 0.;
        return false;
    }
    
    zt.re = 0.;
    zt.im = 0.;
    
    for(i=0;i<x.nr;i++)
        zt+=x.array[i][i];
    
    z = zt;
    
    return true;
    
}/* trace */


fpComplex trace(const matc& x)
{
    fpComplex		z;
    
    if(trace(z, x))
    {
        return z;
    }
    else
    {
        z.re = 0.;
        z.im = 0.;
        return z;
    }
    
}/* trace */


// puts z.array in echelon form
void matcGaussElim(matc& z, INT32& swapParity, const matc& x)
{
    matc        xt;
    INT32       ii, jj;
    
    swapParity = 1;
    
    init(xt, x.nr, x.nc);
    
    xt = x;
    
    ii = jj = 0;
    while(ii<xt.nr && jj<xt.nc)
    {
        if(doGauss(xt, swapParity, ii, jj))
        {
            ii++;
            jj++;
        }
        else
            jj++;
    }
    
    z = xt;
    
}/* matcGaussElim */


// reduced echelon form; x must be in echolon form
void matcReduce(matc& z, const matc& x)
{
    matc            xt;
    INT32           ii, jj;
    
    init(xt, x.nr, x.nc);
    
    xt = x;
    
    ii = jj = 0;
    while(ii<xt.nr && jj<xt.nc)
    {
        if(doReduce(xt, ii, jj))
        {
            ii++;
            jj++;
        }
        else
            jj++;
    }
    z = xt;
    
}/* matcReduce */


// z = inverse(x)
void inv(matc& z, fpComplex& determinant, const matc& x)
{
    matc            xt;
    INT32           n, i, j, k, L, *ik, *jk;
    fpComplex       amax, save, zero=0., one = 1.;
    
    if(x.nr!=x.nc)
        return;
    
    n = x.nr;
    
    init(xt, x.nr, x.nc);
    xt = x;
    
    ik = (INT32 *)malloc(n*sizeof(INT32));
    jk = (INT32 *)malloc(n*sizeof(INT32));
    
    determinant = one;
    
    for(k=0; k<n; k++)
    {
        /* find largest element in abs value in rest of matrix */
        amax = zero;
        
        for(i=k; i<n; i++)
        {
            for(j=k; j<n; j++)
            {
                if(abs(xt.array[i][j]) > abs(amax))
                {
                    amax = xt.array[i][j];
                    ik[k] = i;
                    jk[k] = j;
                }
            }
        }
        
        if(!abs(amax))
        {
            z.nr = z.nc = 0;
            z.array = NULL;
            determinant.re = 0;
            determinant.im = 0;
            return;
        }
        
        /* interchange rows and columns to put amax in array(k,k) */
        i = ik[k];
        if(i>k)
        {
            for(j=0; j<n; j++)
            {
                save = xt.array[k][j];
                xt.array[k][j] = xt.array[i][j];
                xt.array[i][j] = -save;
            }
        }
        
        j = jk[k];
        if(j>k)
        {
            for(i=0; i<n; i++)
            {
                save = xt.array[i][k];
                xt.array[i][k] = xt.array[i][j];
                xt.array[i][j] = -save;
            }
        }
        
        /* accumulate elements of inverse matrix */
        for(i=0; i<n; i++)
        {
            if(i!=k)
                xt.array[i][k] = -xt.array[i][k]/amax;
        }
        
        for(i=0; i<n; i++)
        {
            for(j=0; j<n; j++)
            {
                if(i!=k && j!=k)
                    xt.array[i][j] = xt.array[i][j] + xt.array[i][k]*xt.array[k][j];
            }
        }
        
        for(j=0; j<n; j++)
        {
            if(j!=k)
                xt.array[k][j] = xt.array[k][j]/amax;
        }
        
        xt.array[k][k] = one/amax;
        determinant = determinant*amax;
    }
    
    /* restore ordering of the matrix */
    for(L=0; L<n; L++)
    {
        k = n - L - 1;
        j = ik[k];
        
        if(j>k)
        {
            for(i=0; i<n; i++)
            {
                save = xt.array[i][k];
                xt.array[i][k] = -xt.array[i][j];
                xt.array[i][j] = save;
            }
        }
        
        i = jk[k];
        
        if(i>k)
        {
            for(j=0; j<n; j++)
            {
                save = xt.array[k][j];
                xt.array[k][j] = -xt.array[i][j];
                xt.array[i][j] = save;
            }
        }
    }
    
    z = xt;
    
    free(ik);
    free(jk);
    
} /* inv */

matc inv(const matc& x)
{
    fpComplex      det;
    matc   z;
    inv(z, det, x);
    
    return z;
    
}/* inv */

fpComplex determinant(const matc& x)
{
    matc            z;
    fpComplex      det;
    
    inv(z, det, x);
    
    return det;
    
}/* determinant */


