#include "SurfaceMotionPlugin.h"

SurfaceMotionControlPlugin::SurfaceMotionControlPlugin()
{

}

void SurfaceMotionControlPlugin::recoveryFrom(const QJsonObject& obj)
{
}

void SurfaceMotionControlPlugin::saveTo(QJsonObject& obj) const
{
}

void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const HorizontalArcMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto exec_result = std::make_shared<RespondDefault>();
	// 如果上一个机动命令存在，更新起始位置
	if (_current_cmd) {
		auto exec = std::make_shared<MotionDeduceRequest>();
		exec->reset(in->targetEntity(), in->sourceEntity());
		exec->_target_time = _current_cmd->startTimepoint();
		map->execute(exec);
	}

	auto req = std::make_shared<Get3DBox>();
	req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
	auto rst = map->execute<Get3DBox, Box3DDesc>(req);
	if (rst.size()) {
		// 保存当前命令
		this->_current_cmd = in;
		// 更新基准坐标
		auto item_result = rst.first();
		this->_start_lla = item_result->_lla_pos;
		this->_start_posture = item_result->_posture_d3;

		exec_result->_success_mark = true;
	}
	else {
		exec_result->_reason_text = QString(u8"无法在指定实体（id=%1）获取%2消息，无法正常工作！")
			.arg(this->_bind_entity.lock()->entityID()).arg(Box3DDesc().topicString());
	}
	out << exec_result;
}

void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const StrightLineMotion> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto exec_result = std::make_shared<RespondDefault>();
	// 如果上一个机动命令存在，更新起始位置
	if (_current_cmd) {
		auto exec = std::make_shared<MotionDeduceRequest>();
		exec->reset(in->targetEntity(), in->sourceEntity());
		exec->_target_time = _current_cmd->startTimepoint();
		map->execute(exec);
	}

	// 提取起始机动状态
	auto req = std::make_shared<Get3DBox>();
	req->reset(this->_bind_entity.lock()->entityID(), this->_bind_entity.lock()->entityID());
	auto rst = map->execute<Get3DBox, Box3DDesc>(req);
	if (rst.size()) {
		// 保存当前命令
		this->_current_cmd = in;
		// 更新基准坐标
		auto item_result = rst.first();
		this->_start_lla = item_result->_lla_pos;
		this->_start_posture = item_result->_posture_d3;

		exec_result->_success_mark = true;
	}
	else {
		exec_result->_reason_text = QString(u8"无法在指定实体（id=%1）获取%2消息，无法正常工作！")
			.arg(this->_bind_entity.lock()->entityID()).arg(Box3DDesc().topicString());
	}
	out << exec_result;
}

#include <QMatrix4x4>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const SyncRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto reqs = std::make_shared<MotionDeduceRequest>();
	reqs->reset(in->targetEntity(), in->sourceEntity());
	reqs->_target_time = in->_time_current;

	for (auto rst : map->execute(reqs)) {
		auto conv = std::dynamic_pointer_cast<RespondDefault>(rst);
		if (conv) out << conv;
	}
}

#include <cmath>
void SurfaceMotionControlPlugin::execute(std::shared_ptr<Immediate> map,
	std::shared_ptr<const MotionDeduceRequest> in, QList<std::shared_ptr<RespondDefault>>& out)
{
	auto rst = std::make_shared<RespondDefault>();
	rst->reset(in->targetEntity(), in->sourceEntity());
	if (_current_cmd) {
		// 机体坐标系推演
		auto pos_d3 = _current_cmd->posAtTimespan(in->_target_time - _current_cmd->startTimepoint());
		auto pos_p3 = _current_cmd->speedAtTimespan(in->_target_time - _current_cmd->startTimepoint());

		QMatrix4x4 t;
		t.rotate(-_start_posture._azimuth_deg, QVector3D(0, 0, 1));

		// 本地坐标系结果
		auto conv_pos = pos_d3 * t;
		auto conv_psr = pos_p3 * t;

		// 坐标转换，提取最终目标
		auto azimuth_target_cos = QVector3D::dotProduct(conv_pos.normalized(), QVector3D(0, 1, 0));
		auto azimuth_target = rad2d(std::acos(azimuth_target_cos));
		if (QVector3D::crossProduct(conv_pos.normalized(), QVector3D(0, 1, 0)).z() < 0)
			azimuth_target = 360 - azimuth_target;
		LonLatAlt deduce_lla;
		StandardGlobe::getLLAWithDistance(_start_lla, conv_pos.length(), azimuth_target, deduce_lla);

		// 朝向转换，提取方位角
		auto azimuth_self_cos = QVector3D::dotProduct(conv_psr.normalized(), QVector3D(0, 1, 0));
		auto azimuth_self = rad2d(std::acos(azimuth_self_cos));
		if (QVector3D::crossProduct(conv_psr.normalized(), QVector3D(0, 1, 0)).z() < 0)
			azimuth_self = 360 - azimuth_self;

		auto setpos = std::make_shared<Set3DBoxLLAPos>();
		setpos->_lla_pos = deduce_lla;
		map->execute(setpos);

		auto setpsr = std::make_shared<Set3DBoxPosture>();
		setpsr->_posture_d3._azimuth_deg = azimuth_self;
		map->execute(setpsr);

		rst->_success_mark = true;
	}
	else {
		rst->_reason_text = QString("实体%1未设置机动命令，无法响应“%2”类型消息。")
			.arg(this->_bind_entity.lock()->entityID()).arg(in->topicString());
	}

	out << rst;
}

std::shared_ptr<WsComponent> SurfaceMotionControlPlugin::defaultNew() const
{
	return std::make_shared<SurfaceMotionControlPlugin>();
}

void SurfaceMotionControlPlugin::bindEntity(std::weak_ptr<WsEntity> host)
{
	this->_bind_entity = host;
}

QString SurfaceMotionControlPlugin::name() const
{
	return NAME(SurfaceMotionControlPlugin);
}