#include <stdlib.h>
#include "Mat4.h"

#define VERIFY(X)
#define ASSERT(X)

Mat4::Mat4()
{
	memset(m,0,16*sizeof(float));
	m[0] = 1; m[5] = 1; m[10] = 1; m[15] = 1; //identity
}

Mat4::Mat4(float a11, float a21, float a31, float a41,
					float a12, float a22, float a32, float a42,
					float a13, float a23, float a33, float a43,
					float a14, float a24, float a34, float a44)
{
	m[0] = a11;  m[1] = a21;  m[2] = a31;  m[3] = a41;
	m[4] = a12;  m[5] = a22;  m[6] = a32;  m[7] = a42;
	m[8] = a13;  m[9] = a23;  m[10] = a33; m[11] = a43;
	m[12] = a14; m[13] = a24; m[14] = a34; m[15] = a44;
}

Mat4::~Mat4()
{
}

void Mat4::MatVecMultiplication(float *src, float *dst)
{
	dst[0] = mtScalarP(4,&m[0],1,src,1);
	dst[1] = mtScalarP(4,&m[4],1,src,1);
	dst[2] = mtScalarP(4,&m[8],1,src,1);
	dst[3] = mtScalarP(4,&m[12],1,src,1);
}

void Mat4::SolveLinSys(float *x, float *b)
{
	mtSolve(m,4,4,b,x);	
}


//global math functions
int mtSolve(float *AOrig,int Nequat,int Nvar,float *bOrig, float *x)
{
    int         i,size,result;
    double      *A = NULL,*b = NULL ,*p = NULL, *ww = NULL,*xd = NULL;

    result = 0;
    size = Nvar*sizeof(double);
    xd = (double *)malloc(size);
    if (size == 0 || xd == NULL) goto quit;

    size = Nequat*sizeof(double);
    b = (double *)malloc(size);
    if (size == 0 || b == NULL) goto quit;

    for (i = 0; i < Nequat; i++) b[i] = bOrig[i];
    size  *= Nvar;
    if ((A = (double *)malloc(size)) == NULL) goto quit;

    for (i = 0; i < Nvar*Nequat; i++) A[i] = AOrig[i];
    ww = NULL;
    if (Nequat > Nvar)
    {
       if ((p = (double *)malloc(Nvar*sizeof(double))) == NULL) goto quit;

       result = mtLeastSquareD(1,1,Nequat,Nvar,A,p,b, xd);
    }
    else if (Nequat == Nvar)
    {
       int         n,j,k,index1,index2,index3,l,lref,siz;
       double      piv;
       double      eps2,hlp,ref;

       n = Nvar;
       eps2 = (double)(FLT_EPSILON*FLT_EPSILON);
       siz  = n*sizeof(double);
       if ((ww = (double *)malloc(siz)) == NULL) goto quit;
       index1 = 0;
       for (i = 0; i< n; i++)
       {
          ref = 0.0f; lref= -1;
          for (l = i; l < n; l++)
          {
             hlp = (double)fabs(A[l*n+i]);
             if (hlp > ref) {
                lref = l;
                ref = hlp;
             } ;
          }
          if (ref < eps2)
          {
             result = 0;
             goto quit;
          }
          if (lref != i)
          {
             hlp = b[lref];
             b[lref] = b[i];
             b[i] = hlp;
             memcpy( ww,A+n*lref,siz);
             memcpy( A+n*lref,A+n*i,siz);
             memcpy( A+n*i,ww,siz);
          }

          piv = A[index1+i];
          piv  = 1.0/piv;
          b[i] = piv*b[i];
          for (j = i+1; j < n; j++) *(A+index1+j) *= (double)piv;
          index2 = (i+1)*n;
          for (k = i+1; k < n; k++) {
             index3  = index2 + i;
             b[k]   -= A[index3]*b[i]; 
             for (j = i+1; j< n; j++)
                A[index2+j] -= A[index3] * A[index1+j];
             index2  += n;
          }
          index1 += n;
       } 
       for (j = n-1; j>= 0; j--)
       {
          k = j + 1;
          xd[j] = b[j] - mtScalarD(n-k,&A[j*n+k],1,&xd[k],1);
       }
       result = 1;
    }
    if (result == 1)
    {
       for (i = 0; i < Nvar; i++) x[i] = (float)xd[i];
    }
quit:
    if (A)  free(A);
    if (b)  free(b);
    if (xd) free(xd);
    if (ww) free(ww);
    if (p)  free(p);
    return result;
}

void mtLineLoadD(int line_second,
                 double *a_line,int column,
                 double second,double *a,double *b)
{
   int     i,j,index;
   union  {long l; double f;} temp;
   for (i = 0; i < column; i++)
   {
      temp.f = a_line[i];
      if (temp.l) {
         b[i] += temp.f * second;
         if (line_second) {
           index = i*column+i;
           for (j = i; j < column; j++) a[index++] += temp.f * a_line[j];
         } 
      } 
   } 
} 


void mtLoadSecondD(int m,int n,double *a,double *b,double *right)
{  int     i, IsSame, Len;
   double  *atb;
   if (IsSame = b==right)
   {
      Len = sizeof(double)*n;
      atb = (double *)malloc(Len);
   }
   else
      atb = right;
   for (i = 0; i < n; i++) atb[i] = mtScalarD(m,a+i,n,b,1);
   if (IsSame) { memcpy ( b,atb,Len); free(atb); }
}

void mtLoadSymetricD(int m,int n,double *a,double *asym)
{  int     i , j, index, IsSame, Len;
   double *ata;
   Len = sizeof(double)*n*n;
   if (IsSame = (a==asym))
      ata = (double *)malloc(Len);
   else
      ata = asym;
   memset( ata,0,Len);
   for (i = 0; i < n; i++)
   {
      index = i*n+i;
      for (j = i; j < n; j++)
         ata[index++] = mtScalarD(m,a+i,n,a+j,n);
   }
   if (IsSame) {memcpy( a,ata,Len); free(ata); }
}

double mtCholeskyD(int n,double *a,double *p)
{
   int     i,j,k,in,jn,index;
   double   x,y,det;
   det = 1.0;
   for (i = 0;  i < n; i++)
   {
      in = i*n;
      for (j = 0;  j <= i; j++)
      {
         jn = j*n;
         x  = a[jn+i];
         if (j == i)
         {
            index = in;
            for (k = 0;  k < j ; k++)
            {
               y         = a[index];
               a[index] *= p[k];
               x        -= y * a[index];
               index++;
            }
            if (fabs(x) > 1.0E-20)
            {
                det = det*x;
                p[i] = 1.0f/x;
            }
            else
            {
                det = 0.0;
                goto quit;
            }
         }
         else
         {
            a[in+j] = x;
            if (j) a[in+j] -= mtScalarD(j,a+jn,1,a+in,1);
         }
      }
   }
quit:
   return(det);
}


int mtLeastSquareD(int to_transpose,int to_decompose,
                   int m,int n,double *a,double *p,
                   double *b,double *sol)
{
   int     i,isc;
   int   Continue = 1;
   if (to_transpose) { mtLoadSecondD  (m,n,a,b,b); mtLoadSymetricD(m,n,a,a); }
   if (to_decompose)  Continue = mtCholeskyD(n,a,p) != 0.0f;
   if (Continue) {
      sol[0] = b[0];
      for (i = 1; i < n; i++) sol[i] = b[i]-mtScalarD(i,a+i*n,1,sol,1);
      isc = n;
      for (i = n-1; i>=0; i--) {
         sol[i] = sol[i]*p[i]-mtScalarD((int)(n-isc),a+isc*n+i,n,sol+isc,1);
         isc = i;
      } 
   } 
   return(Continue);
} 

float mtScalarP(int N,const float *x,int pas_x,const float *y,int pas_y)
{  
   double sum = 0.0;
   register int i;
   for (i = 0;  i < N; i++)
   {
      VERIFY(x != NULL); VERIFY(y != NULL);
      sum += (double)(*x) * (double)(*y);
      x += pas_x;
      y += pas_y;
   }
   return (float)(sum);
} 

double mtScalarD(int N,const double *x,int pas_x,const double *y,int pas_y)
{
   double sum = 0.;
   register int i;
   for (i = 0; i < N; i++)
   {
      VERIFY(x != NULL); VERIFY(y != NULL);
      sum += (*x) * (*y);
      x += pas_x;
      y += pas_y;
   }
   return sum;
} 

void mt3DVecProdP(const float *x,const float *y,float *result)
{
   result[0] = x[1]*y[2]-x[2]*y[1];
   result[1] = x[2]*y[0]-x[0]*y[2];
   result[2] = x[0]*y[1]-x[1]*y[0];
}

void mtBaseVecFunc(int dim,  float *v1, char func, float *v2, float *r)
{
	if (func == '+')
	{
		for (int i=0; i<dim; i++)
			r[i] = v1[i] + v2[i];
		return;
	}
	else if (func == '-')
	{
		for (int i=0; i<dim; i++)
			r[i] = v1[i] - v2[i];
		return;
	}
	else if (func == '*')
	{
		for (int i=0; i<dim; i++)
			r[i] = v1[i] * v2[0];
		return;
	}
	else if (func == '/') 
	{
		ASSERT(!mtEqual(v2[0],0.0f));
		float temp = 1.0f/v2[0];
		for (int i=0; i<dim; i++)
			r[i] = v1[i] * temp;
		return;
	}
}

void mtScalarVector(int dim,float alf,float *v1,float bet, float *v2, float *r)
{
	bool isbet = !mtEqual(bet,0.0f);
	bool mulalf = !mtEqual(alf,1.0f);
	bool mulbet = !mtEqual(bet,1.0f);
	for (int i=0; i<dim; i++)
	{
		if (mulalf)
			r[i] = alf * v1[i];
		else
			r[i] = v1[i];
		if (isbet) 
		{
         if(mulbet)
            r[i] += bet * v2[i];
			else
            r[i] += v2[i];
		}
	}
}

bool mtEqual(float a, float b) 
{
	return (fabs(a-b) <= FLT_EPSILON*fabs(a+b));
}
