//
//  matfpMath.cpp
//  fp
//
//  Created by Robert Delaney on 2/1/17.
//
//

#include "fp.h"
#include "matfpMath.hpp"
//#include "mb.h"
#include "matfp.hpp"
#include "matfpConv.hpp"

static fp abs(fp& x)
{
    fp      z;
    
    z = x;
    if(z<0)
        z.i.n = -z.i.n; // so don't create a new instance
    
    return z;
    
}/* abs */

// swap row i with row j
static void matfpSwapRows(matfp& x, INT32 i, INT32 j)
{
    fp		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;
    }
    
}/* matqSwapRows */


static bool doGauss(matfp& x, INT32& swapParity, INT32 ii, INT32 jj)
{
    fp          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])
    {
        // 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]!=0)
            {
                isZero = false;
                k = i;
                break;
            }
        }
        
        if(isZero)
            return false;
        
        // have non-zero element in row k, so swap rows ii with k
        matfpSwapRows(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]!=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(matfp& x, INT32 ii, INT32 jj)
{
    fp             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]==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]!=0)
            {
                isZero = false;
                k = i;
                break;
            }
        }
        
        if(isZero)
            return false;
        
        // have non-zero element in row k, so swap rows ii with k
        matfpSwapRows(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]!=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(matfp& z, const matfp& x, const matfp& y)
{
    matfp		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 sub(matfp& z, const matfp& x, const matfp& y)
{
    matfp		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(matfp& z, const matfp& x, const matfp& y)
{
    matfp		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] = 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(matfp& z, const matfp& x, const fp& y)
{
    matfp		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(matfp& z, const matfp& x, const fp& y)
{
    matfp		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(matfp& z, const fp& x, const matfp& y)
{
    mul(z, y, x);
    
}/* mul */

void mul(matfp& z, const matfp& x, const mb&  y)
{
    matfp		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(matfp& z, const matfp& x, const mb& y)
{
    matfp		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(matfp& z, const mb& x, const matfp& y)
{
    mul(z, y, x);
    
}/* mul */


void mul(matfp& z, const matfp& x, double y)
{
    matfp		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(matfp& z, const matfp& x, double y)
{
    matfp		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(matfp& z, double x, const matfp& y)
{
    mul(z, y, x);
    
}/* mul */

// creates unit matrix of size n
void matfpUnit(matfp& 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.;
        }
    
}/* matfpUnit */

matfp matfpUnit(INT32 n)
{
    matfp		z;
    
    matfpUnit(z, n);
    
    return z;
    
}/* matfpUnit */

void transpose(matfp& z, const matfp& x)
{
    matfp		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 */


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

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

fp trace(const matfp& x)
{
    fp		z;
    
    if(trace(z, x))
    {
        return z;
    }
    else
    {
        z = 0;
        return z;
    }
    
}/* trace */

// puts z.array in echelon form
void matfpGaussElim(matfp& z, INT32& swapParity, const matfp& x)
{
    matfp       xt;
    INT32       i, j, 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;
    
}/* matfpGaussElim */


// reduced echelon form; x must be in echolon form
void matfpReduce(matfp& z, const matfp& x)
{
    matfp           xt;
    INT32           i, 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;
    
}/* matfpReduce */

// z = inverse(x)
void inv(matfp& z, fp& determinant, const matfp& x)
{
    matfp   xt;
    INT32	n, i, j, k, L, *ik, *jk;
    fp      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) == zero)
        {
            z.nr = z.nc = 0;
            z.array = NULL;
            determinant = 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 */

matfp inv(const matfp& x)
{
    fp      det;
    matfp   z;
    inv(z, det, x);
    
    return z;
    
}/* inv */

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

