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

void noMoreMemory()
{
  cout << "can't allocate memory\n";
  exit(1);
}

/**
 * ベクトルの正規化
 *
 * @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;
}

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

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];
	}
    }
}

void get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
{
  float radx,rady,radz;
  radx = rxyz[0]*3.14/180;
  rady = rxyz[1]*3.14/180;
  radz = rxyz[2]*3.14/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 */
  matrix[0] = cosz*cosy+sinz*sinx*siny;
  matrix[1] = sinz*cosx;
  matrix[2] = -cosz*siny+sinz*sinx*cosy;
  matrix[3] = 0;
  matrix[4] = -sinz*cosy+cosz*sinx*siny;
  matrix[5] = cosz*cosx;
  matrix[6] = sinz*siny+cosz*sinx*cosy;
  matrix[7] = 0;
  matrix[8] = cosx*siny;
  matrix[9] = -sinx;
  matrix[10] = cosx*cosy;
  matrix[11] = 0;
  matrix[12] = txyz[0];
  matrix[13] = txyz[1];
  matrix[14] = txyz[2];
  matrix[15] = 1;
  
  float m[16];

  for(int i=0; i<16; i++)
    {
      m[i] = matrix[i];
    }

  if(stack)
    {
	matrix4x4(matrix, m, stack);
    }

}

void rotate_x(float *xyz, float r)
{
  float rad = r*3.14/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*3.14/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*3.14/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;
}