/*
 * image_serializer.cpp
 *
 */


//MTU=1518
//      18-    (ETH)
//      28-    (IP)
//       8-    (UDP)


#include "gc_geometry.h"

#define MFD_DX	gc_cfg::XY_MAX_LOGICAL_DISPLAY_SIZE
#define MFD_DY	gc_cfg::XY_MAX_LOGICAL_DISPLAY_SIZE
#define MFD_OFFSET_X	gc_cfg::X_LOGICAL_DISPLAY_OFFSET
#define MFD_OFFSET_Y	gc_cfg::Y_LOGICAL_DISPLAY_OFFSET

#include "image_serializer.h"

#include "grifo_image_serializer.h"

#include "mrk_plus.h"
#include "mrk_net.h"

#include "rtlog.h"

#define SFP_STATIC_CHECKS
//#include "sfp_helper_tx.h"
#include "sfp/sfp_helper.h"

#include <string.h>

//#pragma GCC optimize("-Og")

#ifdef __MINGW_32__

static int determine_eth_port()
{
	return 0;
}

#else

#include "bsp_platform.h"

static int determine_eth_port()
{
	return bsp_is_evm() ? 0 : 1;
}
#endif

#define MAX_TEST_SIZE	2048

static uint32_t gray_scale[256];

static mrk_mutex_t tidMutex;

class ImageSerializerThread: public mrk_thread_functor_with_stack<4*1024>
{
public:
	struct scenario_t
	{
		unsigned int batch;
		unsigned int tt64;
		uint16_t tod[2];
		float scale_meters;
		//SFP::image_info_t info;
		float crs_orientation;
		float crs_latitude;
		float crs_longitude;
		int16_t crs_x;
		int16_t crs_y;

		float platform_orientation;
		float platform_latitude;
		float platform_longitude;
		float platform_altitude;
	};
	static scenario_t scenario;

	int initialized;
	volatile int enabled;
	volatile int connected;
	volatile int broadcast_mode;

	volatile int fast_send;
	volatile int no_wait;

	volatile unsigned int frag_size;

	volatile unsigned int test;
	volatile unsigned int test_timeout;
	volatile unsigned int test_size;
	volatile unsigned int test_win;

	volatile unsigned int f_out;
	volatile unsigned int f_abort;
	volatile unsigned int f_errs;

	volatile unsigned int decimator;

	unsigned int nominal_fps100x;

	unsigned int dst_fixed_ip;
	unsigned int dst_fixed_port;

	struct img_descriptor_t
	{
		unsigned int bpp;

		unsigned int dx;
		unsigned int dy;

		const uint8_t* data;

		struct
		{
			uint32_t color_map[256];

			int pos_valid;

			float heading;
			float latitude;
			float longitude;
			float meters_per_pixel;
		} cvdata;
	};

#if 0
	struct img_header_t
	{
		uint8_t marker;
		uint8_t cnt;
		uint8_t fcs;
		uint8_t win;

		uint16_t counter;
		uint16_t payload_size;

		uint16_t total_frags;

		uint16_t frag;

		uint16_t dx;
		uint16_t dy;
	};
#else
#endif

	mrk_net_udp_socket_t s;
	mrk_event_t evn;
	mrk_mutex_t mutex;
    unsigned int busy;

	volatile img_descriptor_t commited_image;

	//img_header_t h;
	SFP::image_leader_packet_t h;

	struct {
		SFP::frag_reply_t reply;
		unsigned int dummy[1024];
	} reply;

	unsigned int mfd_serializer;

	mrk_net_ip_raw_address_t destination;
/*
	struct {
		enum { k_dx=484};
		unsigned int palette[256];
		uint8_t image[484][484];
	} mfd_image_t;

	mfd_image_t mfd_image;
*/
	void out_log(const char* msg)
	{
		if (mfd_serializer)
			return;
		unsigned int a0=destination & 0x0FF;
		unsigned int a1=(destination>>8) & 0x0FF;
		unsigned int a2=(destination>>16) & 0x0FF;
		unsigned int a3=(destination>>24) & 0x0FF;

		rtlog_printf("ISER", rtlog_report|rtlog_breaking_news, "%s: %u[%u/%u] @%u.%u.%u.%u", msg, //(unsigned int)h.counter, (unsigned int)h.dx, (unsigned int)h.frag, a0, a1, a2, a3);
				(unsigned int)h.frag_header.record_counter, (unsigned int)h.image_leader.image_header.dx, (unsigned int)h.frag_header.frag_counter, a0, a1, a2, a3);
	}

	img_descriptor_t d;//=commited_image;

	static unsigned int nextTid()
	{
		static volatile unsigned int tid_counter;

		unsigned int tid_current;
		mrk_mutex_acquire(&tidMutex, mrk_wait_forever);
		tid_current=tid_counter++;
		mrk_mutex_release(&tidMutex);
		return tid_current;
	}

	void mrk_exec() override
	{
		static uint8_t test_vector[MAX_TEST_SIZE];
		const int FRAG_DEFAULT=32;
		//const int FRAG_MAX=64;

		for(int i=0; i<MAX_TEST_SIZE; ++i)
		{
			test_vector[i]=i;
		}

		for(int i=0; i<256; ++i)
		{
			gray_scale[i]=i|(i<<8)|(i<<16);
		}

		int eth_port=determine_eth_port();


		memset(&d, 0, sizeof d);

		mrk_mutex_register(&mutex, "IMGSER");
		mrk_event_register(&evn, "IMGSER");
		mrk_net_udp_socket(&s, "IMGSER");

		mfd_serializer=this->arg;

		unsigned int my_port=this->arg ? 55554 : 55555;
		mrk_net_udp_bind_if(&s, eth_port, my_port);
		mrk_net_udp_set_capacity(&s, 4);


		sfp_frag_header_initialize(h.frag_header, SFP::frag_sync_marker_sender, 'R', mfd_serializer ? 'M' : 'S');

		destination=mrk_net_myip_address_broadcast(0);
		mrk_net_ip_raw_address_t broadcast=mrk_net_myip_address_broadcast(0);

		mrk_net_ip_raw_address_t client_address;
		mrk_net_ip_port_t client_port;
		unsigned int received_len;

		frag_size=SFP::TX_IMAGE_MTU;

		unsigned int timeout=mrk_wait_forever;
#if 0
		if (bsp_is_evm())
		{
			timeout=100;
		}
		else
		{
			timeout=100;	//just for test
		}
#endif

		unsigned int decimator_counter=0;

		unsigned int frame_counter=0;

		h.image_leader.image_header.nominal_frame_rate_x100=nominal_fps100x;

		SFP::palette_tags_t pt=SFP::palette_gray_scale;

		if (mfd_serializer)
		{
			rtlog_printf("ISER", rtlog_success|rtlog_breaking_news, "MFD Serializer start");
			pt=SFP::palette_color_map;
			sfp_image_header_prepare(h, true, true);
			h.image_leader.image_header.image_type=0;
			h.image_leader.image_header.image_subtype=0;
			h.image_leader.image_header.image_logical_color=31;
		}
		else
		{
			rtlog_printf("ISER", rtlog_success|rtlog_breaking_news, "SAR Serializer start");
			sfp_image_header_prepare(h, true, false);
			h.image_leader.image_header.nominal_frame_rate_x100=0;
			h.image_leader.image_header.image_type=1;
			h.image_leader.image_header.image_subtype=0;
			h.image_leader.image_header.image_logical_color=0;
			h.image_leader.image_color_map_tag.type_validity=0;
		}

		volatile bool mutex_acquired=false;

		connected=0;

		for(;;)
		{
			if (mutex_acquired)
			{
				mrk_mutex_release(&mutex);
				mutex_acquired=false;
			}

			if (test_timeout>0)
				timeout=test_timeout;

			if(connected<=0)
			{
				mrk_thread_sleep_ms(1000);
			}

			mrk_sts_t wr=mrk_event_wait(&evn, timeout);

			mrk_mutex_acquire(&mutex, mrk_wait_forever);

			mutex_acquired=true;

			unsigned int stride=commited_image.dx;

			if (test)
			{
				if (test_size<1024)
					test_size=1024;

				stride=(test_size<=MAX_TEST_SIZE) ? test_size : MAX_TEST_SIZE;
				d.dx=stride;
				d.dy=stride;
				d.data=test_vector;
			}
			else
			{
				d.dx=commited_image.dx;
				d.dy=commited_image.dy;
				d.data=commited_image.data;
				d.bpp=commited_image.bpp;
			}

			sfp_image_header_init(h, d.dx, d.dy, d.dx, d.bpp, pt);

			if (!enabled)
				continue;

			if (enabled>0)
			{
				--enabled;
			}

			if (!test && wr!=mrk_ok)
				continue;

			if (decimator)
			{
				++decimator_counter;
				if (decimator_counter<decimator)
				{
					continue;
				}
				decimator_counter=0;
			}

			sfp_image_header_next(h, frame_counter, 0);
			++frame_counter;

			if (broadcast==0)
			{
				broadcast=mrk_net_myip_address_broadcast(0);
			}
#ifdef __MINGW32__
			//WARNING: PC net retargeting bug in retriving the broadcast address
			//TODO: Fix IT!!!!
			if (broadcast!=0x0100007F)
				broadcast=broadcast|0xFF000000;
#endif

			destination=broadcast;

			if (!d.data)
				continue;

			unsigned int f_size=frag_size;

			if (test)
			{
				stride=0;
			}
			else
			{
				stride=f_size;
			}

			//const uint8_t* p=d.data;

			//extern mrk_net_result_t mrk_net_udp_send2a32(mrk_net_udp_socket_t* const, const void* const buffer0, unsigned int len0, const void* const buffer1, unsigned int len1, mrk_net_ip_raw_address_t destination, mrk_net_ip_port_t port, unsigned int timeout);

			//unsigned int frag_mask=FRAG_DEFAULT-1;
			unsigned frag_win=FRAG_DEFAULT;
			//unsigned int frag_counter=0;
			//unsigned int win_counter=0;

			//mrk_net_socket_purge(&s);

			unsigned int continous_frag=0;
			unsigned int max_continous_frag=8-1;

			//mrk_net_udp_receive(&s, &reply, sizeof reply, &client_address, &client_port, &received_len, 1); //drop pending packets
			unsigned int dst_port=GrifoImageSerializerDefaultPort(); //55556;

			if (dst_fixed_port)
				dst_port=dst_fixed_port;
			if (dst_fixed_ip)
			{
				broadcast=dst_fixed_ip;
				destination=dst_fixed_ip;
			}

			memcpy(h.image_leader.color_map.map, (const void*)commited_image.cvdata.color_map, sizeof h.image_leader.color_map.map);

			/*
			sfp_image_set_info(h.image_leader.image_info,
					commited_image.cvdata.heading,
					0, 0, 0,
					commited_image.cvdata.latitude,
					commited_image.cvdata.longitude,
					commited_image.cvdata.meters_per_pixel);
            */
			GrifoImageSerializerHeaderData headerCustomizedData;

			h.image_leader.image_header.batch=scenario.batch;
			h.image_leader.image_header.timetag_us64=scenario.tt64;
			h.image_leader.image_header.tod[0]=scenario.tod[0];
			h.image_leader.image_header.tod[1]=scenario.tod[1];

			h.image_leader.image_header.batch=scenario.batch;
			h.image_leader.image_header.batch=scenario.batch;

			if (!mfd_serializer)
			{
				h.image_leader.image_info.latitude=commited_image.cvdata.latitude;
				h.image_leader.image_info.longitude=commited_image.cvdata.longitude;
				h.image_leader.image_info.orientation=commited_image.cvdata.heading;

				h.image_leader.image_info.scale_x=commited_image.cvdata.meters_per_pixel;
				h.image_leader.image_info.scale_y=commited_image.cvdata.meters_per_pixel;

				h.image_leader.image_info.poi_latitude=scenario.platform_latitude;
				h.image_leader.image_info.poi_longitude=scenario.platform_longitude;
				h.image_leader.image_info.poi_altitude=scenario.platform_altitude;
				h.image_leader.image_info.poi_orientation=scenario.platform_orientation;

				h.image_leader.image_info.invaldity_mask=0;
			}
			else
			{
				h.image_leader.image_info.invaldity_mask=SFP::image_info_t::poi_is_cursor;

				h.image_leader.image_info.ref_x=241;
				h.image_leader.image_info.ref_y=483;

				h.image_leader.image_info.latitude=scenario.platform_latitude;
				h.image_leader.image_info.longitude=scenario.platform_longitude;
				h.image_leader.image_info.orientation=scenario.platform_orientation;

				h.image_leader.image_info.poi_altitude=scenario.platform_altitude;
				h.image_leader.image_info.poi_latitude=scenario.crs_latitude;
				h.image_leader.image_info.poi_longitude=scenario.crs_longitude;
				h.image_leader.image_info.poi_orientation=scenario.crs_orientation;

				if (scenario.scale_meters>1)
				{
					h.image_leader.image_info.scale_x=scenario.scale_meters/h.image_leader.image_header.dx;
					h.image_leader.image_info.scale_y=scenario.scale_meters/h.image_leader.image_header.dy;
				}
				else
				{
					h.image_leader.image_info.scale_x=0;
					h.image_leader.image_info.scale_y=0;
					h.image_leader.image_info.invaldity_mask|=SFP::image_info_t::scale_invalid;
				}
			}

			if (GrifoImageSerializerGetHeaderData)
			{
				GrifoImageSerializerGetHeaderData(mfd_serializer, &headerCustomizedData);
				h.image_leader.image_header.image_type=headerCustomizedData.image_type;
				//h.image_leader.image_header.timetag_us64=headerCustomizedData.timetag_us64;
				//h.image_leader.image_header.tod[0]=headerCustomizedData.tod[0];
				//h.image_leader.image_header.tod[1]=headerCustomizedData.tod[1];
			}


			if (GrifoImageSerializerSetInfoData)
				GrifoImageSerializerSetInfoData(mfd_serializer, &h.image_leader.image_info, sizeof h.image_leader.image_info);

			if (GrifoImageSerializerSetReservedData)
				GrifoImageSerializerSetReservedData(mfd_serializer, &h.image_leader.reserved_data, sizeof h.image_leader.reserved_data);

			h.frag_header.transaction_id=nextTid();

			out_log("Image Begin");

			mrk_net_socket_purge(&s);

			mrk_net_udp_send(&s, &h, sizeof h, destination, dst_port, mrk_wait_forever);

			mrk_net_result_t r=mrk_net_udp_receive(&s, &reply, sizeof reply, &client_address, &client_port, &received_len, 2);
			if (r!=mrk_net_ok)
			{
				out_log("Image Abort");
				++f_abort;
				if (connected>0)
					--connected;
				continue;
			}
			connected=10;

			if ((reply.reply.header.transaction_id==h.frag_header.transaction_id))
			{
				if (!broadcast_mode)
					destination=client_address;
			}
			else
			{
				out_log("mismatch transaction id");
				++f_abort;
				continue;
			}

			frag_win=reply.reply.header.win;

			unsigned int frags=h.frag_header.frag_total;

			for(unsigned int i=1; i<frags; ++i)
			{
				sfp_frag_header_next(h.frag_header, frag_win);

				const uint8_t* p=&d.data[sfp_image_payload_offset(h.frag_header)];

				bool ack_required=h.frag_header.flags & SFP::frag_flag_acq_required;
				if (false) //no_wait && ack_required)
				{
					h.frag_header.flags=0;
					ack_required=false;
				}

				if (fast_send)
				{
					unsigned int conmpletition_timeout=(continous_frag & max_continous_frag)==0 ? 2 : 0;
					mrk_net_udp_send2x2_wait(&s, &h.frag_header, sizeof h.frag_header, p, h.frag_header.payload_size, destination, 55556, mrk_wait_forever, conmpletition_timeout);
				}
				else
				{
					mrk_net_udp_send2x2/*mrk_net_udp_send2a32*/(&s, &h.frag_header, sizeof h.frag_header, p, h.frag_header.payload_size, destination, 55556, mrk_wait_forever);
				}

				++continous_frag;

				//if (!no_wait)
				{
					if (ack_required)
					{
						for(int retry=4; retry>0; --retry)
						{
							mrk_net_result_t r=mrk_net_udp_receive(&s, &reply, sizeof reply, &client_address, &client_port, &received_len, 4);
							if (r!=mrk_net_ok)
							{
								out_log("ACK timeout");

								++f_abort;
								goto frame_abort;
							}
							if ((reply.reply.header.frag_counter==h.frag_header.frag_counter))
							{
								if (!broadcast_mode)
									destination=client_address;
								break;
							}
						}
						if ((reply.reply.header.frag_counter!=h.frag_header.frag_counter))
						{
							out_log("Frag lost");
							++f_errs;
							goto frame_abort;
						}

						if (test_win)
						{
							frag_win=test_win;
						}

					}
				}
				//++h.frag;
			}
			++f_out;
			out_log("Image End");

frame_abort:
			(void)0;
		}
	}

	void imageCommit(unsigned int bpp, const uint32_t* color_map, const uint8_t* data, unsigned int stride, unsigned int dy, float heading, float latitude, float longitude, float meters_per_pixel)
	{
		if (mrk_mutex_acquire(&mutex, 0)!=mrk_ok)
		{
			if (connected>1)
			{
				++busy;
				if (busy & 0xFFFFFF00U)
					rtlog_printf("ISER", rtlog_warning|rtlog_breaking_news, "%u: serializer busy (%u)", mfd_serializer, busy);
			}
			return;
		}


		commited_image.bpp=bpp;

		commited_image.dx=stride;
		commited_image.dy=dy;
		commited_image.data=data;
		//commited_image.color_map=color_map;

		commited_image.cvdata.pos_valid=meters_per_pixel ? 1 : 0;

		commited_image.cvdata.heading=heading;
		commited_image.cvdata.latitude=latitude;
		commited_image.cvdata.longitude=longitude;
		commited_image.cvdata.meters_per_pixel=meters_per_pixel;

		if (color_map)
		{
			memcpy(const_cast<uint32_t*>(&commited_image.cvdata.color_map[0]), color_map, sizeof commited_image.cvdata.color_map);
		}
		else
		{
			memcpy(const_cast<uint32_t*>(&commited_image.cvdata.color_map[0]), gray_scale, sizeof commited_image.cvdata.color_map);
		}
		mrk_mutex_release(&mutex);
		mrk_event_signal(&evn);
	}
};

static ImageSerializerThread autoMonitorable_imgser;
static ImageSerializerThread autoMonitorable_mfdser;

ImageSerializerThread::scenario_t ImageSerializerThread::scenario;

void GrifoImageSerializer::rectifiedImageCommit(unsigned int bpp, const uint8_t* data, unsigned int stride, unsigned int dy, float heading, float latitude, float longitude, float meters_per_pixel)
{
	if (!autoMonitorable_imgser.initialized)
	{
		autoMonitorable_imgser.initialized=1;
		autoMonitorable_imgser.create("IMGSER", 0, mrk_std_priority_low+2, 0);
	}
	if (data)
		autoMonitorable_imgser.imageCommit(bpp, 0, data, stride, dy, heading, latitude, longitude, meters_per_pixel);
}

void GrifoImageSerializerInitialize(int enabled)
{
	GrifoImageSerializer::rectifiedImageCommit(0, 0, 0, 0, 0, 0, 0, 0);
	autoMonitorable_imgser.enabled=enabled;

	autoMonitorable_imgser.no_wait=1;
	autoMonitorable_imgser.fast_send=1;
	autoMonitorable_mfdser.no_wait=1;
	autoMonitorable_mfdser.fast_send=1;
	autoMonitorable_mfdser.enabled=enabled;
}


void GrifoMfdSerializer(const uint32_t* palette, const uint8_t* data, unsigned int rows, unsigned int cols, unsigned int stride)
{
	static unsigned char buffer[MFD_DY][MFD_DX];
	static uint32_t color_map[256];

	static unsigned int mfd_limbo;
	if (mfd_limbo<50)
	{
		++mfd_limbo;
		return;
	}

	if (!autoMonitorable_mfdser.enabled)
		return;

	if (!autoMonitorable_mfdser.initialized)
	{
		autoMonitorable_mfdser.initialized=1;
		autoMonitorable_mfdser.create("MFDSER", 1, mrk_std_priority_low+1, 0);
		autoMonitorable_mfdser.nominal_fps100x=25*100;

		return;
	}

	if (data==0)
		return;

	memcpy(color_map, palette, sizeof color_map);

	int DX=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;
	int DY=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;

	int n_rows=(int)rows;
	if (n_rows>DY)
		n_rows=DY;

	data+=gc_cfg::X_LOGICAL_DISPLAY_OFFSET; //(cols-DX)/2;
	unsigned char* p=buffer[0];
	for(int r=0; r<n_rows; ++r)
	{
		memcpy(p, data, DX); //cols); //MFD_DX);
		data+=stride;
		p+=DX;
	}
	autoMonitorable_mfdser.imageCommit(1, color_map, buffer[0], DX, DY, 0, 0, 0, 0);
}

void GrifoImageSerializerSetScenario(
		unsigned batch, unsigned int timetag64,
		uint16_t tod[2],

		float scale_meters,

		float ac_true_heading,
		float ac_latitude,
		float ac_longitude,
		float ac_altitude,

		float tgt_latitude,
		float tgt_longitude
)
{
	ImageSerializerThread::scenario.platform_latitude=ac_latitude;
	ImageSerializerThread::scenario.platform_longitude=ac_longitude;
	ImageSerializerThread::scenario.platform_orientation=ac_true_heading;
	ImageSerializerThread::scenario.platform_altitude=ac_altitude;

	ImageSerializerThread::scenario.crs_latitude=tgt_latitude;
	ImageSerializerThread::scenario.crs_longitude=tgt_longitude;
	ImageSerializerThread::scenario.crs_orientation=ac_true_heading; //TODO: use the correct value - to be calculated!!!

	ImageSerializerThread::scenario.batch=batch;
	ImageSerializerThread::scenario.tt64=timetag64;

	ImageSerializerThread::scenario.tod[0]=tod[0];
	ImageSerializerThread::scenario.tod[1]=tod[1];

	ImageSerializerThread::scenario.scale_meters=scale_meters;
}

extern void GrifoSarSerializerSetup(const char* dst_ip, unsigned int dst_port)
{
	autoMonitorable_imgser.dst_fixed_ip=mrk_net_inet_addr(dst_ip);
	autoMonitorable_imgser.dst_fixed_port=dst_port;
}

void GrifoMfdSerializerSetup(const char* dst_ip, unsigned int dst_port)
{
	autoMonitorable_mfdser.dst_fixed_ip=mrk_net_inet_addr(dst_ip);
	autoMonitorable_mfdser.dst_fixed_port=dst_port;
}

void GrifoImageSerializerSetup(const char* dst_ip, unsigned int dst_port)
{
	GrifoSarSerializerSetup(dst_ip, dst_port);
	GrifoMfdSerializerSetup(dst_ip, dst_port);
}

unsigned int GrifoImageSerializerDefaultPort()
{
	return 55556;
}
