#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <math.h>
#include "matrix_calc.h"
using namespace std;

void
transMatrix(float *m0, float *m1, float *v)
{
    memcpy(m0, m1, sizeof(float)*16);

    m0[12] = m1[12] + v[0];
    m0[13] = m1[13] + v[1];
    m0[14] = m1[14] + v[2];
    m0[15] = m1[15] + v[3];
}

void
unitMatrix(float *m)
{
    bzero(m, sizeof(float)*16);

    m[0] = 1.0f;
    m[5] = 1.0f;
    m[10] = 1.0f;
    m[15] = 1.0f;
}

float
determinant(float *m) {

    float det = 1.0f;
    float buf;
    int n = 4; // 4次                                                                  

    float det_m[16];
    for (int i = 0; i < 16; i++) det_m[i] = m[i];

    // 三角行列を作成                                                                   
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            if ( i < j ) {
                buf = det_m[j*n+i] / det_m[i*n+i];
                for (int k = 0; k < n; k++) {
                    det_m[j*n+k] -= det_m[i*n+k] * buf;
                }
            }
        }
    }

    // 対角部分の積                                                                     
    for (int i = 0; i < n; i++) {
        det *= det_m[i*n+i];
    }

    return det;
}


void
inverseMatrix(float *m0, float *m1)
{
    float m[16];

    for (int i = 0; i < 4; i++) {
	for (int j = 0; j < 4; j++) {
	    m0[i*4+j] = m[i*4+j] = m1[j*4+i];
	}
    }

    m0[12] = -(m1[12]*m[0] + m1[13]*m[1] + m1[14]*m[2]);
    m0[13] = -(m1[12]*m[4] + m1[13]*m[5] + m1[14]*m[6]);
    m0[14] = -(m1[12]*m[8] + m1[13]*m[9] + m1[14]*m[10]);
    m0[3] = m0[7] = m0[11] = 0.0f;
    m0[15] = 1.0f;

}

/**
 * マトリックス m にベクトル v1 を右から乗算して、v0に与える
 * @param[out] v0 output vector (float[4])
 * @param[in] v1 input vector (float[4])
 * @param[in] m matrix (float[16])
 */
void
applyMatrix(float *v0, float *m, float *v1)
{
    for (int i = 0; i < 4; i++) {
	v0[i] = v1[0]*m[i] + v1[1]*m[i+4] + v1[2]*m[i+8] + v1[3]*m[i+12];
    }
}


void
copyMatrix(float *m0, float *m1)
{
    for (int i = 0; i < 16; i++) {
	m0[i] = m1[i];
    }
}


/**
 * ベクトルの正規化
 *
 * @param[out] v0 output vector
 * @param[in]  v1 input vector
 */
void
normalize(float *v0, float *v1)
{
    float norm, dnorm;

    norm = sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]);
    if (norm > 0) {
	dnorm = 1.0/norm;
	v0[0] = v1[0]*dnorm;
	v0[1] = v1[1]*dnorm;
	v0[2] = v1[2]*dnorm;
	v0[3] = v1[3]*dnorm;
    }
}

/**
 * ベクトルの減算 v0 = v1 - v2
 */
void
subVector(float *v0, float *v1, float *v2)
{
    v0[0] = v1[0] - v2[0];
    v0[1] = v1[1] - v2[1];
    v0[2] = v1[2] - v2[2];
    v0[3] = v1[3] - v2[3];
}

/**
 * ベクトルの外積 v0 = v1 x v2
 */
void
outerProduct(float *v0, float *v1, float *v2)
{
    v0[0] = v1[1] * v2[2] - v1[2] * v2[1];
    v0[1] = v1[2] * v2[0] - v1[0] * v2[2];
    v0[2] = v1[0] * v2[1] - v1[1] * v2[0];
    v0[3] = 0;
}

void
transposeMatrix(float *m0, float *m1)
{
    float t[16];

    // こういう小細工よりベクタ使った方が良いんだが
    for (int i = 0; i < 4; i++) {
	for (int j = i+1; j < 4; j++) {
	    float tmp = t[j*4+i];
	    t[i*4+j] = m1[j*4+i];
	    m1[j*4+i] = tmp;
	}
    }
}

/**
 * ベクトルの内積 f = v0 * v1
 */
float
innerProduct(float *v0, float *v1)
{
    return (v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2]);
}

/**
 *     xyz = xyz1 * xyz2
 */
void matrix4x4(float *xyz, float *xyz1, float *xyz2) //xyz[16]
{

  for(int t=0; t<16; t+=4)
    {
      for(int i=0; i<4; i++)
	{
	  xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
	}
    }
}

/**
 *     xyz = xyz * xyz1
 */
void mulMatrix4x4(float *xyz, float *xyz1) //xyz[16]
{

    float tmp[16];
    for(int i = 0; i<16; i++) {
        tmp[i] = xyz[i];
    }

  for(int t=0; t<16; t+=4) {
      for(int i=0; i<4; i++) {
      	  xyz[t+i] = tmp[t]*xyz1[i] + tmp[t+1]*xyz1[4+i] + tmp[t+2]*xyz1[8+i] + tmp[t+3]*xyz1[12+i];
      }
  }
}


void matrix4x4R(float xyz[16], float xyz1[16], float xyz2[16]) 
{
  for(int t=0; t<16; t+=4)
    {
      for(int i=0; i<4; i++)
	{
	  xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
	}
    }
}

/**
 *   c_xyz を中心に sacle 倍する
 */
void
scale_matrix(float *xyz, float *scale, float *c_xyz) 
{
  float xyz2[16] = {
        scale[0],  0,         0,         scale[0]*(-c_xyz[0]) + c_xyz[0],
        0,         scale[1],  0,         scale[1]*(-c_xyz[1]) + c_xyz[1],
        0,         0,         scale[2],  scale[2]*(-c_xyz[2]) + c_xyz[2],
        0,         0,         0,         1

  };
  float xyz1[16]  = {  
    xyz[0], xyz[1], xyz[2], xyz[3],
    xyz[4], xyz[5], xyz[6], xyz[7],
    xyz[8], xyz[9], xyz[10], xyz[11],
    xyz[12], xyz[13], xyz[14], xyz[15]};

  for(int t=0; t<16; t+=4)
    {
      for(int i=0; i<4; i++)
	{
	  xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
	}
    }
}

/**
       stack 上の変換行列に、相対的に、rxyz の回転、txyz の平行移動、scale の拡大を
       行ったものを matrix に代入する
 */
void
get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
{
  float radx,rady,radz;
  radx = rxyz[0]*M_PI/180;
  rady = rxyz[1]*M_PI/180;
  radz = rxyz[2]*M_PI/180;

  float sinx = sin(radx);
  float cosx = cos(radx);
  float siny = sin(rady);
  float cosy = cos(rady);
  float sinz = sin(radz);
  float cosz = cos(radz);

  float m1[16];
  float *m = stack? m1 : matrix;

  /* View Transform */
  m[0] = cosz*cosy+sinz*sinx*siny;
  m[1] = sinz*cosx;
  m[2] = -cosz*siny+sinz*sinx*cosy;
  m[3] = 0;
  m[4] = -sinz*cosy+cosz*sinx*siny;
  m[5] = cosz*cosx;
  m[6] = sinz*siny+cosz*sinx*cosy;
  m[7] = 0;
  m[8] = cosx*siny;
  m[9] = -sinx;
  m[10] = cosx*cosy;
  m[11] = 0;
  m[12] = txyz[0];
  m[13] = txyz[1];
  m[14] = txyz[2];
  m[15] = 1;
  
  if(stack)
    {
	matrix4x4(matrix, m, stack);
    }

}

void
get_matrix_scale( float *matrix, float *rxyz, float *txyz, float *scale, float *stack)
{
  float radx,rady,radz;
  radx = rxyz[0]*M_PI/180;
  rady = rxyz[1]*M_PI/180;
  radz = rxyz[2]*M_PI/180;

  float sinx = sin(radx)*scale[0];
  float cosx = cos(radx)*scale[0];
  float siny = sin(rady)*scale[1];
  float cosy = cos(rady)*scale[1];
  float sinz = sin(radz)*scale[2];
  float cosz = cos(radz)*scale[2];

  float m1[16];
  float *m = stack? m1 : matrix;

  /* View Transform */
  m[0] = cosz*cosy+sinz*sinx*siny;
  m[1] = sinz*cosx;
  m[2] = -cosz*siny+sinz*sinx*cosy;
  m[3] = 0;
  m[4] = -sinz*cosy+cosz*sinx*siny;
  m[5] = cosz*cosx;
  m[6] = sinz*siny+cosz*sinx*cosy;
  m[7] = 0;
  m[8] = cosx*siny;
  m[9] = -sinx;
  m[10] = cosx*cosy;
  m[11] = 0;
  m[12] = txyz[0];
  m[13] = txyz[1];
  m[14] = txyz[2];
  m[15] = 1;
  
  if(stack)
    {
	matrix4x4(matrix, m, stack);
    }

}

void
rotate_matrix( float m[16], float rxyz[4] )
{
  float radx,rady,radz;
  radx = rxyz[0]*M_PI/180;
  rady = rxyz[1]*M_PI/180;
  radz = rxyz[2]*M_PI/180;

  float sinx = sin(radx);
  float cosx = cos(radx);
  float siny = sin(rady);
  float cosy = cos(rady);
  float sinz = sin(radz);
  float cosz = cos(radz);

  /* View Transform */
  m[0] = cosz*cosy+sinz*sinx*siny;
  m[1] = sinz*cosx;
  m[2] = -cosz*siny+sinz*sinx*cosy;
  m[3] = 0;
  m[4] = -sinz*cosy+cosz*sinx*siny;
  m[5] = cosz*cosx;
  m[6] = sinz*siny+cosz*sinx*cosy;
  m[7] = 0;
  m[8] = cosx*siny;
  m[9] = -sinx;
  m[10] = cosx*cosy;
  m[11] = 0;
  m[12] = 0;
  m[13] = 0;
  m[14] = 0;
  m[15] = 1;
}

void rotate_x(float *xyz, float r)
{
  float rad = r*M_PI/180;

  xyz[0] = xyz[0];
  xyz[1] = xyz[1]*cos(rad) - xyz[2]*sin(rad);
  xyz[2] = xyz[1]*sin(rad) + xyz[2]*cos(rad);
}

void rotate_y(float *xyz, float r)
{
  float rad = r*M_PI/180;

  xyz[0] = xyz[0]*cos(rad) + xyz[2]*sin(rad);
  xyz[1] = xyz[1];
  xyz[2] = -xyz[0]*sin(rad) + xyz[2]*cos(rad);
}

void rotate_z(float *xyz, float r)
{
  float rad = r*M_PI/180;

  xyz[0] = xyz[0]*cos(rad) - xyz[1]*sin(rad);
  xyz[1] = xyz[0]*sin(rad) + xyz[1]*cos(rad);
  xyz[2] = xyz[2];
}

void rotate(float *xyz, float *matrix)
{
  float abc[4];
  abc[0] = xyz[0];
  abc[1] = xyz[1];
  abc[2] = xyz[2];
  abc[3] = xyz[3];

  for(int i=0; i<4; i++)
    {
      //xyz[i] = abc[0]*rot[i] + abc[1]*rot[i+4] + abc[2]*rot[i+8] + abc[3]*rot[i+12];
      xyz[i] = abc[0]*matrix[i] + abc[1]*matrix[i+4] + abc[2]*matrix[i+8] + abc[3]*matrix[i+12];
    }
}


void translate(float *xyz, float x, float y, float z)
{
  xyz[0] += x;
  xyz[1] += y;
  xyz[2] += z;
}

/**                                                                                       
 *  ベクトルに行列を乗算する                                                              
 * @param[out] v vector (float[4])                                                        
 * @param[in] m matrix (float[16])                                                        
 */
void
ApplyMatrix(float *v, float *m)
{
  float t[4];

  t[0] = v[0];
  t[1] = v[1];
  t[2] = v[2];
  t[3] = v[3];

  for (int i = 0; i < 4; i++) {
    v[i] = t[0]*m[i] + t[1]*m[i+4] + t[2]*m[i+8] + t[3]*m[i+12];
  }
}

void
ScaleMatrixXYZ(float *m, float x, float y, float z)
{
    for(int i=0;i<3;i++) {
       m[i] *= x;
       m[i+4] *= y;
       m[i+8] *= z;
    }
}