#include <boost/asio.hpp>
#include <boost/asio/steady_timer.hpp>
#include <iostream>


namespace asio = boost::asio;

class UdpSendTest
{
	public:
		UdpSendTest(asio::io_service& io_service, const std::string& dest_ip, int dest_port, int packet_size, int packet_count, int interval)
			: io_service_(io_service),
	   		timer_(io_service),
			udp_socket_(io_service),
			dest_ip_(std::move(dest_ip)),
			dest_port_(dest_port),
			packet_size_(packet_size),
			packet_count_(packet_count),
			period_(interval)
		{
		}

		void start()
		{
			timer_.expires_from_now(period_);
			timer_.async_wait(std::bind(&UdpSendTest::handleTimer, this, std::placeholders::_1));
			try
			{
				udp_socket_.connect(asio::ip::udp::endpoint(boost::asio::ip::address::from_string(dest_ip_), dest_port_));
			}
			catch(boost::system::system_error e)
			{
				std::cerr<< "Could not connect:"<<e.what()<<std::endl;
			}
		}
	private:
		static_assert(std::chrono::steady_clock::is_steady, "steady_clock does not use the monotonic system clock. Please use a toolchain with full support for std::chrono!");

		void sendPackets()
		{
			std::vector<unsigned char> buffer(packet_size_,0);
			
			for (int i=0; i<packet_count_; i++)
			{
				if (buffer.size() > 1)
				{
					buffer[0] = i / 255;
					buffer[1] = i % 255;
				}
				else
					buffer[0]=i%255;

				auto t0 = std::chrono::steady_clock::now();
				try
				{
					udp_socket_.send(asio::buffer(buffer));
				}
				catch(boost::system::system_error& error)
				{
					std::cerr<<"Could not send UDP packet, reason: "<<error.what()<<std::endl;
				}
				auto delta_t = std::chrono::steady_clock::now() - t0;
				auto delta_t_us = std::chrono::duration_cast<std::chrono::microseconds>(delta_t).count();
				if (delta_t_us > 10000)
				{
					std::cout<<"Sending UDP packet took >10ms: "<<delta_t_us<<"us"<<std::endl;
				}
				if (delta_t_us > period_.count() * 1000)
				{
					std::cout<<"This would lead to a lost frame!"<<std::endl;
				}
			}
		}

		void handleTimer(boost::system::error_code ec)
		{
			if (ec)
			{
				std::cerr<<"Timer interrupted, exiting!"<<std::endl;
				return;
			}

			sendPackets();
			timer_.expires_at(timer_.expires_at() + period_);
			timer_.async_wait(std::bind(&UdpSendTest::handleTimer, this, std::placeholders::_1));

		}

		asio::io_service& io_service_;
		asio::steady_timer timer_;

		asio::ip::udp::socket udp_socket_;

		std::string dest_ip_;
		int dest_port_;
		int packet_size_;
		int packet_count_;
		std::chrono::milliseconds period_;
};

class UdpSendTestLowlevel
{
	public:
		UdpSendTestLowlevel(asio::io_service& io_service, const std::string& dest_ip, int dest_port, int packet_size, int packet_count, int interval)
			: io_service_(io_service),
	   		timer_(io_service),
			dest_ip_(std::move(dest_ip)),
			dest_port_(dest_port),
			packet_size_(packet_size),
			packet_count_(packet_count),
			period_(interval)
		{
		}

		void start()
		{
			timer_.expires_from_now(period_);
			timer_.async_wait(std::bind(&UdpSendTestLowlevel::handleTimer, this, std::placeholders::_1));
			socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
			if (socket_fd_ < 0)
				std::cerr<<"could not create socket: " <<strerror(errno)<<std::endl;

			auto h = gethostbyname(dest_ip_.c_str());
			if (h == nullptr)
				std::cerr<<"Could not find host: "<<dest_ip_<<std::endl;

			server_addr_.sin_family = h->h_addrtype;
			memcpy((char*)&server_addr_.sin_addr.s_addr, h->h_addr_list[0], h->h_length);
			server_addr_.sin_port = htons(dest_port_);

			client_addr_.sin_family = AF_INET;
			client_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
			client_addr_.sin_port = htons(0);
			auto rc = bind(socket_fd_,reinterpret_cast<sockaddr*>(&client_addr_), sizeof(client_addr_));
			if (rc < 0)
				std::cerr<<"Could not open Port"<<std::endl;
		}
	private:
		static_assert(std::chrono::steady_clock::is_steady, "steady_clock does not use the monotonic system clock. Please use a toolchain with full support for std::chrono!");

		void sendPackets()
		{
			std::vector<unsigned char> buffer(packet_size_,0);
			
			for (int i=0; i<packet_count_; i++)
			{
				if (buffer.size() > 1)
				{
					buffer[0] = i / 255;
					buffer[1] = i % 255;
				}
				else
					buffer[0]=i%255;

				auto t0 = std::chrono::steady_clock::now();
				auto rc = sendto(socket_fd_, buffer.data(), buffer.size(), 0, (sockaddr* )&server_addr_, sizeof(server_addr_));
				if (rc<0)
					std::cerr<<"Could not send UDP packet"<<std::endl;
				auto delta_t = std::chrono::steady_clock::now() - t0;
				auto delta_t_us = std::chrono::duration_cast<std::chrono::microseconds>(delta_t).count();
				if (delta_t_us > 10000)
				{
					std::cout<<"Sending UDP packet took >10ms: "<<delta_t_us<<"us"<<std::endl;
				}
				if (delta_t_us > period_.count() * 1000)
				{
					std::cout<<"This would lead to a lost frame!"<<std::endl;
				}
			}
		}

		void handleTimer(boost::system::error_code ec)
		{
			if (ec)
			{
				std::cerr<<"Timer interrupted, exiting!"<<std::endl;
				return;
			}

			sendPackets();
			timer_.expires_at(timer_.expires_at() + period_);
			timer_.async_wait(std::bind(&UdpSendTestLowlevel::handleTimer, this, std::placeholders::_1));

		}

		asio::io_service& io_service_;
		asio::steady_timer timer_;

		std::string dest_ip_;
		int dest_port_;
		int packet_size_;
		int packet_count_;
		std::chrono::milliseconds period_;

		int socket_fd_;
		struct sockaddr_in client_addr_, server_addr_;
};
int main(int argc, char** argv)
{
	if (argc < 7)
	{
		std::cout<<"usage: "<<argv[0]<<" [boost|raw] dest_ip dest_port packet_size packet_count interval_ms"<<std::endl;
		return 1;
	}
	if (std::atoi(argv[4])<1)
	{
		std::cerr<<"Please select a packet size > 0 bytes!"<<std::endl;
		return 1;
	}
	asio::io_service io_service;
	
	std::string mode(argv[1]);

	int dest_port = std::atoi(argv[3]);
	int packet_size = std::atoi(argv[4]);
	int packet_count = std::atoi(argv[5]);
	int frame_interval = std::atoi(argv[6]);

	int bytes_per_sec = packet_size * packet_count *(1000.f/frame_interval);
	int bytes_per_sec_2 = (packet_size+12) * packet_count *(1000.f/frame_interval);
	std::cout<<"Sending "<<packet_count<<" packets ("<<packet_size<<"b each) at an interval of "<<frame_interval<<"ms, expected data rate:"<<bytes_per_sec <<"b/s ("<<bytes_per_sec_2<<"b/s incl udp overhead)"<<std::endl;
	if (bytes_per_sec_2 > 1000 * 1000 * 100)
		std::cerr<<"Warning: trying to transmit > 100Mb/s"<<std::endl;

	if (mode == "boost")
	{
	
		UdpSendTest u(io_service,
				argv[2],dest_port, packet_size, packet_count, frame_interval);
		u.start();
		io_service.run();
	}
	else
	{
		UdpSendTestLowlevel u(io_service,
				argv[2],dest_port, packet_size, packet_count, frame_interval);
		u.start();
		io_service.run();
	}

	return 0;
}