/*************************************************************************/
/*  test_basis.cpp                                                       */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                      https://godotengine.org                          */
/*************************************************************************/
/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
/*                                                                       */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the       */
/* "Software"), to deal in the Software without restriction, including   */
/* without limitation the rights to use, copy, modify, merge, publish,   */
/* distribute, sublicense, and/or sell copies of the Software, and to    */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions:                                             */
/*                                                                       */
/* The above copyright notice and this permission notice shall be        */
/* included in all copies or substantial portions of the Software.       */
/*                                                                       */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
/*************************************************************************/

#include "test_basis.h"

#include "core/math/random_number_generator.h"
#include "core/os/os.h"
#include "core/string/ustring.h"

namespace TestBasis {

enum RotOrder {
	EulerXYZ,
	EulerXZY,
	EulerYZX,
	EulerYXZ,
	EulerZXY,
	EulerZYX
};

Vector3 deg2rad(const Vector3 &p_rotation) {
	return p_rotation / 180.0 * Math_PI;
}

Vector3 rad2deg(const Vector3 &p_rotation) {
	return p_rotation / Math_PI * 180.0;
}

Basis EulerToBasis(RotOrder mode, const Vector3 &p_rotation) {
	Basis ret;
	switch (mode) {
		case EulerXYZ:
			ret.set_euler_xyz(p_rotation);
			break;

		case EulerXZY:
			ret.set_euler_xzy(p_rotation);
			break;

		case EulerYZX:
			ret.set_euler_yzx(p_rotation);
			break;

		case EulerYXZ:
			ret.set_euler_yxz(p_rotation);
			break;

		case EulerZXY:
			ret.set_euler_zxy(p_rotation);
			break;

		case EulerZYX:
			ret.set_euler_zyx(p_rotation);
			break;

		default:
			// If you land here, Please integrate all rotation orders.
			CRASH_NOW_MSG("This is not unreachable.");
	}

	return ret;
}

Vector3 BasisToEuler(RotOrder mode, const Basis &p_rotation) {
	switch (mode) {
		case EulerXYZ:
			return p_rotation.get_euler_xyz();

		case EulerXZY:
			return p_rotation.get_euler_xzy();

		case EulerYZX:
			return p_rotation.get_euler_yzx();

		case EulerYXZ:
			return p_rotation.get_euler_yxz();

		case EulerZXY:
			return p_rotation.get_euler_zxy();

		case EulerZYX:
			return p_rotation.get_euler_zyx();

		default:
			// If you land here, Please integrate all rotation orders.
			CRASH_NOW_MSG("This is not unreachable.");
			return Vector3();
	}
}

String get_rot_order_name(RotOrder ro) {
	switch (ro) {
		case EulerXYZ:
			return "XYZ";
		case EulerXZY:
			return "XZY";
		case EulerYZX:
			return "YZX";
		case EulerYXZ:
			return "YXZ";
		case EulerZXY:
			return "ZXY";
		case EulerZYX:
			return "ZYX";
		default:
			return "[Not supported]";
	}
}

bool test_rotation(Vector3 deg_original_euler, RotOrder rot_order) {
	// This test:
	// 1. Converts the rotation vector from deg to rad.
	// 2. Converts euler to basis.
	// 3. Converts the above basis back into euler.
	// 4. Converts the above euler into basis again.
	// 5. Compares the basis obtained in step 2 with the basis of step 4
	//
	// The conversion "basis to euler", done in the step 3, may be different from
	// the original euler, even if the final rotation are the same.
	// This happens because there are more ways to represents the same rotation,
	// both valid, using eulers.
	// For this reason is necessary to convert that euler back to basis and finally
	// compares it.
	//
	// In this way we can assert that both functions: basis to euler / euler to basis
	// are correct.

	bool pass = true;

	// Euler to rotation
	const Vector3 original_euler = deg2rad(deg_original_euler);
	const Basis to_rotation = EulerToBasis(rot_order, original_euler);

	// Euler from rotation
	const Vector3 euler_from_rotation = BasisToEuler(rot_order, to_rotation);
	const Basis rotation_from_computed_euler = EulerToBasis(rot_order, euler_from_rotation);

	Basis res = to_rotation.inverse() * rotation_from_computed_euler;

	if ((res.get_axis(0) - Vector3(1.0, 0.0, 0.0)).length() > 0.1) {
		OS::get_singleton()->print("Fail due to X %s\n", String(res.get_axis(0)).utf8().get_data());
		pass = false;
	}
	if ((res.get_axis(1) - Vector3(0.0, 1.0, 0.0)).length() > 0.1) {
		OS::get_singleton()->print("Fail due to Y %s\n", String(res.get_axis(1)).utf8().get_data());
		pass = false;
	}
	if ((res.get_axis(2) - Vector3(0.0, 0.0, 1.0)).length() > 0.1) {
		OS::get_singleton()->print("Fail due to Z %s\n", String(res.get_axis(2)).utf8().get_data());
		pass = false;
	}

	if (pass) {
		// Double check `to_rotation` decomposing with XYZ rotation order.
		const Vector3 euler_xyz_from_rotation = to_rotation.get_euler_xyz();
		Basis rotation_from_xyz_computed_euler;
		rotation_from_xyz_computed_euler.set_euler_xyz(euler_xyz_from_rotation);

		res = to_rotation.inverse() * rotation_from_xyz_computed_euler;

		if ((res.get_axis(0) - Vector3(1.0, 0.0, 0.0)).length() > 0.1) {
			OS::get_singleton()->print("Double check with XYZ rot order failed, due to X %s\n", String(res.get_axis(0)).utf8().get_data());
			pass = false;
		}
		if ((res.get_axis(1) - Vector3(0.0, 1.0, 0.0)).length() > 0.1) {
			OS::get_singleton()->print("Double check with XYZ rot order failed, due to Y %s\n", String(res.get_axis(1)).utf8().get_data());
			pass = false;
		}
		if ((res.get_axis(2) - Vector3(0.0, 0.0, 1.0)).length() > 0.1) {
			OS::get_singleton()->print("Double check with XYZ rot order failed, due to Z %s\n", String(res.get_axis(2)).utf8().get_data());
			pass = false;
		}
	}

	if (pass == false) {
		// Print phase only if not pass.
		OS *os = OS::get_singleton();
		os->print("Rotation order: %s\n.", get_rot_order_name(rot_order).utf8().get_data());
		os->print("Original Rotation: %s\n", String(deg_original_euler).utf8().get_data());
		os->print("Quaternion to rotation order: %s\n", String(rad2deg(euler_from_rotation)).utf8().get_data());
	}

	return pass;
}

void test_euler_conversion() {
	Vector<RotOrder> rotorder_to_test;
	rotorder_to_test.push_back(EulerXYZ);
	rotorder_to_test.push_back(EulerXZY);
	rotorder_to_test.push_back(EulerYZX);
	rotorder_to_test.push_back(EulerYXZ);
	rotorder_to_test.push_back(EulerZXY);
	rotorder_to_test.push_back(EulerZYX);

	Vector<Vector3> vectors_to_test;

	// Test the special cases.
	vectors_to_test.push_back(Vector3(0.0, 0.0, 0.0));
	vectors_to_test.push_back(Vector3(0.5, 0.5, 0.5));
	vectors_to_test.push_back(Vector3(-0.5, -0.5, -0.5));
	vectors_to_test.push_back(Vector3(40.0, 40.0, 40.0));
	vectors_to_test.push_back(Vector3(-40.0, -40.0, -40.0));
	vectors_to_test.push_back(Vector3(0.0, 0.0, -90.0));
	vectors_to_test.push_back(Vector3(0.0, -90.0, 0.0));
	vectors_to_test.push_back(Vector3(-90.0, 0.0, 0.0));
	vectors_to_test.push_back(Vector3(0.0, 0.0, 90.0));
	vectors_to_test.push_back(Vector3(0.0, 90.0, 0.0));
	vectors_to_test.push_back(Vector3(90.0, 0.0, 0.0));
	vectors_to_test.push_back(Vector3(0.0, 0.0, -30.0));
	vectors_to_test.push_back(Vector3(0.0, -30.0, 0.0));
	vectors_to_test.push_back(Vector3(-30.0, 0.0, 0.0));
	vectors_to_test.push_back(Vector3(0.0, 0.0, 30.0));
	vectors_to_test.push_back(Vector3(0.0, 30.0, 0.0));
	vectors_to_test.push_back(Vector3(30.0, 0.0, 0.0));
	vectors_to_test.push_back(Vector3(0.5, 50.0, 20.0));
	vectors_to_test.push_back(Vector3(-0.5, -50.0, -20.0));
	vectors_to_test.push_back(Vector3(0.5, 0.0, 90.0));
	vectors_to_test.push_back(Vector3(0.5, 0.0, -90.0));
	vectors_to_test.push_back(Vector3(360.0, 360.0, 360.0));
	vectors_to_test.push_back(Vector3(-360.0, -360.0, -360.0));
	vectors_to_test.push_back(Vector3(-90.0, 60.0, -90.0));
	vectors_to_test.push_back(Vector3(90.0, 60.0, -90.0));
	vectors_to_test.push_back(Vector3(90.0, -60.0, -90.0));
	vectors_to_test.push_back(Vector3(-90.0, -60.0, -90.0));
	vectors_to_test.push_back(Vector3(-90.0, 60.0, 90.0));
	vectors_to_test.push_back(Vector3(90.0, 60.0, 90.0));
	vectors_to_test.push_back(Vector3(90.0, -60.0, 90.0));
	vectors_to_test.push_back(Vector3(-90.0, -60.0, 90.0));
	vectors_to_test.push_back(Vector3(60.0, 90.0, -40.0));
	vectors_to_test.push_back(Vector3(60.0, -90.0, -40.0));
	vectors_to_test.push_back(Vector3(-60.0, -90.0, -40.0));
	vectors_to_test.push_back(Vector3(-60.0, 90.0, 40.0));
	vectors_to_test.push_back(Vector3(60.0, 90.0, 40.0));
	vectors_to_test.push_back(Vector3(60.0, -90.0, 40.0));
	vectors_to_test.push_back(Vector3(-60.0, -90.0, 40.0));
	vectors_to_test.push_back(Vector3(-90.0, 90.0, -90.0));
	vectors_to_test.push_back(Vector3(90.0, 90.0, -90.0));
	vectors_to_test.push_back(Vector3(90.0, -90.0, -90.0));
	vectors_to_test.push_back(Vector3(-90.0, -90.0, -90.0));
	vectors_to_test.push_back(Vector3(-90.0, 90.0, 90.0));
	vectors_to_test.push_back(Vector3(90.0, 90.0, 90.0));
	vectors_to_test.push_back(Vector3(90.0, -90.0, 90.0));
	vectors_to_test.push_back(Vector3(20.0, 150.0, 30.0));
	vectors_to_test.push_back(Vector3(20.0, -150.0, 30.0));
	vectors_to_test.push_back(Vector3(-120.0, -150.0, 30.0));
	vectors_to_test.push_back(Vector3(-120.0, -150.0, -130.0));
	vectors_to_test.push_back(Vector3(120.0, -150.0, -130.0));
	vectors_to_test.push_back(Vector3(120.0, 150.0, -130.0));
	vectors_to_test.push_back(Vector3(120.0, 150.0, 130.0));

	// Add 1000 random vectors with weirds numbers.
	RandomNumberGenerator rng;
	for (int _ = 0; _ < 1000; _ += 1) {
		vectors_to_test.push_back(Vector3(
				rng.randf_range(-1800, 1800),
				rng.randf_range(-1800, 1800),
				rng.randf_range(-1800, 1800)));
	}

	bool success = true;
	for (int h = 0; h < rotorder_to_test.size(); h += 1) {
		int passed = 0;
		int failed = 0;
		for (int i = 0; i < vectors_to_test.size(); i += 1) {
			if (test_rotation(vectors_to_test[i], rotorder_to_test[h])) {
				//OS::get_singleton()->print("Success. \n\n");
				passed += 1;
			} else {
				OS::get_singleton()->print("FAILED                   FAILED                        FAILED. \n\n");
				OS::get_singleton()->print("------------>\n");
				OS::get_singleton()->print("------------>\n");
				failed += 1;
				success = false;
			}
		}

		if (failed == 0) {
			OS::get_singleton()->print("%i passed tests for rotation order: %s.\n", passed, get_rot_order_name(rotorder_to_test[h]).utf8().get_data());
		} else {
			OS::get_singleton()->print("%i FAILED tests for rotation order: %s.\n", failed, get_rot_order_name(rotorder_to_test[h]).utf8().get_data());
		}
	}

	if (success) {
		OS::get_singleton()->print("Euler conversion checks passed.\n");
	} else {
		OS::get_singleton()->print("Euler conversion checks FAILED.\n");
	}
}

MainLoop *test() {
	OS::get_singleton()->print("Start euler conversion checks.\n");
	test_euler_conversion();

	return nullptr;
}

} // namespace TestBasis