/*
 *  Copyright (c) 2004-2013 Gert Wollny <gw.fossdev@gmail.com>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 *  As an exception to this license, "NEC C&C Research Labs" may use
 *  this software under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation.
 *
 */

#include "mialm.h"
#include <math.h>

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

#ifndef NDEBUG
extern void print_vectors_left(void); 
extern void print_quats_left(void);
extern void print_camera_left(void);
#endif

gboolean
mia_vector3d_test (void)
{
	gboolean result = FALSE;
	gfloat f; 
	const float sqrt14 = sqrt(14.0f);
	MiaVector3d *dup; 
	MiaVector3d *test = mia_vector3d_new (1.0, 2.0, 3.0);
	MiaVector3d *test2 =  mia_vector3d_new (2.0, 5.0, 7.0);
	
	g_return_val_if_fail (test, FALSE);
	g_return_val_if_fail (test2, FALSE);
	
	g_assert(MIA_IS_VECTOR3D (test));
	g_assert(test->x == 1.0 && test->y == 2.0 && test->z == 3.0);
	
	dup = mia_vector3d_dup(test);
	g_assert(MIA_IS_VECTOR3D (dup));
	g_assert(mia_vector3d_equal(test, dup));
	
	
	
	mia_vector3d_addup(test, test2);
	g_assert(test->x == 3.0 && test->y == 7.0 && test->z == 10.0);
	
	mia_vector3d_scale(test, 0.5);
	g_assert(test->x == 1.5 && test->y == 3.5 && test->z == 5.0);
	
	g_assert(mia_vector3d_dot(dup,test2) == 33.0);
	
	f = mia_vector3d_get_norm (dup);
	
	g_assert(fabs(f - sqrt(14.0)) < 1e-5);
	
	mia_vector3d_normalize (dup);
	
	f = mia_vector3d_get_norm(dup);
	g_assert( fabs(f - 1.0f) < 1e-5 );
		
	
	g_assert(fabs (dup->x - (1.0f / sqrt14)) < 1e-5 && 
	         fabs (dup->y - (2.0f / sqrt14)) < 1e-5 && 
	         fabs (dup->z - (3.0f / sqrt14)) < 1e-5 );
	
	
	mia_vector3d_copy(dup, test2);
	g_assert(dup->x == 2.0 && dup->y == 5.0 && dup->z == 7.0);
	
	mia_vector3d_add(test, test2, dup);
	g_assert(dup->x == 3.5 && dup->y == 8.5 && dup->z == 12.0);

	g_object_unref (G_OBJECT (dup));
	
	dup = mia_vector3d_add(test, test2, NULL);
	g_assert(dup->x == 3.5 && dup->y == 8.5 && dup->z == 12.0);

	g_object_unref (G_OBJECT (test));
	g_object_unref (G_OBJECT (test2));
	g_object_unref (G_OBJECT (dup));
	return result;
}


MiaCamera *
create_camera ()
{
	MiaVector3d *cloc;
	MiaQuaternion *crot;

	cloc = mia_vector3d_new (10.1, -20.2, 400.1);
	crot = mia_quaternion_new (12.1,0.543, 13.3, 1.4);
	return mia_camera_new (cloc, crot, 20.12);
}

MiaLandmark *
create_landmark (gchar *name)
{
	MiaCamera *camera = create_camera ();
	MiaVector3d *loc = mia_vector3d_new (-11.34, 21.12, -50.20);

	return mia_landmark_new (name,loc, 128.7, camera, "picfile.jpg");
}



gboolean
compare_camera (MiaCamera * c1, MiaCamera * c2)
{
	if (!mia_vector3d_equal (mia_camera_get_location (c1),
				 mia_camera_get_location (c2)))
		return FALSE;
	if (!mia_quaternion_equal (mia_camera_get_rotation (c1),
				   mia_camera_get_rotation (c2)))
		return FALSE;
	return mia_camera_get_zoom (c1) == mia_camera_get_zoom (c2);
}

gboolean
compare_landmarks (MiaLandmark * lm1, MiaLandmark * lm2)
{
	if (mia_landmark_get_iso_value (lm1) !=
	    mia_landmark_get_iso_value (lm2))
		return FALSE;

	if (!mia_vector3d_equal (mia_landmark_get_location (lm1),
				 mia_landmark_get_location (lm2)))
		return FALSE;
	if (!compare_camera (mia_landmark_get_camera (lm1),
			     mia_landmark_get_camera (lm2)))
		return FALSE;
	if (strcmp (mia_landmark_get_picfile (lm1),
		    mia_landmark_get_picfile (lm2)))
		return FALSE;
	return !strcmp (mia_landmark_get_name (lm1),
			mia_landmark_get_name (lm2));
}

void check_camera(void)
{
	MiaCamera *c = create_camera ();
	g_object_unref(c);
#ifndef NDEBUG	
	print_vectors_left();
	print_quats_left();
	print_camera_left();
#endif	
}

void check_landmark(void)
{
	MiaLandmark *lm = create_landmark("test");
	g_object_unref(lm);
#ifndef NDEBUG	
	print_vectors_left();
	print_quats_left();
	print_camera_left();
#endif	
}


void check_landmarklist(void)
{
	MiaLandmark *lm;
	MiaLandmarklist *lml = mia_landmarklist_new ("P1TEST");
	
	lm = create_landmark("A");
	mia_landmarklist_insert(lml, lm);
	lm = create_landmark("B");
	mia_landmarklist_insert(lml, lm);
	
	g_object_unref(lml);
#ifndef NDEBUG	
	print_vectors_left();
	print_quats_left();
	print_camera_left();
#endif	
}


int
main (int UNUSED(argc), char **UNUSED(args))
{
	MiaLandmarklist *lml1;
	MiaLandmarklist *lml2;
	MiaLandmark *lm; 
	MiaLandmark *lm2; 
	const gchar *lmname; 
	
	g_type_init ();
	
	check_camera();
	check_landmark();
	check_landmarklist();
	
	mia_vector3d_test();

	lml1 = mia_landmarklist_new ("P1TEST");

	lm = create_landmark ("Landmark 1");
	lmname =mia_landmark_get_name(lm); 
	
	mia_landmarklist_insert(lml1, lm);
	
	g_assert (mia_landmarklist_save (lml1, "lmtest.xml"));
	lml2 = mia_landmarklist_new_from_file ("lmtest.xml");
	g_assert (!strcmp (mia_landmarklist_get_name(lml1),
			   mia_landmarklist_get_name(lml2)));

	lm2 = mia_landmarklist_get_landmark(lml2, lmname);
	g_assert (compare_landmarks (lm, lm2));
	g_object_unref (lml2);	
	g_object_unref (lml1);
	

	/* be nice to valgrind */
	xmlCleanupParser ();
	
#ifndef NDEBUG
	print_vectors_left();
	print_quats_left();
	print_camera_left();
#endif

	
	return 0;
}
