/*
 * DspRIS.cpp
 *
 * WARNING: ALL section (code, data, rodata, bss, ...) shall be allocated onto a dedicated memory area, reserved at link time
 *   It shall work as a "static DLL": the Rsi Code shall not alter in any way the operative code, even in term of linker time memory allocation.
 *   In other word, the operative CSC shall link also without DspRis and its binary image shall be equal on an executable linker with the RisDsp
 *
 * The target memory shall have 3 partition:
 *   -the operative, spread around without constrains except it cannot use the RIS reserved partitions
 *   -Fixed RIS I/F partition: I/F between operative CSC and RIS CSC,
 *   -Fixed RIS CSC partition: all RIS section
 *
 * The operative partition of an executable linked without RIS shall be equal to an executable linked with the RIS
 *
 */


#include "dsp_ris_internals_.h"

#include "sfp_dsp_ris.h"

#include "gc_math.h"

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

#include "rtlog.h"

#include "mex_hobbit_register.h"

#include "mex.h"

#include <stdint.h>
#include <string.h>


#include <new>

#define LOGID	"RIS"

#define MAX_PACKET_SIZE	(10*1024)   /*provision for jumbo buffer*/

struct ris_dsp_message_header_t
{
	uint32_t begin_marker[2]; //=="RIS0>>>>"
	uint16_t version;
	uint16_t type;
	uint32_t cmd;
	char data[1024];

};

#define RIS_MARKER0	0x30534952	/*"RIS0"*/
#define RIS_MARKER1	0x3E3E3E3E	/*">>>>"*/

enum dsp_ris_features_flags_t
{
	dsp_ris_feat_sim_none,
	dsp_ris_feat_sim_lcu,
	dsp_ris_feat_sim_pps,
	dsp_ris_feat_sim_aesa,

	dsp_ris_feat_sim_simple_map
};

struct dsp_ris_tgt_cmd_t
{
	int id;
	uint32_t data_mask;

	float ground_range;
	float baro_altitude;
	float nav_azimuth;
	float ground_speed;
	float vertical_speed;
	float turn_rate;
	float turn_time;

	float spare[4];
};

struct dsp_ris_cmd_packet_t
{
	uint32_t features_enable;

	dsp_ris_tgt_cmd_t tgt_cmd[16];
};

static_assert((sizeof(SFP_RIS::status_payload_t)<SFP::TX_MTU), "SFP_RIS::status_payload_t to big");
static_assert((sizeof(SFP_RIS::script_payload_t)<SFP::TX_MTU), "SFP_RIS::scirpt_payload_t to big");

class DspRisAgent: public mrk_thread_functor_with_stack<8*1024>
{
public:
	explicit DspRisAgent()
	{
		mrk_mutex_create(&mutex, "RisAgent");
	}

	mrk_mutex_t mutex;

	int active;

	int connected;
	int eth_if;
	mrk_net_ip_port_t server_port;
	mrk_net_udp_socket_t ris_socket;

	mrk_net_ip_raw_address_t client_ip;
	mrk_net_ip_port_t client_port;

	unsigned int rcv_datagram;
	unsigned int rcv_script;

	SFP_RIS::status_message_t stsmsg;
	SFP_RIS::script_message_t cmdmsg;

	unsigned int raw_packet[MAX_PACKET_SIZE/(sizeof(unsigned int))];

	void go()
	{
		create("RIS", mrk_std_priority_below_normal, 0);
	}

	void mrk_exec() override
	{
		mex_hobbit_register_instance("riscom", *this);

		eth_if=0;
		server_port=60001;

		mrk_net_udp_socket(&ris_socket, "Ris");
#ifdef __MINGW32__
		mrk_net_udp_bind(&ris_socket, mrk_net_str2ip("127.0.0.1"), server_port);
#else
		mrk_net_udp_bind_if(&ris_socket, eth_if, server_port);
#endif

		client_ip=mrk_net_myip_address_broadcast(eth_if);
		client_port=server_port+1;

		unsigned int timeout=5000; //mrk_wait_forever;

		active=1;

		memset(&stsmsg, 0, sizeof stsmsg);
		memset(&cmdmsg, 0, sizeof cmdmsg);

		sfp_frag_header_initialize(stsmsg.header, SFP::frag_sync_marker_sender, SFP::src_rdr, SFP::flow_id_ris_sts);

		//sfp_frag_header_payload_init(stsmsg.header, 'R', sizeof  fh, uint8_t record_type, unsigned int payload_leader_size, unsigned int payload_body_size);
		{
			stsmsg.header.frag_tag=SFP::frag_single;
			stsmsg.header.payload_total_size=sizeof stsmsg.payload;
			stsmsg.header.payload_size=sizeof stsmsg.payload;
			stsmsg.header.frag_total=1;
			stsmsg.header.record_type=SFP::flow_id_ris_sts;

		}
		sfp_data_tag_initialize(stsmsg.payload.scenario_tag, sfp_data_tag('R', 'S'), 0, sizeof stsmsg.payload.scenario);
		sfp_data_tag_initialize(stsmsg.payload.target_tag, sfp_data_tag('R', 'T'), 0, sizeof stsmsg.payload.tgt);

		auto script_tag=sfp_data_tag('C', 'S');

		rtlog_printf("RIS", rtlog_report|rtlog_breaking_news, "RIS COM Active I/F %d, port %u", eth_if, server_port);


		for(;;)
		{
			mrk_thread_sleep_ms(10);

			unsigned int received_len=0;


			mrk_net_result_t r=mrk_net_udp_receive(&ris_socket, raw_packet, sizeof raw_packet, &client_ip, &client_port, &received_len, 0);

			if (r==mrk_net_ok)
			{
				++rcv_datagram;

			connected=1;

				bool valid=received_len>(sizeof cmdmsg.header);
				memcpy(&cmdmsg, raw_packet, sizeof cmdmsg);

				valid&=
						(cmdmsg.payload.script_tag.type_validity)
						&& (cmdmsg.payload.script_tag.tag==script_tag);
				if (valid)
			{
					++rcv_script;
					if (cmdmsg.payload.script_tag.size<((sizeof cmdmsg.payload.script)-1))
					{
						cmdmsg.payload.script[cmdmsg.payload.script_tag.size]=0;
						mex_script_asynch_exec(reinterpret_cast<char*>(cmdmsg.payload.script), 0);
					}
				}
			}

			++stsmsg.header.transaction_id;
			++stsmsg.header.record_counter;
			mrk_net_udp_send(&ris_socket, &stsmsg, sizeof stsmsg, client_ip, client_port, 0);
		}
	}
};

#ifdef _TMS320C6X
#define ATTR_NO_INIT __attribute__((section(".far:ris_noinit_")))
#else
#define ATTR_NO_INIT
#endif

struct dsp_ris_static_noinit_data_t
{
	uint64_t align;
	unsigned int active[2];

	mrk_mutex_t tgtMutex;

	char risAgentImage[sizeof(DspRisAgent)];
	DspRisAgent* risAgent;
	volatile DspRisAgent* risAgentVolatile;
};

static dsp_ris_static_noinit_data_t dsp_ris_static_noinit_data ATTR_NO_INIT;

bool DspRigTgtMutexAcquire(bool please_wait)
{
	mrk_tick_t tm=please_wait ? mrk_wait_forever : 0;
	mrk_sts_t r=mrk_mutex_acquire(&dsp_ris_static_noinit_data.tgtMutex, tm);
	return r==mrk_ok;
}

void DspRigTgtMutexRelease()
{
	mrk_mutex_release(&dsp_ris_static_noinit_data.tgtMutex);
}

static void DspRisMexInstall();

#include "mex.h"

void DspRisInstall()
{
	bool activated=(dsp_ris_static_noinit_data.active[0]==0x12345678) && (dsp_ris_static_noinit_data.active[1]==0x87654321);
	if (activated)
		return;

	rtlog_const_message(LOGID, rtlog_breaking_news|rtlog_report, "RIS: Activating...");

	memset(&dsp_ris_tgt_db, 0, sizeof dsp_ris_tgt_db);

#ifdef __MINGW32__
	dsp_ris_tgt_db.agr.enable=1;
#endif

	for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
	{
		float factor=i-float(DSP_RIS_MAX_TGT/2);
		float abs_factor=factor<0 ? -factor: factor;

		dsp_ris_tgt_db.t[i].unique_id=i;

		dsp_ris_tgt_db.t[i].ground_range=18*1852-abs_factor*(1852);
		dsp_ris_tgt_db.t[i].speed=100;
		dsp_ris_tgt_db.t[i].altitude=1828;

		dsp_ris_tgt_db.t[i].az=GC_DEG_TO_RAD(factor*(100.0f/DSP_RIS_MAX_TGT));

		dsp_ris_tgt_db.t[i].rng_start_meters=dsp_ris_tgt_db.t[i].ground_range;
		dsp_ris_tgt_db.t[i].speed_start=dsp_ris_tgt_db.t[i].speed;
		dsp_ris_tgt_db.t[i].altitude_start=dsp_ris_tgt_db.t[i].altitude;
		dsp_ris_tgt_db.t[i].az_start=dsp_ris_tgt_db.t[i].az;

		dsp_ris_tgt_db.t[i].enabled=-2;
	}

	dsp_ris_tgt_db.t[0].ground_range=10000;
	dsp_ris_tgt_db.t[0].speed=100;
	dsp_ris_tgt_db.t[0].altitude=1828;
	dsp_ris_tgt_db.t[0].az=0;
	dsp_ris_tgt_db.t[0].enabled=1;

#if 0
	dsp_ris_tgt_db.t[2].enabled=0;
	dsp_ris_tgt_db.t[2].ground_range=10000;
	dsp_ris_tgt_db.t[2].az=GC_DEG_TO_RAD(-20);
	dsp_ris_tgt_db.t[2].speed=100;
	dsp_ris_tgt_db.t[2].altitude=1000;

	dsp_ris_tgt_db.t[3].enabled=0;
	dsp_ris_tgt_db.t[3].ground_range=10000;
	dsp_ris_tgt_db.t[3].az=GC_DEG_TO_RAD(20);
	dsp_ris_tgt_db.t[3].speed=100;
	dsp_ris_tgt_db.t[3].altitude=1000;

	dsp_ris_tgt_db.t[1]=dsp_ris_tgt_db.t[0];
	dsp_ris_tgt_db.t[1].enabled=0;
	dsp_ris_tgt_db.t[1].ground_range=13000;
#endif

	memset(&dsp_ris_static_noinit_data, 0, sizeof dsp_ris_static_noinit_data);

	mrk_mutex_register(&dsp_ris_static_noinit_data.tgtMutex, "RISTGT");

	dsp_ris_static_noinit_data.risAgent=new(&dsp_ris_static_noinit_data.risAgentImage) DspRisAgent;


	dsp_ris_static_noinit_data.risAgentVolatile=dsp_ris_static_noinit_data.risAgent;
	dsp_ris_static_noinit_data.active[0]=0x12345678;
	dsp_ris_static_noinit_data.active[1]=0x87654321;

	DspRisMexInstall();

	dsp_ris_static_noinit_data.risAgent->go();

	mex_set_prompt("DSP-RIS");
}

#include "idd_nav_types.h"

static void DspRisSfpUpdateScenario(unsigned int timetag, const iddtypes::inu_data_t& inu)
{
	if (!dsp_ris_static_noinit_data.risAgent)
		return;
	DspRisAgent& ra=*dsp_ris_static_noinit_data.risAgent;

	ra.stsmsg.payload.scenario.timetag=timetag;
	ra.stsmsg.payload.scenario.platform_azimuth=inu.own_ship_attitude.vector.platform_azimuth;

	ra.stsmsg.payload.scenario.vx=inu.own_ship_velocity.vector.x;
	ra.stsmsg.payload.scenario.vy=inu.own_ship_velocity.vector.y;
	ra.stsmsg.payload.scenario.vz=inu.own_ship_velocity.vector.z;
	ra.stsmsg.payload.scenario.baro_altitude=inu.inertial_altitude.data;

	ra.stsmsg.payload.scenario.latitude=inu.geo_pos.latitude;
	ra.stsmsg.payload.scenario.longitude=inu.geo_pos.longitude;

	ra.stsmsg.payload.scenario.true_heading=inu.own_ship_attitude.true_heading;

	//Targets
	for(int i=0; i<DSP_RIS_MAX_TGT; ++i)
	{
		ra.stsmsg.payload.tgt.tgt[i].flags=dsp_ris_tgt_db.t[i].enabled;
		ra.stsmsg.payload.tgt.tgt[i].heading=dsp_ris_tgt_db.t[i].heading;
		ra.stsmsg.payload.tgt.tgt[i].x=dsp_ris_tgt_db.t[i].x;
		ra.stsmsg.payload.tgt.tgt[i].y=dsp_ris_tgt_db.t[i].y;
		ra.stsmsg.payload.tgt.tgt[i].z=dsp_ris_tgt_db.t[i].altitude;
	}
}

void DspRisSfpUpdateHeader(const DspLrpHeaderOutput::Header* hdrptr)
{
	if (!dsp_ris_static_noinit_data.risAgent)
		return;
	DspRisAgent& ra=*dsp_ris_static_noinit_data.risAgent;

	ra.stsmsg.payload.scenario.ant_nav_az=hdrptr->ge.general_settings.antenna.position.navaz_rad;
	ra.stsmsg.payload.scenario.ant_nav_el=hdrptr->ge.general_settings.antenna.position.navel_rad;
}

//MEx monitor
#include "mex_rtg_io.h"

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
#endif


static void nl()
{
	mex_putchar('\n');
}

static int eth_n;
static int eth_stat_refresh;
static int eth_msg_stat;
static int eth_msg_period;
static const mex_cmd_param_descriptor_t eth_show_table[] =
{
  { "ethif", "Ethernet I/F", 0, &eth_n, &mex_mt_int},
  { "r", "refresh", mex_cmd_qualifier, &eth_stat_refresh },
  { "m", "message stat", mex_cmd_qualifier, &eth_msg_stat },
  { "p", "show exact period", mex_cmd_qualifier, &eth_msg_period },
#if 0

  { "r", "relative timetags", mex_cmd_qualifier, &drs.param_relative_timetag },
  { "a", "abs time", mex_cmd_qualifier, &drs.param_abs_time },
  { "rev", "reverse (oldest first)", mex_cmd_qualifier, &drs.reverse },

  { "t", "hide timetag (default)", mex_cmd_qualifier, &drs.tt_hide },
  { "b", "show local batch", mex_cmd_qualifier, &drs.lb },
  { "g", "show global batch", mex_cmd_qualifier, &drs.gb },
#endif
  { 0 }
};



static unsigned period_round(unsigned int period, char& rounded)
{
	rounded=' ';

	if (period<1)
		return period;

	if (period>9999)
	{
		rounded='>';
		return 9999;
	}
	if (eth_msg_period)
		return period;

	unsigned int p10=period/10;
	p10=p10*10;
	int p_remainder=period-(p10);
	if (p_remainder>=8)
	{
		rounded='~';
		period=10+p10;
	}
	else if (p_remainder==1)
	{
		rounded='~';
		period=p10;
	}
	return period;
}

static mex_res_t Cmd_ShowEth()
{
	int eth_id=1;

	int eth_mask=1<<eth_id;

	if (mex_param_is_present(&eth_n))
	{
		if (eth_n==0)
			eth_mask=1;
		else if (eth_n==1)
			eth_mask=2;
		else if (eth_n==2)
		{
			eth_mask=3;
		}
	}
	//int flags=0;
	//if (mex_param_is_present(&eth_stat_refresh))
	//	flags=1;

	mex_page_loop_init(1);
	//one_per_line=true; //(mex_page_loop_end(0, 0)!= mex_page_event_quit) ? false : true;

	bool show_msg=mex_param_is_present(&eth_msg_stat);
	eth_msg_period=mex_param_is_present(&eth_msg_period) ? 1 : 0;

	do
	{
		mex_page_loop_top();

		volatile DspRisAgent* ra=dsp_ris_static_noinit_data.risAgentVolatile;
		if (!ra)
		{
			mex_printf("RIS: NOT INSTALLED\n");
			continue;
		}

		mex_printf("RIS: %s\n", ra->connected ? "CONNECTED    " : "NOT CONNECTED");

		//mrk_net_stat_t eth_stat=*mrk_net_stat(ra->eth_if, 0);

		mex_printf("  DSP Server: %u received: %u (%u datagrams)\n", ra->server_port, ra->rcv_script, ra->rcv_datagram);

		unsigned int ccip=ra->client_ip;
		mex_printf("  CC Client : %u.%u.%u.%u::%u\n",
				ccip & 0x0FF,
				(ccip>>8) & 0x0FF,
				(ccip>>16) & 0x0FF,
				(ccip>>24) & 0x0FF,
				ra->client_port);

		if (show_msg)
			continue;

	} while (mex_page_loop_end(0, 0) != mex_page_event_quit);
	nl();

	return mex_ok;
}



static int sleep_time;

static mex_res_t Cmd_Sleep()
{
	mrk_thread_sleep_ms(sleep_time);
	return mex_ok;
}

static const mex_cmd_param_descriptor_t sleep_table[] =
{
  { "time", "sleep time(ms)", mex_cmd_required, &sleep_time, &mex_mt_uint },
  { 0 }
};

static const mex_cmd_descriptor_t cmd_sleep =
{ "sleep", "script sleep", (mex_cmd_handler_t) Cmd_Sleep, sleep_table};

static const mex_cmd_descriptor_t cmd_eth =
{ "rissts", "Show RIS status", (mex_cmd_handler_t) Cmd_ShowEth, eth_show_table};

extern const mex_cmd_descriptor_t cmd_msg;
extern const mex_cmd_descriptor_t cmd_msg_res;

extern const mex_cmd_descriptor_t cmd_scenario;
extern const mex_cmd_descriptor_t cmd_tgtinit;
extern const mex_cmd_descriptor_t cmd_tgtset;
extern const mex_cmd_descriptor_t cmd_tgtturn;

extern const mex_cmd_descriptor_t cmd_pause;
extern const mex_cmd_descriptor_t cmd_continue;
extern const mex_cmd_descriptor_t cmd_tgtreset;
extern const mex_cmd_descriptor_t cmd_aclatch;
extern const mex_cmd_descriptor_t cmd_acunlatch;
extern const mex_cmd_descriptor_t cmd_tgtrng;
extern const mex_cmd_descriptor_t cmd_tgtaz;
extern const mex_cmd_descriptor_t cmd_tgthead;
extern const mex_cmd_descriptor_t cmd_tgtspeed;
extern const mex_cmd_descriptor_t cmd_tgtalt;
extern const mex_cmd_descriptor_t cmd_tgtmap;

extern const mex_cmd_descriptor_t cmd_bite_reset;
extern const mex_cmd_descriptor_t cmd_bite_set;
extern const mex_cmd_descriptor_t cmd_bite_clear;

extern const mex_mt_struct_t mex_mt_ris_tgtovr;

static const mex_mt_object_t ris_str[] =
{
    MEX_HOTCMD_REF("sts", &cmd_eth),
    MEX_HOTCMD_REF("sleep", &cmd_sleep),

    MEX_HOTCMD_REF("tgtinit", &cmd_tgtinit),
    MEX_HOTCMD_REF("tgtset", &cmd_tgtset),
    MEX_HOTCMD_REF("tgtturn", &cmd_tgtturn),

#if 0
    //MEX_HOTCMD_REF("message", &cmd_msg),

	//MEX_HOTCMD_REF("scenario", &cmd_scenario),

	//MEX_HOTCMD_REF("tgtinit", &cmd_tgtinit),
    //MEX_HOTCMD_REF("tgtset", &cmd_tgtset),
    //MEX_HOTCMD_REF("tgtreset", &cmd_tgtreset),

    MEX_HOTCMD_REF("tgtrng", &cmd_tgtrng),
    MEX_HOTCMD_REF("tgtaz", &cmd_tgtaz),
    MEX_HOTCMD_REF("tgtvel", &cmd_tgtspeed),
    MEX_HOTCMD_REF("tgtalt", &cmd_tgtalt),
    MEX_HOTCMD_REF("tgthead", &cmd_tgthead),
    MEX_HOTCMD_REF("tgtmap", &cmd_tgtmap),

    MEX_HOTCMD_REF("aclatch", &cmd_aclatch),
    MEX_HOTCMD_REF("acunlatch", &cmd_acunlatch),
    MEX_HOTCMD_REF("pause", &cmd_pause),
    MEX_HOTCMD_REF("continue", &cmd_continue),

    MEX_HOTCMD_REF("recorder", &cmd_recorder),
    MEX_HOTCMD_REF("wow", &cmd_wow),

    MEX_HOTCMD_REF("biterest", &cmd_bite_reset),
    MEX_HOTCMD_REF("biteset", &cmd_bite_set),
    MEX_HOTCMD_REF("biteclear", &cmd_bite_clear),

    { 0, "ovr", &MEX_MT_CAST(mex_mt_ris_tgtovr), 0 },
    { 0, "dbgdeclutter", &mex_mt_uint, &cmp_dbg_display_hide_ },

#ifdef MCD_UDPFMT
    MEX_CMD_REF("udp1553fmt", &cmd_udp1553fmt),
#endif
    //MEX_CMD_REF("restart", &cmd_restart),

    MEX_CMD_REF("_showreserved", &cmd_msg_res),
#endif
    { 0 }
};

const mex_mt_struct_t mex_mt_ris =
{
{ "ris", 0, &mex_mt_struct_methods }, ris_str };

static mex_mt_object_t ris_obj={
  0, "ris", &MEX_MT_CAST(mex_mt_ris), 0
};

static void DspRisMexInstall()
{
	static mex_cmd_server_t ris_srv;
	mex_object_register(mex_object_root(), &ris_obj);
	mex_add_command_server(&ris_srv, &ris_obj, "Radar Interface Simulator Commands");
	//mex_set_user_mode(-1, "ris.");

	mex_hobbit_register_instance("riscom", dsp_ris_static_noinit_data.risAgent);
}

bool DspRisIsActive()
{
	bool activated=(dsp_ris_static_noinit_data.active[0]==0x12345678) && (dsp_ris_static_noinit_data.active[1]==0x87654321);

	return activated;
}

void DspRisProcessEnvironment(unsigned int timetag, const iddtypes::inu_data_t* inu)
{
	static unsigned int previous_time;

	unsigned int dt=timetag-previous_time;

	previous_time=timetag;

	DspRisSfpUpdateScenario(timetag, *inu);

	float delta_time=dt*64.0E-6;

	if  ((delta_time<0) || (delta_time>5))
		return;

	DspRisTgtUpdate(*inu, delta_time);
}