#include <stdio.h>
#include <assert.h>
#include "../../src/V/Vlibmath.h"

static int err = 0;

#define EPSILON 1e-6


static int VPointEquals(VPoint *a, VPoint *b)
{
	return fabs(a->x - b->x) < EPSILON
	    && fabs(a->y - b->y) < EPSILON
	    && fabs(a->z - b->z) < EPSILON;
}


/**
 * Transforms the point according to the matrix and compares with expected.
 * @param line Line of this source (for error reporting).
 * @param a Point to transform.
 * @param m Transformation matrix.
 * @param exp Expected transformed point.
 */
static void transform_test(int line, VPoint *a, VMatrix *m, VPoint *exp)
{
	VPoint got;
	
	VTransform(a, m, &got);
	if( ! VPointEquals(exp, &got) ){
		err++;
		printf("in line %d\n\tgot {%g, %g, %g}\n\texp {%g, %g, %g}\n",
			line, got.x, got.y, got.z, exp->x, exp->y, exp->z);
	}
}


static void eulerangles_test(int line, VMatrix *m, double exp_phi, double exp_theta, double exp_psi)
{
	double got_phi, got_theta, got_psi;
	
	VMatrixToEuler(m, &got_phi, &got_theta, &got_psi);
	if( !( fabs(got_phi   - exp_phi  ) < EPSILON
	&&     fabs(got_theta - exp_theta) < EPSILON
	&&     fabs(got_psi   - exp_psi  ) < EPSILON
	)){
		err++;
		printf("in line %d\n\tgot phi,theta,psi=%g, %g, %g\n\texp phi,theta,psi=%g, %g, %g\n",
			line,
			got_phi, got_theta, got_psi,
			exp_phi, exp_theta, exp_psi);
	}
}


static void rotateAroundAxis_test()
{
	VMatrix m;
	
	// Rotating (1,0,0) by 90 DEG around z axis gives (0,1,0):
	VIdentMatrix(&m);
	VRotateAroundAxis(&m, &(VPoint){0,0,1}, M_PI/2);
	VPoint p = (VPoint){1,0,0};
	VTransform(&p, &m, &p);
	VSub(&p, &(VPoint){0,1,0}, &p);
	assert(VMagnitude(&p) < 1e-15);
	
	// Rotating (0,0,1) by 90 DEG around y axis gives (1,0,0):
	VIdentMatrix(&m);
	VRotateAroundAxis(&m, &(VPoint){0,1,0}, M_PI/2);
	p = (VPoint){0,0,1};
	VTransform(&p, &m, &p);
	VSub(&p, &(VPoint){1,0,0}, &p);
	assert(VMagnitude(&p) < 1e-15);
	
	// Rotating (0,1,0) by 90 DEG around x axis gives (0,0,1):
	VIdentMatrix(&m);
	VRotateAroundAxis(&m, &(VPoint){1,0,0}, M_PI/2);
	p = (VPoint){0,1,0};
	VTransform(&p, &m, &p);
	VSub(&p, &(VPoint){0,0,1}, &p);
	assert(VMagnitude(&p) < 1e-15);
}


int main(int argc, char **argv)
{
	VMatrix m;
	double sin60;
	
	/* Identity matrix transforms {1,1,1} in itself: */
	VEulerToMatrix(0.0, 0.0, 0.0, &m);
	transform_test(__LINE__, &(VPoint){1.0, 1.0, 1.0}, &m, &(VPoint){1.0, 1.0, 1.0});
	
	/* Roll +90 DEG: */
	VEulerToMatrix(M_PI/2, 0.0, 0.0, &m);
	transform_test(__LINE__, &(VPoint){1.0, 0.0, 0.0}, &m, &(VPoint){1.0, 0.0, 0.0});
	transform_test(__LINE__, &(VPoint){0.0, 1.0, 0.0}, &m, &(VPoint){0.0, 0.0, 1.0});
	transform_test(__LINE__, &(VPoint){0.0, 0.0, 1.0}, &m, &(VPoint){0.0, -1.0, 0.0});
	eulerangles_test(__LINE__, &m, M_PI/2, 0.0, 0.0);
	
	/* Pitch +60 DEG: */
	VEulerToMatrix(0.0, M_PI/3, 0.0, &m);
	sin60 = sin(M_PI/3);
	transform_test(__LINE__, &(VPoint){1.0, 0.0, 0.0}, &m, &(VPoint){0.5, 0.0, -sin60});
	transform_test(__LINE__, &(VPoint){0.0, 1.0, 0.0}, &m, &(VPoint){0.0, 1.0, 0.0});
	transform_test(__LINE__, &(VPoint){0.0, 0.0, 1.0}, &m, &(VPoint){sin60, 0.0, 0.5});
	eulerangles_test(__LINE__, &m, 0.0, M_PI/3, 0.0);
	
	/* Yaw +90 DEG: */
	VEulerToMatrix(0.0, 0.0, M_PI/2, &m);
	transform_test(__LINE__, &(VPoint){1.0, 0.0, 0.0}, &m, &(VPoint){0.0, 1.0, 0.0});
	transform_test(__LINE__, &(VPoint){0.0, 1.0, 0.0}, &m, &(VPoint){-1.0, 0.0, 0.0});
	transform_test(__LINE__, &(VPoint){0.0, 0.0, 1.0}, &m, &(VPoint){0.0, 0.0, 1.0});
	eulerangles_test(__LINE__, &m, 0.0, 0.0, M_PI/2);
	
	rotateAroundAxis_test();
	
	return err == 0? 0 : 1;
}
