#include "motion_access.h"
#include <QVariant>

NavigateWithRoute::NavigateWithRoute()
	:AbstractMessage(NAME(NavigateWithRoute)) {
}

void NavigateWithRoute::recoveryFrom(const QJsonObject& obj)
{
	AbstractMessage::recoveryFrom(obj);

	STRING_PEAK(_route_name);
	uint64_t _drive_mode;
	UINT64_PEAK(_drive_mode);
	this->_drive_mode = (NavigateDriveMode)_drive_mode;
}

void NavigateWithRoute::saveTo(QJsonObject& obj) const
{
	AbstractMessage::saveTo(obj);

	STRING_SAVE(_route_name);
	uint64_t _drive_mode = (uint64_t)this->_drive_mode;
	UINT64_SAVE(_drive_mode);
}

#include <QJsonArray>
#include <QJsonObject>
MotionSequencePreviewGet::MotionSequencePreviewGet()
	:AbstractMessage(NAME(MotionSequencePreviewGet)) {
}


PlatformMotionSequence::PlatformMotionSequence()
	:AbstractMessage(NAME(PlatformMotionSequence))
{

}

void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
{
	throw - 1;
}

void PlatformMotionSequence::saveTo(QJsonObject& obj) const
{
	throw - 1;
}

#include <standardglobe.h>

#define PI 3.14159265354
auto deg2a = [](double v) { return v / 180.0 * PI; };
auto rad2d = [](double v) { return v / PI * 180.0; };
#include <QVector2D>
#include <cmath>
StrightLineMotion::StrightLineMotion()
	: PlatformMotionCommand(NAME(StrightLineMotion)) {
}

double StrightLineMotion::startTimepoint() const
{
	return _start_time;
}

double StrightLineMotion::timeExpend() const
{
	if(_accerlate_rates == 0.0)
		return _length_total / _speed_motion;

	auto sqrt_v = std::sqrt(_speed_motion * _speed_motion + 2 * _accerlate_rates * _length_total);
	return (sqrt_v - _speed_motion) / _accerlate_rates;
}

#include <QVector3D>
QVector3D StrightLineMotion::posAtTimespan(double time) const
{
	auto length = _speed_motion * time +
		0.5 * _accerlate_rates * time * time;

	return QVector3D(0, length, 0);
}

QVector3D StrightLineMotion::speedAtTimespan(double time) const
{
	auto vspeed = _speed_motion + _accerlate_rates * time;
	return QVector3D(0, vspeed, 0);
}

void StrightLineMotion::recoveryFrom(const QJsonObject& obj)
{
	AbstractMessage::recoveryFrom(obj);

	DOUBLE_PEAK(_speed_motion);
	DOUBLE_PEAK(_accerlate_rates);
	DOUBLE_PEAK(_length_total);
}

void StrightLineMotion::saveTo(QJsonObject& obj) const
{
	AbstractMessage::saveTo(obj);

	DOUBLE_SAVE(_speed_motion);
	DOUBLE_SAVE(_accerlate_rates);
	DOUBLE_SAVE(_length_total);
}

HorizontalArcMotion::HorizontalArcMotion()
	: PlatformMotionCommand(NAME(HorizontalArcMotion)) {
}

double HorizontalArcMotion::startTimepoint() const
{
	return _start_time;
}

double HorizontalArcMotion::timeExpend() const
{
	auto radius = std::abs(_center_point.x());
	auto arc_length = radius * deg2a(_rotate_deg);
	return arc_length / _speed_motion;
}

QVector3D HorizontalArcMotion::posAtTimespan(double time) const
{
	auto radius = std::abs(_center_point.x());
	auto arc_length = _speed_motion * time;
	auto _rotate_rads = arc_length / radius;

	QVector3D vec(std::cos(_rotate_rads) * radius / _center_point.x(),
		std::sin(_rotate_rads), 0);
	return vec * radius + QVector3D(_center_point);
}

QVector3D HorizontalArcMotion::speedAtTimespan(double time) const
{
	auto dist_vec = (posAtTimespan(time) - QVector3D(_center_point)).normalized();
	auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(0, 0, 1));
	return speed_nv.normalized() * _speed_motion;
}

void HorizontalArcMotion::recoveryFrom(const QJsonObject& obj)
{
	AbstractMessage::recoveryFrom(obj);

	double posx, posy, posz;
	DOUBLE_PEAK(posx);
	DOUBLE_PEAK(posy);
	DOUBLE_PEAK(posz);
	DOUBLE_PEAK(_rotate_deg);
	DOUBLE_PEAK(_speed_motion);

	_center_point = QVector3D(posx, posy, posz);
}

void HorizontalArcMotion::saveTo(QJsonObject& obj) const
{
	AbstractMessage::saveTo(obj);

	double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
	DOUBLE_SAVE(posx);
	DOUBLE_SAVE(posy);
	DOUBLE_SAVE(posz);
	DOUBLE_SAVE(_rotate_deg);
	DOUBLE_SAVE(_speed_motion);
}

PlatformMotionCommand::PlatformMotionCommand(const QString& nm)
	: AbstractMessage(nm) {
}

VerticalArcMotion::VerticalArcMotion()
	: PlatformMotionCommand(NAME(VerticalArcMotion)) {
}

double VerticalArcMotion::startTimepoint() const
{
	return _start_time;
}

double VerticalArcMotion::timeExpend() const
{
	auto radius = std::abs(_center_point.y());
	auto arc_length = radius * deg2a(_rotate_deg);
	return arc_length / _speed_motion;
}

QVector3D VerticalArcMotion::posAtTimespan(double time) const
{
	auto radius = std::abs(_center_point.z());
	auto arc_length = _speed_motion * time;
	auto rotate_rads = arc_length / radius;

	QVector3D vec_r(0, std::sin(rotate_rads),
		std::cos(rotate_rads) * radius / _center_point.z());
	return _center_point + vec_r * radius;
}

QVector3D VerticalArcMotion::speedAtTimespan(double time) const
{
	auto dist_vec = (posAtTimespan(time) - QVector3D(_center_point)).normalized();
	auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(1, 0, 0));
	return speed_nv.normalized() * _speed_motion;
}

void VerticalArcMotion::recoveryFrom(const QJsonObject& obj)
{
	AbstractMessage::recoveryFrom(obj);

	double posx, posy, posz;
	DOUBLE_PEAK(posx);
	DOUBLE_PEAK(posy);
	DOUBLE_PEAK(posz);
	DOUBLE_PEAK(_rotate_deg);
	DOUBLE_PEAK(_speed_motion);

	_center_point = QVector3D(posx, posy, posz);
}

void VerticalArcMotion::saveTo(QJsonObject& obj) const
{
	AbstractMessage::saveTo(obj);

	double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
	DOUBLE_SAVE(posx);
	DOUBLE_SAVE(posy);
	DOUBLE_SAVE(posz);
	DOUBLE_SAVE(_rotate_deg);
	DOUBLE_SAVE(_speed_motion);
}

MotionDeduceRequest::MotionDeduceRequest()
	: AbstractMessage(NAME(MotionDeduceRequest)) {
}

void MotionDeduceRequest::recoveryFrom(const QJsonObject& obj)
{
	AbstractMessage::recoveryFrom(obj);
	DOUBLE_PEAK(_target_time);
}

void MotionDeduceRequest::saveTo(QJsonObject& obj) const
{
	AbstractMessage::saveTo(obj);
	DOUBLE_SAVE(_target_time);
}