#include <string.h>
#include <math.h>
#include "xgalaxy.h"

void
vector_sub(Vector *res, Vector *a, Vector *b) {
	res->x =  a->x - b->x;
	res->y =  a->y - b->y;
	res->z =  a->z - b->z;
}

void
vector_add(Vector *res, Vector *a, Vector *b) {
	res->x =  a->x + b->x;
	res->y =  a->y + b->y;
	res->z =  a->z + b->z;
}

void
vector_vmul(Vector *res, Vector *a, Vector *b) {
        res->x =  a->y * b->z - b->y * a->z;
        res->y =  a->z * b->x - b->z * a->x;
        res->z =  a->x * b->y - b->x * a->y;
}

double
vector_smul(Vector *a, Vector *b) {
        double res;
        res =  a->x * b->x;
        res+=  a->y * b->y;
        res+=  a->z * b->z;
        return res;
}

double
vector_length(Vector *v) {
	return sqrt( v->x*v->x + v->y*v->y + v->z*v->z );
}

void
vector_mul(Vector *res, Vector *a, double b) {
        res->x =  a->x * b;
        res->y =  a->y * b;
        res->z =  a->z * b;
}

void
quater_vmul(Quaternion *res, Quaternion *a, Quaternion *b) {
        Vector  aXb, wb, Wa;

        vector_vmul(&aXb, &(a->d), &(b->d));
        vector_mul(&wb, &(b->d), a->w);
        vector_mul(&Wa, &(a->d), b->w);

        res->d.x = aXb.x + wb.x + Wa.x;
        res->d.y = aXb.y + wb.y + Wa.y;
        res->d.z = aXb.z + wb.z + Wa.z;

                 /*XXX*/
        res->w = a->w * b->w - vector_smul( &(a->d),  &(b->d) );
}

double
quater_length(Quaternion *a) {
	return sqrt(a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w);
}

void
quater_inverse(Quaternion *res, Quaternion *a) {
        double norm = a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w;
        res->d.x = -a->d.x/norm;
        res->d.y = -a->d.y/norm;
        res->d.z = -a->d.z/norm;
        res->w   =  a->w  /norm;
}

void
rotate(Vector *res, Vector *v, Quaternion *q) {
        Quaternion V, qV, Q, R;

        memcpy( &(V.d), v, sizeof(Vector) );
        V.w = 0;

        quater_vmul(&qV, q, &V);
        quater_inverse(&Q, q);
        quater_vmul(&R, &qV, &Q);

        memcpy( res, &(R.d), sizeof(Vector) );
}

void
quater_normalize(Quaternion *q) {
    	double mag;

    	mag = sqrt(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w);
	q->d.x /= mag;
	q->d.y /= mag;
	q->d.z /= mag;
	q->w   /= mag;
}

void
vector_normal(Vector *v) { 
	vector_mul( v, v, 1.0/ vector_length(v) ); 
}

void 
quater_to_matrix( Quaternion *q, double m[3][3]  ) {
        double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
        double s  = 2.0/(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w);  // 4 mul 3 add 1 div
        x2 = q->d.x * s;    y2 = q->d.y * s;    z2 = q->d.z * s;
        xx = q->d.x * x2;   xy = q->d.x * y2;   xz = q->d.x * z2;
        yy = q->d.y * y2;   yz = q->d.y * z2;   zz = q->d.z * z2;
        wx =   q->w * x2;   wy =   q->w * y2;   wz =   q->w * z2;

        m[0][0] = 1.0 - (yy + zz);
        m[1][0] = xy - wz;
        m[2][0] = xz + wy;

        m[0][1] = xy + wz;
        m[1][1] = 1.0 - (xx + zz);
        m[2][1] = yz - wx;

        m[0][2] = xz - wy;
        m[1][2] = yz + wx;
        m[2][2] = 1.0 - (xx + yy);
}

void 
matrix_to_quater(  double m[3][3], Quaternion *q ) {
        double tr = m[0][0] + m[1][1] + m[2][2]; // trace of martix
        if (tr > 0.0){     // if trace positive than "w" is biggest component
            	q->d.x = m[1][2] - m[2][1];
	    	q->d.y = m[2][0] - m[0][2];
	    	q->d.z = m[0][1] - m[1][0];
	    	q->w   =  tr+1.0;
        }else  if( (m[0][0] > m[1][1] ) && ( m[0][0] > m[2][2]) ) {
            	q->d.x = 1.0 + m[0][0] - m[1][1] - m[2][2];
	    	q->d.y = m[1][0] + m[0][1];
	    	q->d.z = m[2][0] + m[0][2];
	    	q->w   = m[1][2] - m[2][1];
        } else if ( m[1][1] > m[2][2] ){
            	q->d.x = m[1][0] + m[0][1];
	    	q->d.y = 1.0 + m[1][1] - m[0][0] - m[2][2];
	    	q->d.z = m[2][1] + m[1][2];
		q->w   = m[2][0] - m[0][2];
       	} else {
      		q->d.x = m[2][0] + m[0][2];
    		q->d.y = m[2][1] + m[1][2];
    		q->d.z = 1.0 + m[2][2] - m[0][0] - m[1][1];
    		q->w   = m[0][1] - m[1][0];
	}
	quater_normalize(q);
}

void
quater_to_angles( Quaternion *q, Vector *a) {
	double	m[3][3];
	double D,C,trx,try;

	quater_to_matrix(q, m);

	D = a->y = asin(m[2][0]);
	C =  cos( a->y );

	if ( fabs(C) > 1e-8 ) {
		trx =  m[2][2] / C;
		try = -m[2][1]  / C;
		a->x = atan2( try, trx );

		trx =  m[0][0] / C;
		try = -m[1][0] / C;
		a->z =  atan2( try, trx );
	} else {
		a->x = 0.0;
		trx      = m[1][1];
		try      = m[0][1];

		a->z  = atan2( try, trx );
	}
}