#include "Motion.h"
#include <iostream>
#include "easylogging++/easylogging++.h"
#include "cenos_exception.h"

Motion::Motion(json input_json)
{
	if (input_json.is_null())
	{
		throw cenos_exception("Empty json object passed to Motion.");
	}

	std::vector<std::string> required_keys = { "name", "isEnabled", "properties", "domains" };
	std::vector<std::string> required_motion_keys = { "Velocity_x", "Velocity_y", "Velocity_z", "Omega_Z" };

	for (auto& key : required_keys)
	{
		if (input_json.find(key) == input_json.end())
		{
			std::string message = "Error while reading motion data: Could not find key " + key + " in Motion json";
			throw cenos_exception(message);
		}
	}

	for (auto& key : required_motion_keys)
	{
		if (input_json["properties"].find(key) == input_json["properties"].end())
		{
			std::string message = "Error while reading motion data: Could not find motion key " + key + " in Motion json";
			throw cenos_exception(message);
		}
	}

	name = input_json.at("name").get<std::string>();

	is_enabled = input_json.at("isEnabled").get<bool>();

	domain_names = input_json.at("domains").get<std::vector<std::string>>();

	linear_velocity[0] = input_json.at("properties").at("Velocity_x").get<double>();
	linear_velocity[1] = input_json.at("properties").at("Velocity_y").get<double>();
	linear_velocity[2] = input_json.at("properties").at("Velocity_z").get<double>();
	angular_velocity = input_json.at("properties").at("Omega_Z").get<double>();
	std::string motion_type = input_json.at("type").get<std::string>();
	if (motion_type == "SIMPLE")
		type = MotionType::SIMPLE;
	else if (motion_type == "COMPLEX")
		type = MotionType::COMPLEX;
	else if (motion_type == "OLD_WORKFLOW")
		type = MotionType::COMPLEX_WITH_PARAMETERS;
	else
		throw(cenos_exception("Unknown motion type (" + motion_type + "). This is internal error, please contact support!"));


	if (type == MotionType::COMPLEX_WITH_PARAMETERS)
	{
		for (auto item : input_json.at("oldWorkflowVariables"))
		{
			Parameter_OM par;
			par.is_enabled = item.at("enabled").get<bool>();
			par.initial_value = item.at("initialValue").get<double>();
			par.velocity = item.at("velocity").get<double>();
			par.name = item.at("name").get<std::string>();
			par.new_value = 0.0;
			par.old_value = par.initial_value;
			params_OM.push_back(par);
		}
	}
	scale = 1.0;
}

Motion::Motion(const Motion& mt)
{
	name = mt.name;
	domain_names = mt.domain_names;
	is_enabled = mt.is_enabled;
	linear_velocity[0] = mt.linear_velocity[0];
	linear_velocity[1] = mt.linear_velocity[1];
	linear_velocity[2] = mt.linear_velocity[2];
	angular_velocity = mt.angular_velocity;
	type = mt.type;
	scale = mt.scale;
	for (auto par : mt.params_OM)
	{
		Parameter_OM p;
		p.is_enabled = par.is_enabled;
		p.name = par.name;
		p.initial_value = par.initial_value;
		p.new_value = par.new_value;
		p.old_value = par.old_value;
		p.velocity = par.velocity;
		params_OM.push_back(p);
	}
}



Motion& Motion::operator=(const Motion& other) 
{
	name = other.name;
	domain_names = other.domain_names;
	is_enabled = other.is_enabled;
	linear_velocity[0] = other.linear_velocity[0];
	linear_velocity[1] = other.linear_velocity[1];
	linear_velocity[2] = other.linear_velocity[2];
	angular_velocity = other.angular_velocity;
	type = other.type;
	scale = other.scale;
	for (auto par : other.params_OM)
	{
		Parameter_OM p;
		p.is_enabled = par.is_enabled;
		p.name = par.name;
		p.initial_value = par.initial_value;
		p.new_value = par.new_value;
		p.old_value = par.old_value;
		p.velocity = par.velocity;
		params_OM.push_back(p);
	}
	return *this;
}

Motion::~Motion()
{
}


gp_Trsf Motion::getTransform(double time)
{
	gp_Trsf transform_translate;
	transform_translate.SetTranslation(time /scale* gp_Vec(linear_velocity[0],
									linear_velocity[1], 
									linear_velocity[2]));

	gp_Trsf transform_rotate;

	gp_Ax1 axis = gp_Ax1(gp_Pnt(0.0, 0.0, 0.0), gp_Dir(0.0, 0.0, 1.0));
	transform_rotate.SetRotation(axis, angular_velocity * time);
	transform_rotate.Multiply(transform_translate);
	return transform_rotate;
}

std::vector<std::string> Motion::getDomainNames()
{
	return domain_names;
}

void Motion::setScale(double sc)
{
	scale = sc;
}

Motion::MotionType Motion::getType()
{
	return type;
}

bool Motion::isEnabled()
{
	return is_enabled;
}

std::vector<double> Motion::getVelocity()
{
	return { linear_velocity[0], linear_velocity[1], linear_velocity[2] };
}

double Motion::getAngularVelocity()
{
	return angular_velocity;
}

void Motion::readOldParameters_OM(std::string fname)
{
	std::ifstream in_file(fname);
	if (!in_file.is_open())
	{
		std::cout << "All motion parameters reset to initial values." << std::endl;
		for (auto par : params_OM)
			par.old_value = par.initial_value;
		return;
	}

	json j = json::parse(in_file);

	for (auto item : j)
	{
		for (auto& par : params_OM)
		{
			if (item.at("name") == par.name)
				par.old_value = item.at("value");
		}
	}
	in_file.close();
}


void Motion::calculateParameters_OM( double time, double time_step)
{
	if (type == COMPLEX_WITH_PARAMETERS)
	{
		for (auto& par : params_OM)
		{
			if (time == 0.0)
				par.new_value = par.initial_value + time_step * par.velocity;
			else
				par.new_value = par.old_value + time_step * par.velocity;

			std::cout << "initial " << par.initial_value << ", oldval " << par.old_value << ", tstep " << time_step << ", velocity " << par.velocity << ", time " << time << ", newval  " << par.new_value << std::endl;

		}
	}
}

json Motion::getParameters_OM()
{
	json j;
	for (auto par : params_OM)
	{
		json p;
		p["name"] = par.name;
		p["value"] = par.new_value;
		j.push_back(p);
	}


	std::cout << "getParameters_OM :: " << j.dump() << std::endl;
	return j;
}

void Motion::saveUpdatedParameters_OM(std::string fname)
{
	json j;
	for (auto par : params_OM)
	{
		json p;
		p["name"] = par.name;
		p["value"] = par.new_value;
		j.push_back(p);
	}

	std::cout << "saveUpdatedParameters_OM :: " << j.dump() << std::endl;

	std::ofstream out_file(fname);
	out_file << j;
	out_file.close();
}