#include "udp_client.h"
#include <boost/bind.hpp>

namespace OpenGTA {
	

	void updateStaticPositionController(const OpenGTA::Map::ObjectPosition& op, PositionController& c)
	{
		
		auto m_M = TranslateMatrix3D(Vector3D(c.pos(0), c.pos(1), c.pos(2)));

		m_M.RotZ(-c.rot);

		c.rot = op.rotation * 360 / 1024;
	}


	PositionController& UdpClient::createStaticPositionController(Vector3D pos)
	{
		PositionController c;
		c.pos = Eigen::Vector3f{ pos.x, pos.y, pos.z };
		c.rot = 0;
		staticPositionControllers[staticPosControllerCount] = c;

		staticPosControllerCount++;

		return staticPositionControllers[staticPosControllerCount - 1];
	}


	PositionController& UdpClient::createStaticPositionController(Vector3D pos, float rot)
	{
		PositionController c;
		c.pos = Eigen::Vector3f{ pos.x, pos.y, pos.z };
		c.rot = rot;
		staticPositionControllers[staticPosControllerCount] = c;

		staticPosControllerCount++;

		return staticPositionControllers[staticPosControllerCount - 1];
	}



	void UdpClient::handleReceive(const boost::system::error_code& error, std::size_t bytes_transferred) {
		if (error) {
			std::cout << "Receive failed: " << error.message() << "\n";
			return;
		}

		net::ServerDatagramToSend serverDatagram;

		std::copy(&recvBuffer[0], &recvBuffer[0] + sizeof(net::ServerDatagramToSend), reinterpret_cast<char*>(&serverDatagram));

		if (serverDatagram.number > latestServerDatagram.number)
		{

			for (uint16_t i = 0; i < serverDatagram.numberOfPlayers; i++)
			{
				if (serverDatagram.players[i].type != serverDatagram.yourType)
				{

				}
			}


			latestServerDatagram = serverDatagram;
		}



		wait();
		
	}

	void UdpClient::wait()
	{
		receiverSocket.async_receive_from(boost::asio::buffer(recvBuffer),
			remoteEndpoint,
			boost::bind(&UdpClient::handleReceive, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
	}

	void UdpClient::udpSendTimerHandler(const boost::system::error_code& error)
	{

		player.pressedButtons = playerController.pressedButtons;
		player.posX = playerController.pos(0);
		player.posY = playerController.pos(1);
		player.posZ = playerController.pos(2);
		player.angle = playerController.rot;

		std::array<char, 256> arrayBuffer;

		arrayBuffer.fill(0);

		std::copy(reinterpret_cast<char*>(&player), reinterpret_cast<char*>(&player) + sizeof(net::ReceivedDatagram), &arrayBuffer[0]);

		//std::copy(&arrayBuffer[0], &arrayBuffer[0] + sizeof(net::ReceivedDatagram), reinterpret_cast<char*>(&player));



		udp::socket socket(udpSenderIoContext);

		udp::endpoint remote_endpoint = udp::endpoint(boost::asio::ip::make_address("127.0.0.1"), 13);

		socket.open(udp::v4());

		boost::system::error_code err;

		auto sent = socket.send_to(boost::asio::buffer(arrayBuffer), remote_endpoint, 0, err);

		socket.close();

		player.number++;




		if (!quit)
		{
			sendTimer.expires_at(sendTimer.expiry() + boost::asio::chrono::milliseconds(50));
			sendTimer.async_wait(boost::bind(&UdpClient::udpSendTimerHandler, this, boost::asio::placeholders::error));
		}
	}

	void UdpClient::start()
	{
		sendTimer.async_wait(boost::bind(&UdpClient::udpSendTimerHandler, this, boost::asio::placeholders::error));

		timerThread = std::thread([this]() {this->udpSenderIoContext.run(); });
	}

	UdpClient::UdpClient()
	{

		player.angle = 0;
		player.localTime = 0;
		player.number = 1;
		player.posX = 12.0;
		player.posY = 6.1;
		player.posZ = 12.0;

		receiverSocket.open(udp::v4());
		receiverSocket.bind(udp::endpoint(boost::asio::ip::make_address("127.0.0.1"), 14));

		wait();

		receiverThread = std::thread([this]() {this->udpReceiverIoContext.run(); });

		
	}


}