/* 
 * Puppeteer - A Motion Capture Mapping Tool
 * Copyright (c) 2013-2015 Martin Felis <martin.felis@iwr.uni-heidelberg.de>.
 * All rights reserved.
 * 
 * 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* 
 */

#ifndef MARKER_MODEL_LUA_TYPES
#define MARKER_MODEL_LUA_TYPES

#include <rbdl/addons/luamodel/luatables.h>

// SimpleMath Vector3f
template<> Vector3f LuaTableNode::getDefault<Vector3f>(const Vector3f &default_value) { 
	Vector3f result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);

		if (vector_table.length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d vector!" << std::endl;
			abort();
		}
		
		result[0] = static_cast<float>(vector_table[1].get<double>());
		result[1] = static_cast<float>(vector_table[2].get<double>());
		result[2] = static_cast<float>(vector_table[3].get<double>());
	}

	stackRestore();

	return result;
}

template<>
void LuaTableNode::set<Vector3f>(const Vector3f &value) {
	LuaTable custom_table = stackCreateLuaTable();

	custom_table[1] = static_cast<double>(value[0]);
	custom_table[2] = static_cast<double>(value[1]);
	custom_table[3] = static_cast<double>(value[2]);

	stackRestore();
}

// SimpleMath Vector4f
template<> Vector4f LuaTableNode::getDefault<Vector4f>(const Vector4f &default_value) { 
	Vector4f result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);

		if (vector_table.length() != 4) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 4d vector!" << std::endl;
			abort();
		}
		
		result[0] = static_cast<float>(vector_table[1].get<double>());
		result[1] = static_cast<float>(vector_table[2].get<double>());
		result[2] = static_cast<float>(vector_table[3].get<double>());
		result[3] = static_cast<float>(vector_table[4].get<double>());
	}

	stackRestore();

	return result;
}

// SimpleMath Matrix33f
template<> SimpleMath::Fixed::Matrix<float, 3, 3> LuaTableNode::getDefault<SimpleMath::Fixed::Matrix<float, 3, 3> >(const SimpleMath::Fixed::Matrix<float, 3, 3> &default_value) { 
	SimpleMath::Fixed::Matrix<float, 3, 3> result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
		
		if (vector_table.length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d matrix!" << std::endl;
			abort();
		}

		if (vector_table[1].length() != 3
				|| vector_table[2].length() != 3
				|| vector_table[3].length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d matrix!" << std::endl;
			abort();
		}

		result(0,0) = static_cast<float>(vector_table[1][1].get<double>());
		result(0,1) = static_cast<float>(vector_table[1][2].get<double>());
		result(0,2) = static_cast<float>(vector_table[1][3].get<double>());

		result(1,0) = static_cast<float>(vector_table[2][1].get<double>());
		result(1,1) = static_cast<float>(vector_table[2][2].get<double>());
		result(1,2) = static_cast<float>(vector_table[2][3].get<double>());

		result(2,0) = static_cast<float>(vector_table[3][1].get<double>());
		result(2,1) = static_cast<float>(vector_table[3][2].get<double>());
		result(2,2) = static_cast<float>(vector_table[3][3].get<double>());
	}

	stackRestore();

	return result;
}

template<>
void LuaTableNode::set<SimpleMath::Fixed::Matrix<float, 3, 3> >(const SimpleMath::Fixed::Matrix<float, 3, 3> &value) {
	LuaTable custom_table = stackCreateLuaTable();

	custom_table[1][1] = static_cast<double>(value(0,0));
	custom_table[1][2] = static_cast<double>(value(0,1));
	custom_table[1][3] = static_cast<double>(value(0,2));

	custom_table[2][1] = static_cast<double>(value(1,0));
	custom_table[2][2] = static_cast<double>(value(1,1));
	custom_table[2][3] = static_cast<double>(value(1,2));

	custom_table[3][1] = static_cast<double>(value(2,0));
	custom_table[3][2] = static_cast<double>(value(2,1));
	custom_table[3][3] = static_cast<double>(value(2,2));

	stackRestore();
}

template<> RigidBodyDynamics::Math::Vector3d LuaTableNode::getDefault<RigidBodyDynamics::Math::Vector3d>(const RigidBodyDynamics::Math::Vector3d &default_value) { 
	RigidBodyDynamics::Math::Vector3d result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);

		if (vector_table.length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d vector!" << std::endl;
			abort();
		}
		
		result[0] = vector_table[1];
		result[1] = vector_table[2];
		result[2] = vector_table[3];
	}

	stackRestore();

	return result;
}

template<> RigidBodyDynamics::Math::SpatialVector LuaTableNode::getDefault<RigidBodyDynamics::Math::SpatialVector>(const RigidBodyDynamics::Math::SpatialVector &default_value) {
	RigidBodyDynamics::Math::SpatialVector result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
		
		if (vector_table.length() != 6) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 6d vector!" << std::endl;
			abort();
		}
		result[0] = vector_table[1];
		result[1] = vector_table[2];
		result[2] = vector_table[3];
		result[3] = vector_table[4];
		result[4] = vector_table[5];
		result[5] = vector_table[6];
	}

	stackRestore();

	return result;
}

template<> RigidBodyDynamics::Math::Matrix3d LuaTableNode::getDefault<RigidBodyDynamics::Math::Matrix3d>(const RigidBodyDynamics::Math::Matrix3d &default_value) {
	RigidBodyDynamics::Math::Matrix3d result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
		
		if (vector_table.length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d matrix!" << std::endl;
			abort();
		}

		if (vector_table[1].length() != 3
				|| vector_table[2].length() != 3
				|| vector_table[3].length() != 3) {
			std::cerr << "LuaModel Error at " << keyStackToString() << " : invalid 3d matrix!" << std::endl;
			abort();
		}

		result(0,0) = vector_table[1][1];
		result(0,1) = vector_table[1][2];
		result(0,2) = vector_table[1][3];

		result(1,0) = vector_table[2][1];
		result(1,1) = vector_table[2][2];
		result(1,2) = vector_table[2][3];

		result(2,0) = vector_table[3][1];
		result(2,1) = vector_table[3][2];
		result(2,2) = vector_table[3][3];
	}

	stackRestore();

	return result;
}

template<> RigidBodyDynamics::Math::SpatialTransform LuaTableNode::getDefault<RigidBodyDynamics::Math::SpatialTransform>(const RigidBodyDynamics::Math::SpatialTransform &default_value) {
	RigidBodyDynamics::Math::SpatialTransform result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);
	
		result.r = vector_table["r"].getDefault<RigidBodyDynamics::Math::Vector3d>(RigidBodyDynamics::Math::Vector3d::Zero(3));
		result.E = vector_table["E"].getDefault<RigidBodyDynamics::Math::Matrix3d>(RigidBodyDynamics::Math::Matrix3d::Identity (3,3));
	}

	stackRestore();

	return result;
}

template<> RigidBodyDynamics::Joint LuaTableNode::getDefault<RigidBodyDynamics::Joint>(const RigidBodyDynamics::Joint &default_value) {
	using namespace RigidBodyDynamics;
	using namespace RigidBodyDynamics::Math;

	Joint result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);

		int joint_dofs = vector_table.length();

		if (joint_dofs == 1) {
			std::string dof_string = vector_table[1].getDefault<std::string>("");
			if (dof_string == "JointTypeSpherical") {
				stackRestore();
				return Joint(JointTypeSpherical);
			} else if (dof_string == "JointTypeEulerZYX") {
				stackRestore();
				return Joint(JointTypeEulerZYX);
			}
			if (dof_string == "JointTypeEulerXYZ") {
				stackRestore();
				return Joint(JointTypeEulerXYZ);
			}
			if (dof_string == "JointTypeEulerYXZ") {
				stackRestore();
				return Joint(JointTypeEulerYXZ);
			}
			if (dof_string == "JointTypeTranslationXYZ") {
				stackRestore();
				return Joint(JointTypeTranslationXYZ);
			}
		}

		if (joint_dofs > 0) {
			if (vector_table[1].length() != 6) {
				std::cerr << "LuaModel Error: invalid joint motion subspace description at " << this->keyStackToString() << std::endl;
				abort();
			}
		}

		switch (joint_dofs) {
			case 0: result = Joint(JointTypeFixed);
							break;
			case 1: result = Joint (vector_table[1].get<SpatialVector>());
							break;
			case 2: result = Joint(
									vector_table[1].get<SpatialVector>(),
									vector_table[2].get<SpatialVector>()
									);
							break;
			case 3: result = Joint(
									vector_table[1].get<SpatialVector>(),
									vector_table[2].get<SpatialVector>(),
									vector_table[3].get<SpatialVector>()
									);
							break;
			case 4: result = Joint(
									vector_table[1].get<SpatialVector>(),
									vector_table[2].get<SpatialVector>(),
									vector_table[3].get<SpatialVector>(),
									vector_table[4].get<SpatialVector>()
									);
							break;
			case 5: result = Joint(
									vector_table[1].get<SpatialVector>(),
									vector_table[2].get<SpatialVector>(),
									vector_table[3].get<SpatialVector>(),
									vector_table[4].get<SpatialVector>(),
									vector_table[5].get<SpatialVector>()
									);
							break;
			case 6: result = Joint(
									vector_table[1].get<SpatialVector>(),
									vector_table[2].get<SpatialVector>(),
									vector_table[3].get<SpatialVector>(),
									vector_table[4].get<SpatialVector>(),
									vector_table[5].get<SpatialVector>(),
									vector_table[6].get<SpatialVector>()
									);
							break;
			default:
							std::cerr << "Invalid number of DOFs for joint." << std::endl;
							abort();
		}
	}

	stackRestore();

	return result;
}

template<> RigidBodyDynamics::Body LuaTableNode::getDefault<RigidBodyDynamics::Body>(const RigidBodyDynamics::Body &default_value) {
	RigidBodyDynamics::Body result = default_value;

	if (stackQueryValue()) {
		LuaTable vector_table = LuaTable::fromLuaState (luaTable->L);

		double mass = 0.;
		RigidBodyDynamics::Math::Vector3d com (RigidBodyDynamics::Math::Vector3d::Zero(3));
		RigidBodyDynamics::Math::Matrix3d inertia (RigidBodyDynamics::Math::Matrix3d::Identity(3,3));

		mass = vector_table["mass"];
		com = vector_table["com"].getDefault<RigidBodyDynamics::Math::Vector3d>(com);
		inertia = vector_table["inertia"].getDefault<RigidBodyDynamics::Math::Matrix3d>(inertia);

		result = RigidBodyDynamics::Body (mass, com, inertia);
	}

	stackRestore();

	return result;
}

/* MARKER_MODEL_LUA_TYPES */
#endif
