#include <QtCore/QCoreApplication>
#include <standardglobe.h>
#include <QDebug>
#include <memory>
#include <motion_access.h>
#include <SurfaceMotionPlugin.h>
#include <internal_impl.h>
#include <VisibleCube.h>

int main(int argc, char* argv[])
{
	QCoreApplication app(argc, argv);

	auto cmd = std::make_shared<StrightLineMotion>();
	cmd->_start_time = 0;
	cmd->_speed_motion = 20;
	cmd->_length_total = 200000;

	auto enti = std::make_shared<RtWsEntity>();
	auto motion = std::make_shared<SurfaceMotionControlPlugin>();
	enti->append(motion);
	auto box3d = std::make_shared<VisibleCubePlugin>();
	enti->append(box3d);

	auto immediate = std::make_shared<ImmediateKernel>(enti);
	QList<std::shared_ptr<RespondDefault>> out;
	motion->execute(immediate, cmd, out);

	for (auto rst : out)
		qDebug() << rst->_success_mark << rst->_reason_text;


	auto total = cmd->timeExpend();
	for (auto timepoint = 0; timepoint < 10000; timepoint += 1) {
		//qDebug() << cmd->posAtTimespan(timepoint);

		auto sync = std::make_shared<SyncRequest>();
		sync->_time_current = timepoint;
		motion->execute(immediate, sync, out);

		qDebug() <<QString("L:%1 B:%2 H:%3").arg(box3d->_self_lla._lon_deg).arg(box3d->_self_lla._lat_deg).arg(box3d->_self_lla._alt_m);
		//qDebug() << box3d->_self_posture._azimuth_deg << box3d->_self_posture._pitch_deg << box3d->_self_posture._roll_deg;
		//qDebug() << timepoint << "========================";
	}

	return app.exec();
}