/*
 * b1553_udp.cpp
 *
 */


#include "b1553_driver.h"

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

struct b1553_playback_t
{
	unsigned int rx_mask;

	uint16_t pbmsg[32][32];
	unsigned int pbmask[32];
};

static b1553_playback_t b1553_playback;

int b1553_playback_message(uint32_t /*cw*/, unsigned int sa, int rxtx, unsigned int wc, unsigned int wmask, void* buffer)
{
	if (rxtx)
		return 0;

	b1553_playback.rx_mask|=(1<<sa);
	memcpy(b1553_playback.pbmsg[sa], buffer, wc*2);
	b1553_playback.pbmask[sa]=wmask;

	return 0;

#if 0
	static uint16_t tmp_buffer[32];

	if (rxtx)
		return 0;

	b1553_playback.rx_mask|=(1<<sa);
	b1553_drv_msg_get_noupdate(sa, rxtx, tmp_buffer, wc);
	uint16_t* src=reinterpret_cast<uint16_t*>(buffer);
	for(unsigned int i=0; i<wc; ++i)
	{
		if (wmask & (1<<i))
		{
			tmp_buffer[i]=src[i];
		}
		else
		{
			src[i]=tmp_buffer[i];
			//uint16_t* ovr=(uint16_t*)buffer;
			//ovr[i]=tmp_buffer[i];
		}
	}
	return b1553_drv_simulate_msg(cw, sa, rxtx, wc, tmp_buffer);
#endif

}

static int simulate_msg(uint32_t cw, unsigned int sa, int rxtx, unsigned int wc, void* buffer)
{
	if (rxtx)
		return b1553_drv_simulate_msg(cw, sa, rxtx, wc, buffer);
	if (((b1553_playback.rx_mask & (1<<sa)))==0)
		return b1553_drv_simulate_msg(cw, sa, rxtx, wc, buffer);

	static uint16_t tmp_buffer[32];
	uint16_t* src=(uint16_t*)buffer;
	unsigned int wmask=b1553_playback.pbmask[sa];
	uint16_t* pb=b1553_playback.pbmsg[sa];

	for(unsigned int i=0; i<wc; ++i)
	{
		if (wmask & (1<<i))
		{
			tmp_buffer[i]=pb[i];
		}
		else
		{
			tmp_buffer[i]=src[i];
		}
	}
	return b1553_drv_simulate_msg(cw, sa, rxtx, wc, tmp_buffer);
#if 0
	if (rxtx || ((b1553_playback.rx_mask & (1<<sa)))==0)
		return b1553_drv_simulate_msg(cw, sa, rxtx, wc, buffer);
	else
		return 0;
#endif
}

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

//#include "AvionicsBusDriver.h"

#include "rtlog.h"

#include "mrk.h"
#include "mrk_types.h"
#include "mrk_net.h"
#include "mrk_net_rt.h"
#include "grifo_inet.h"
#include "grifo_inet_ports.h"

#define RT_ADDRESS_DEFAULT	20

#include "udp1553_types.h"
#include "udp1553.h"

//#include "fe_takeover.h"
#include "grifo_1553_icd.h"

#include "rtg_high_resolution_timer.h"

#include "DataRecorder.h"

void b1553_udp_lock();
void b1553_udp_unlock();

static udp1553_stat_t stat;
static volatile bool broadcast_reply;

static uint16_t reply_packet[1024];

static unsigned int RT_ADDRESS=RT_ADDRESS_DEFAULT;

static inline void myrta_set(unsigned int ip_adr)
{
	unsigned int a=(ip_adr>>24) && 0x0FF;
	if ((a>=201) && (a<=230))
	{
		RT_ADDRESS=a-200;
	}
}

static inline unsigned int myrta()
{
	return RT_ADDRESS;
}

static unsigned int processMessages(uint16_t* reply, const uint16_t* data,unsigned int size, unsigned long time_ms, unsigned int& ecode)
{
	unsigned int len=size/2;
	unsigned int out_len=0;

	reply[0]=udp_1553_header_t::k_marker_end_1553;

	ecode=0;

	for(unsigned int i=0; i<len;)
	{
		udp_1553_msg_t* m=(udp_1553_msg_t*)&data[i];

		if (m->marker==udp_1553_header_t::k_marker_end_1553)
			break;

		if (m->marker!=udp_1553_header_t::k_ctrl_marker_begin)
		{
			++stat.invalid_message;
			ecode=udp_1553_header_t::k_err_missed_msg_marker;
			break;
		}
		udp_1553_cw_t cw;
		cw.raw=m->cw;
		unsigned int wc=cw.str.wc;
		unsigned int sa=cw.str.sa;

		unsigned int tr=cw.str.tr;

		//bool is_mc=(sa==0) || (sa==31);
		//if (is_mc) wc=1;
		if (wc==0)
			wc=32;

		if ((sa==0) || (sa==31))
			wc=0;

		unsigned int wcin=tr ? 0 : wc;

		i+=((sizeof *m)/2)+wcin;
		if (cw.str.rt!=myrta())
		{
			continue;
		}

		if ((data[i+1]!=udp_1553_header_t::k_ctrl_marker_end) || (data[i]!=(0x0FFFF & (~cw.raw))))
		{
			++stat.invalid_message;
			break;
		}

		udp_1553_msg_t* mout=(udp_1553_msg_t*)&reply[out_len];
		mout->marker=m->marker;
		mout->cw=m->cw;
		mout->errcode=0;

		uint16_t* pout=(uint16_t*)&mout[1];
		out_len+=(sizeof *mout)/2;
		if (tr==0)
		{
			pout=(uint16_t*)&m[1];
		}
		//if (m->ctrl==0)

		++stat.valid_message;

		{
			udp_1553_sw_t sw;
			sw.raw=0;
			sw.str.rt=cw.str.rt;
			int res=simulate_msg(cw.raw, sa, cw.str.tr, wc, pout);
			++stat.msg[cw.str.tr][sa].counter;
			stat.msg[cw.str.tr][sa].period_ms=time_ms-stat.msg[cw.str.tr][sa].rt_time;
			stat.msg[cw.str.tr][sa].rt_time=time_ms;

			if (res==1)
			{
				++stat.msg[cw.str.tr][sa].invalid;
				sw.str.me=1;
				mout->errcode=udp_1553_msg_t::e_msg_err;
			}
			else
			{
				if (tr==1)
					out_len+=wc;
			}
			mout->sw=sw.raw;
		}
		reply[out_len]=~(mout->cw);
		reply[out_len+1]=udp_1553_header_t::k_ctrl_marker_end;
		out_len+=2;
		reply[out_len]=udp_1553_header_t::k_marker_end_1553;

		i+=2;
	}

	return out_len;
}


static volatile flying_engineer_takeover_t fe_takeover;
static volatile flying_engineer_observer_t fe_observer;

extern "C" void mrk_net_rtg_drv_check_speed(int eth_n);

#if __MINGW32__
#define MAIN_IF	0
#else
//#define DUAL_IF
//#define MAIN_IF	1
//#define BACKUP_IF	0
#define MAIN_IF	0
#endif

static void avb1553_worker()
{
	struct rp_1553_opaque_t
	{
		uint8_t datagram[1024];
	};

	static DataRecorder<rp_1553_opaque_t> rp1553In("UDP1553IN", 0);
	static DataRecorder<rp_1553_opaque_t> rp1553Out("UDP1553OUT", 0);

	static mrk_net_udp_socket_t socket_rt;
	static mrk_net_rt_socket_descriptor_t rt;

	rp1553In.setEnable(true);
	rp1553Out.setEnable(true);

	mrk_net_udp_socket(&socket_rt, "UDP1553");
	mrk_net_udp_bind_if(&socket_rt, MAIN_IF, grifo_inet::avb1553_rt/*51553*/);
	mrk_net_udp_socket_rt(&socket_rt, &rt);

	mrk_net_ip_raw_address_t broadcast=mrk_net_myip_address_broadcast(1);

	mrk_net_ip_raw_address_t my_addr=mrk_net_myip_address(1);

	myrta_set(my_addr);

#ifdef DUAL_IF
	static mrk_net_udp_socket_t socket_backup;
	static uint16_t b1553_buffer[1024];

	mrk_net_udp_socket(&socket_backup, "UDP1553-2");
	mrk_net_udp_bind_if(&socket_backup, BACKUP_IF, grifo_inet::avb1553_rt/*51553*/);
	mrk_net_udp_set_capacity(&socket_backup, 4);
	//mrk_net_udp_bind(&socket_rt, 0, grifo_inet::avb1553_rt/*51553*/);
#endif

	unsigned short fcounter=0;

	unsigned int timeout=mrk_ms2ticks(200); //0.2 seconds

#if 0
	for(;;)
	{
		mrk_net_ip_raw_address_t new_addr;
		mrk_net_ip_port_t new_port;
		unsigned int len=0;

	    int r=mrk_net_udp_receive(&socket_rt, b1553_buffer,sizeof b1553_buffer, &new_addr, &new_port, &len, timeout); //mrk_wait_forever);
	    if (r==mrk_net_ok)
	    {
			udp_1553_header_t* h=(udp_1553_header_t*)b1553_buffer;
			if ((len>=(sizeof *h)) && (h->marker1553==1553))
				break;
	    }
	}
#endif

	mrk_net_udp_socket_t* connected_socket=&socket_rt;

	udp_1553_header_t* rhdr=(udp_1553_header_t*)reply_packet;

	rhdr->marker1553=udp_1553_header_t::k_marker_1553;
	rhdr->otype=udp_1553_header_t::k_otype_rt;
	rhdr->ta=myrta();

	unsigned int mcounter=0;
	bool first_message=true;

	unsigned int mmiss=0;

	unsigned int connected=0;

	unsigned int success_severity=rtlog_report;

	mrk_net_ip_raw_address_t obs_addr=0;
	mrk_net_ip_port_t obs_port=0;

	const mrk_net_opaque_packet_t* opaque_pkt=0;

	for(;;)
	{
		mrk_net_ip_raw_address_t new_addr;
		mrk_net_ip_port_t new_port;
		unsigned int len=0;
		//mrk_net_result_t r=mrk_net_udp_receive(connected_socket, b1553_buffer,sizeof b1553_buffer, &new_addr, &new_port, &len, timeout);
		//const mrk_net_opaque_packet_t* opaque_pkt=0;
		if (opaque_pkt)
		{
			mrk_net_release_packet(opaque_pkt);
			opaque_pkt=0;
		}

		mrk_net_result_t r=mrk_net_udp_raw_receive(connected_socket, &opaque_pkt,&new_addr, &new_port, timeout);
		if (r!=mrk_net_ok)
		{
			//opaque_pkt=0;

			if (connected>0)
			{
				--connected;
				if (connected==0)
					rtlog_printf("UDP1553", rtlog_warning|rtlog_breaking_news, "Disconnected?");
			}

#ifdef DUAL_IF
			mrk_net_rtg_drv_check_speed((int)connected_socket->raw_socket.eth_if);
			if (connected_socket!=&socket_backup)
			{
				unsigned int backup_timeout=5;

				r=mrk_net_udp_receive(&socket_backup, b1553_buffer,sizeof b1553_buffer, &new_addr, &new_port, &len, backup_timeout);
				if (r==mrk_net_ok)
				{
					bool bck_connected=false;

					flying_engineer_takeover_t* fe=(flying_engineer_takeover_t*)b1553_buffer;
					if (len>=(sizeof *fe))
					{
						bck_connected=fe->takeover_marker==flying_engineer_takeover_t::to_marker;
					}

					udp_1553_header_t* h=(udp_1553_header_t*)b1553_buffer;

					//if (len<(sizeof *h))
					//	continue;
					if (len>=(sizeof *h))
					{
						if (h->marker1553==udp_1553_header_t::k_marker_1553)
						{
							bck_connected=true;
						}
					}

					if (bck_connected)
					{
						connected_socket=&socket_backup;
						rtlog_printf("UDP1553", rtlog_warning|rtlog_breaking_news, "switching to backup socket");
						broadcast=mrk_net_myip_address_broadcast(0);
					}
					continue;
				}
				else
					mrk_net_rtg_drv_check_speed(socket_backup.raw_socket.eth_if);

			}
#endif
			continue;
		}
		if (r==mrk_net_ok)
		{
			void* pkt=mrk_net_get_packet_data(opaque_pkt, &len);
			if (!pkt)
			{
				opaque_pkt=0;
				continue;
			}

			++stat.received_datagrams;

			flying_engineer_takeover_t* fe=(flying_engineer_takeover_t*)pkt;
			if (fe->takeover_marker==flying_engineer_takeover_t::to_marker)
			{
				obs_addr=new_addr;
				obs_port=new_port;
				static int32_t taking_over;
				static uint32_t taking_counter;
				if (sizeof(*fe)>len)
					continue;
				if (taking_over!=fe->to_enable)
				{
					rtlog_printf("UDP1553", rtlog_warning|rtlog_breaking_news, "FE Taking over: %d", (int)fe->to_enable);
				}
				taking_over=fe->to_enable;
				if (taking_counter==fe->takeover_update)
					continue;
				taking_counter=fe->takeover_update;

				b1553_udp_lock();
				memcpy((void*)&fe_takeover, fe, sizeof fe_takeover); //fe_takeover= *fe;
				b1553_udp_unlock();

				//mrk_net_release_packet(opaque_pkt);

				if (fe_observer.pending && obs_addr && obs_port)
				{
					mrk_net_udp_send(connected_socket, (void*)&fe_observer, sizeof fe_observer, obs_addr, obs_port, 0);
					fe_observer.pending=0;
				}
				continue;
			}

			udp_1553_header_t* h=(udp_1553_header_t*)pkt;

			rp1553In.save(*reinterpret_cast<rp_1553_opaque_t*>(pkt));


			if (len<(sizeof *h))
			{
				++stat.invalid_len;
				continue;
			}

			if (h->marker1553!=udp_1553_header_t::k_marker_1553)
			{
				rtlog_printf("UDP1553", rtlog_error|rtlog_breaking_news, "Invalid packet marker (0x%4.4X)", h->marker1553);
				++stat.wrong_marker;
				continue;
			}

			if (connected==0)
			{
				rtlog_printf("UDP1553", success_severity|rtlog_breaking_news, "connected (%s)", connected_socket->raw_socket.name);
				broadcast=mrk_net_myip_address_broadcast(1);
			}

			connected=10;

#if 0
			if (timeout!=mrk_wait_forever)
			{
				timeout=mrk_wait_forever;
				rtlog_printf("UDP1553", rtlog_success|rtlog_breaking_news, "connected (%s)", connected_socket->raw_socket.name);
			}
#endif
			if (h->ta!=0)
			{
				++stat.wrong_ta;
				continue;
			}

			if (first_message)
			{
				first_message=false;
			}
			else
			{
				if (mcounter==h->mcounter)
				{
					rtlog_printf("UDP1553", rtlog_warning|rtlog_breaking_news, "BC retransmit? (%u)", mcounter);
				}
				else if ((mcounter+1)!=h->mcounter)
				{
					++stat.bc_missed;
					++mmiss;
				}
			}
			mcounter=h->mcounter;

			++rhdr->scounter;
			rhdr->mcounter=h->mcounter;
			rhdr->fcounter=h->fcounter;
			rhdr->smiss=mmiss;
			rhdr->mmiss=h->mmiss;

			rtlog_printf("UDP1553", rtlog_debug, "received packet %u (%u)", (unsigned int)rhdr->mcounter, (unsigned int)rhdr->scounter);

			uint16_t* p=(uint16_t*)&h[1];

			//h->discretes_out=0; //avb_cx.discretes_in=h->discretes_in;

			++stat.valid_packets;

			unsigned long long hrt_us=rtg_hrt_to_microseconds_longlong(rtg_hrt_get());
			unsigned long hrt_ms=hrt_us/1000;
			if ((hrt_us-(hrt_ms*1000))>=500)
				++hrt_ms;
			rhdr->ltt=hrt_us;

			unsigned int ecode=0;
			unsigned int rlen=processMessages((uint16_t*)&rhdr[1], p, len-(sizeof *h), hrt_ms, ecode);
			if (rlen || ecode)
			{
				rlen=rlen*2+sizeof(*rhdr)+2;
				rhdr->errors=ecode;
				if (ecode)
					++stat.invalid_message;
#ifdef __MINGW32__
				//new_addr=new_addr|0xFF000000;	//to see the packets with wireshark
#endif
				rp1553Out.save(*reinterpret_cast<rp_1553_opaque_t*>(&reply_packet));

				if (broadcast_reply)
					r=mrk_net_udp_send(connected_socket, reply_packet, rlen, broadcast, new_port, 0);
				else
					r=mrk_net_udp_send(connected_socket, reply_packet, rlen, new_addr, new_port, 0);
			}
			//r=mrk_net_udp_raw_send(connected_socket, (mrk_net_opaque_packet_t*)opaque_pkt, len, new_addr, new_port, 0);
			if (r)
				rtlog_printf("AVB1553", rtlog_warning, "Reply failed: %u, len=%u", r, rlen);
			else
			{
				if (fcounter!=h->fcounter)
				{
					fcounter=h->fcounter;
					rtlog_printf("AVB1553", rtlog_trace, "New frame (%u)",fcounter);
				}
			}

			h->marker1553=0;

			//mrk_net_release_packet(opaque_pkt);
		}

	}
}

static mrk_mutex_t t_mutex;

static void avb1553_upd_initialize()
{
	static uint64_t t_stack[1024];
	static mrk_thread_t t_thread;

	mrk_mutex_create(&t_mutex, "AVB1554MUTEX");
	mrk_thread_create(&t_thread, "AVB1553UDP", avb1553_worker, 0, mrk_std_priority_time_critical/*mrk_std_priority_above_normal*/, sizeof t_stack, t_stack, 0);
}


//External I/F

void b1553_upd_start()
{
	avb1553_upd_initialize();
}

void b1553_udp_lock()
{
	mrk_mutex_acquire(&t_mutex, mrk_wait_forever);
}

void b1553_udp_unlock()
{
	mrk_mutex_release(&t_mutex);
}


const udp1553_stat_t* udp1553_stat()
{
	return &stat;
}

void udp1553_broadcast_reply(bool enable)
{
	broadcast_reply=enable;
}

/*
const flying_engineer_takeover_t* b1553_flying_engineer_takeover()
{
	if (fe_takeover.takeover_flags & 1)
		return &fe_takeover;
	return 0;
}
*/

static bool fe_active;

template<class T_, int min_, int max_> struct fe_takeover_datum
{
	typedef T_ field_t;
	typedef typename T_::type_t type_t;

	//typename field_t::type_t type_t;

	int active_source;
	int fe_counter;

	type_t mcs_cmd;
	type_t fe_cmd;
	type_t last_cmd;

	//fe_takeover_datum(): active_source(0)  -- WARNING: always use as "static", to zero initialize the data
	//{
	//}

	bool ovr_raw(field_t& mcs, const int fe, int fe_count=0)
	{
		if ((fe>=min_) && (fe<=max_))
			return ovr(mcs, static_cast<type_t>(fe), fe_count);
		return false;
	}

	bool ovr(field_t& mcs, const type_t fe, int fe_count)
	{

		if (!fe_active)
		{
			active_source=0;
		}
		else
		{
			type_t tmp_cmd=mcs.get();

			if (mcs_cmd!=tmp_cmd)
			{
				active_source=1;
			}
			else if ((fe_cmd!=fe) || (fe_counter!=fe_count))
			{
				active_source=-1;
			}
			mcs_cmd=tmp_cmd;
		}

		fe_cmd=fe;
		//mcs_cmd=tmp_cmd;
		fe_counter=fe_count;

		if (active_source<0)
		{
			mcs.set(fe);
			last_cmd=fe_cmd;
			return true;
		}
		else
		{
			last_cmd=mcs_cmd;
			return false;
		}
	}
};

class fe_antenna_el_raw_t
{
public:
	typedef int16_t type_t;

	fe_antenna_el_raw_t(type_t v): raw(v)
	{
	}

	void set(type_t v) { raw=v;}
	type_t get() const { return raw;}

	type_t raw;
};

struct fe_to_data_set_t
{
	fe_takeover_datum<icd1553::if_gain_field_t,0, 127> if_gain;

	fe_takeover_datum<icd1553::rdr_mode_field_t, 0, 13> mode;
	fe_takeover_datum<icd1553::gm_submode_field_t, 0, 1> gc_submode;
	fe_takeover_datum<icd1553::acm_submode_field_t, 0, 5> acm_submode;
	fe_takeover_datum<icd1553::range_scale_field_t, 0, 3> range_scale;

	fe_takeover_datum<fe_antenna_el_raw_t, -32768, 32767> ant_el;
	fe_takeover_datum<fe_antenna_el_raw_t, -32768, 32767> ant_az;

	float crs_altitude;

#if 0
	fe_takeover_datum<icd1553::rdr_mode_field_t, 0, 13> mode;
	fe_takeover_datum<icd1553::gm_submode_t, 0, 1> gc_submode;
	fe_takeover_datum<icd1553::acm_submode_t, 0, 5> acm_submode;
	fe_takeover_datum<icd1553::range_scale_t, 0, 3> range_scale;
	fe_takeover_datum<icd1553::scan_width_t, 0, 3> scan_width;
	fe_takeover_datum<icd1553::bars_num_t, 0, 3> bars_num;
#endif
};

static fe_to_data_set_t fe_to_data_set;

float b1553_flying_engineer_takeover_crs_altitude()
{
	return (fe_active && (fe_to_data_set.crs_altitude>0)) ? fe_to_data_set.crs_altitude : 0;
}
void b1553_flying_engineer_takeover_rx(int msgid, void* buffer)
{
	static unsigned int update_counter;
	static bool des_valid=true;
	static unsigned int evn_pending;
	static unsigned int evn_last;
	static flying_engineer_takeover_t fe;
	static bool update_crs;
	static int ovr_ant;
	static int ovr_ant_sts;
	static int16_t p_mc_ant_el;
	static int32_t crs_zero=0;
	static bool crs_zero_event;
	static int obs;

	b1553_udp_lock();
	memcpy(&fe, (void*)&fe_takeover, sizeof fe); //fe=fe_takeover;
	b1553_udp_unlock();

	//static icd1553::des_control_t des;

	bool updated=update_counter!=fe.takeover_update;
	update_counter=fe.takeover_update;

	if (fe.fe_enable!=1)
	{
		evn_pending=0;
		update_crs=false;
		//ovr_ant=0;
		obs=0;
		fe_active=false;
		crs_zero=fe.crs_zero;
		evn_last=fe.evn;
		return;
	}

	fe_active=fe.to_enable;

	fe_to_data_set.crs_altitude=fe.crs_altitude;

	if (updated)
	{
		if (fe.crs_ovr)
			update_crs=true;
		if (fe.evn!=evn_last)
		{
			evn_pending=fe.evn & 0x00FFFF;
			rtlog_printf("FE", rtlog_info|rtlog_breaking_news, "event: %X", evn_pending);
		}

		//ovr_ant=fe.ant_ovr;

		obs=fe.obs_enable;

		crs_zero_event=crs_zero!=fe.crs_zero;
	}

	evn_last=fe.evn;

	crs_zero=fe.crs_zero;

	//unsigned int update_bit=(1<<msgid);

	switch(msgid)
	{
	case icd1553::msg_inu_high_speed_trait::sa:
	{
		icd1553::msg_inu_high_speed_t* msg=reinterpret_cast<icd1553::msg_inu_high_speed_t*>(buffer);
		fe_observer.ptaz=msg->platform_azimuth.get();
		float vx=msg->x_vel.get();
		float vy=msg->y_vel.get();
		if ((vx+vy)<5)
			fe_observer.ground_track_angle=-fe_observer.ptaz;
		else
		{
			float gvv=atan2f(vy, vx);
			fe_observer.ground_track_angle=gvv;
		}
		break;
	}

	case icd1553::msg_graphic_setting_trait::sa:
	{
		if (fe_active)
		{
			if ((fe.spoi_control>0) && (fe.spoi_control<4))
			{
				icd1553::msg_graphic_setting_t* msg=reinterpret_cast<icd1553::msg_graphic_setting_t*>(buffer);
				int n=fe.spoi_control-1;
				msg->waypoints.wp[n].waypoint_lon_code.waypoint_lon.set(fe.spoi_lon);
				msg->waypoints.wp[n].waypoint_lon_code.waypoint_code.set(0);
				msg->waypoints.wp[n].waypoint_lat_sel_qual.waypoint_lat.set(fe.spoi_lat);
				//msg->waypoints.wp[n].waypoint_lat_sel_qual.waypoint_qualifier_validity.setwaypoint_lat.set(fe.spoi_lat);
				msg->graphic_order.bulls_eye_identifier.set(static_cast<icd1553::bulls_eye_identifier_t>(fe.spoi_control));
				switch(n)
				{
				default:
					break;
				case 0:
					msg->graphic_order.waypoint_01_flag.set(true);
					break;
				case 1:
					msg->graphic_order.waypoint_02_flag.set(true);
					break;
				case 2:
					msg->graphic_order.waypoint_03_flag.set(true);
					break;
				}
			}
		}
		break;
	}

	case icd1553::msg_nav_data_and_cursor_trait::sa:
	{
		icd1553::msg_nav_data_and_cursor_t* msg=reinterpret_cast<icd1553::msg_nav_data_and_cursor_t*>(buffer);
		if (update_crs)
		{
			msg->acq_crs_x.abs_pos.set(fe.crs_x);
			msg->acq_crs_y.abs_pos.set(fe.crs_y);
			msg->validity_and_slew.crs_rate_invalid.set(false);
			msg->acq_crs_x.abs.set(true);
			update_crs=false;
		}

		if (p_mc_ant_el!=msg->el_antenna_demand.get_raw())
		{
			p_mc_ant_el=msg->el_antenna_demand.get_raw();
			ovr_ant_sts=0;
		}
		else if (fe_active)
		{
			if ((ovr_ant!=fe.ant_ovr) || ovr_ant_sts)
			{
				msg->el_antenna_demand.set(fe.antenna_el);
				if (fe.ant_evn==2)
					msg->validity_and_slew.ant_slew_valid.set(true);
				ovr_ant_sts=1;
				ovr_ant=fe.ant_ovr;
			}

			if (fe.spoi_control>0)
			{
				msg->spoi_baro_altitude.set(fe.spoi_alt);
				msg->spoi_lat.set(fe.spoi_lat);
				msg->spoi_lon.set(fe.spoi_lon);
			}
			if (crs_zero_event)
			{
				msg->validity_and_slew.crs_zero_valid_field.set(true);
			}
		}

#if 0
		if (ovr_ant)
		{
			msg->el_antenna_demand.set(gc_math::deg_to_rad(fe.antenna_el));
			if (ovr_ant==2)
				msg->validity_and_slew.ant_slew_valid.set(true);
		}
#endif
		fe_observer.magnetic_heading=msg->magnetic_heading.get();
		fe_observer.true_heading=msg->true_heading.get();
		fe_observer.latitude=msg->ppos_lat.get();
		fe_observer.longitude=msg->ppos_lon.get();
		fe_observer.baro_altitude=msg->baro_altitude.get();

		crs_zero_event=false;
		break;
	}
	case icd1553::msg_rdr_settings_and_parameters_trait::sa:
	{
		icd1553::msg_rdr_settings_and_parameters_t* msg=reinterpret_cast<icd1553::msg_rdr_settings_and_parameters_t*>(buffer);

		fe_to_data_set.if_gain.ovr(msg->gains_and_chgrp.if_gain, fe.if_gain, fe.if_gain_ovr);
#if 0
		if (ovr_mode)
		{
			if ((fe.if_gain>0) && (fe.if_gain<=127))
				msg->gains_and_chgrp.if_gain.set(fe.if_gain);
		}
#endif
		fe_observer.if_gain=msg->gains_and_chgrp.if_gain.get();

		fe_observer.date=msg->date_of_mission.raw;
		fe_observer.time=msg->time_of_mission.get_raw();
		break;
	}

	case icd1553::msg_rdr_operation_command_trait::sa:
	{
		icd1553::msg_rdr_operation_command_t* msg=reinterpret_cast<icd1553::msg_rdr_operation_command_t*>(buffer);
		if (evn_pending)
		{
			des_valid=false;
			switch(evn_pending)
			{
			case 1:
				msg->rdr_mode_command.des_ctrl.set(icd1553::LOCK_ON);
				break;
			case 2:
				msg->rdr_mode_command.des_ctrl.set(icd1553::REJECT);
				break;
			case 3:
				msg->rdr_mode_command.des_ctrl.set(icd1553::LOCK_ON_STT);
				break;
			case 0x101:
				msg->rdr_mode_command.des_ctrl.set(icd1553::LOCK_ON_DTT);
				break;
			case 0x102:
				msg->rdr_mode_command.des_ctrl.set(icd1553::TRANS_HPT_BUT);
				break;
			default:
				msg->rdr_mode_command.des_ctrl.set(icd1553::NOT_VALID);
			}
			evn_pending=0;
		}
		else
		{
			if (!des_valid)
				des_valid=msg->rdr_mode_command.des_ctrl.get()==icd1553::NOT_VALID;
			if (!des_valid)
				msg->rdr_mode_command.des_ctrl.set(icd1553::NOT_VALID);
		}

		if (fe.to_enable)
		{
			bool ovr_mode=fe_to_data_set.mode.ovr_raw(msg->rdr_mode_command.master_mode, fe.mode, fe.mode_ovr);
			if (ovr_mode)
			{
				if (fe.mode>=0)
				{
					//msg->rdr_mode_command.master_mode.set(static_cast<icd1553::rdr_modes_t>(fe.mode));
					if (fe.submode>=0)
					{
						switch(fe.mode)
						{
						case icd1553::GM:
							msg->param1.gm_submode.set(static_cast<icd1553::gm_submode_t>(fe.submode));
							break;
						case icd1553::ACM:
							msg->param1.acm_submode.set(static_cast<icd1553::acm_submode_t>(fe.submode));
							break;
						default:
							break;
						}
					}
				}

				//fe_to_data_set.range_scale.ovr(msg->param1.range_scale, fe.scale, fe.scale_ovr);
	#if 0
				if ((fe.scale>=0) && (fe.scale<=3))
				{
					msg->param1.range_scale.set(static_cast<icd1553::range_scale_t>(fe.scale));
				}
				if ((fe.width>=0) && (fe.width<=3))
				{
					msg->param1.scan_width.set(static_cast<icd1553::scan_width_t>(fe.width));
				}
				if ((fe.bars>=0) && (fe.bars<=3))
					msg->param1.bars_num.set(static_cast<icd1553::bars_num_t>(fe.bars));
	#endif

			}
			fe_to_data_set.range_scale.ovr_raw(msg->param1.range_scale, fe.scale, fe.scale_ovr);
		}

		fe_observer.mode=msg->rdr_mode_command.master_mode.get();
		//fe_observer.scale=msg->param1.range_scale.get();
		fe_observer.bars=msg->param1.bars_num.get();

		fe_observer.map=1;
		fe_observer.map_ref_valid=0;
		//memcpy((void*)fe_observer.a2, buffer, 32);
		break;
	}
	}

	if ((obs) && (msgid==icd1553::msg_nav_data_and_cursor_trait::sa))
	{
		b1553_udp_lock();
		fe_observer.marker=flying_engineer_observer_t::obs_marker;
		++fe_observer.updated;
		fe_observer.pending=1;
		b1553_udp_unlock();
	}
}

bool b1553_flying_engineer_takeover_is_active()
{
	return fe_active;
}

void b1553_flying_engineer_takeover_tx(int msgid, void* buffer)
{
	static int16_t pel;
	switch(msgid)
	{
	case icd1553::msg_settings_tellback_trait::sa:
	{
		icd1553::msg_settings_tellback_t* msg=reinterpret_cast<icd1553::msg_settings_tellback_t*>(buffer);
		int16_t el=msg->el_scan_centre.get_raw();
		//float az=msg->az_scan_centre.get();
		//if (fe_observer.frame_center!=el)
		if (el!=pel)
		{
			pel=el;
			fe_observer.frame_center=msg->el_scan_centre.get();
			++fe_observer.frame_center_updated;
		}
		fe_observer.az_center=msg->az_scan_centre.get();

		fe_observer.crs_latitude=msg->crs_lat.get();
		fe_observer.crs_longitude=msg->crs_long.get();
		break;
	}
	case icd1553::msg_rdr_status_tellback_trait::sa:
	{
		icd1553::msg_rdr_status_tellback_t* msg=reinterpret_cast<icd1553::msg_rdr_status_tellback_t*>(buffer);
		if (msg->mode.master_mode.get()!=icd1553::VS)
		{
			fe_observer.scale=msg->param1.range_scale.get();
		}
		else
			fe_observer.scale=icd1553::NM20;
	}
	}
}

#if 0
void b1553_flying_engineer_takeover_rx(int msgid, void* buffer)
{
	static unsigned int update_counter;
	static bool des_valid=true;
	static unsigned int evn_pending;
	static flying_engineer_takeover_t fe;
	static bool update_crs;
	static bool ovr_mode;
	static int ovr_ant;
	static int obs;

	b1553_udp_lock();
	memcpy(&fe, (void*)&fe_takeover, sizeof fe); //fe=fe_takeover;
	b1553_udp_unlock();

	//static icd1553::des_control_t des;


	bool updated=update_counter!=fe.takeover_update;
	update_counter=fe.takeover_update;

	if ((fe.takeover_flags & 1)==0)
	{
		evn_pending=0;
		update_crs=false;
		ovr_mode=false;
		ovr_ant=0;
		return;
	}

	if (updated)
	{
		if (fe.crs_ovr)
			update_crs=true;
		evn_pending=fe.evn;

		ovr_mode=fe.mode_ovr;
		ovr_ant=fe.ant_ovr;

		obs=fe.takeover_flags & flying_engineer_takeover_t::to_obs;
	}

	//unsigned int update_bit=(1<<msgid);

	switch(msgid)
	{
	case icd1553::msg_inu_high_speed_trait::sa:
	{
		icd1553::msg_inu_high_speed_t* msg=reinterpret_cast<icd1553::msg_inu_high_speed_t*>(buffer);
		fe_observer.ptaz=msg->platform_azimuth.get();
		break;
	}
	case icd1553::msg_nav_data_and_cursor_trait::sa:
	{
		icd1553::msg_nav_data_and_cursor_t* msg=reinterpret_cast<icd1553::msg_nav_data_and_cursor_t*>(buffer);
		if (update_crs)
		{
			msg->acq_crs_x.abs_pos.set(fe.crs_x);
			msg->acq_crs_y.abs_pos.set(fe.crs_y);
			msg->validity_and_slew.crs_rate_invalid.set(false);
			msg->acq_crs_x.abs.set(true);
			update_crs=false;
		}
		if (ovr_ant)
		{
			msg->el_antenna_demand.set(gc_math::deg_to_rad(fe.antenna_el));
			if (ovr_ant==2)
				msg->validity_and_slew.ant_slew_valid.set(true);
		}
		//memcpy((void*)fe_observer.a4, buffer, 32);
		fe_observer.magnetic_heading=msg->magnetic_heading.get();
		fe_observer.true_heading=msg->true_heading.get();
		fe_observer.latitude=msg->ppos_lat.get();
		fe_observer.longitude=msg->ppos_lon.get();

		break;
	}
	case icd1553::msg_settings_tellback_trait::sa:
	{
		icd1553::msg_settings_tellback_t* msg=reinterpret_cast<icd1553::msg_settings_tellback_t*>(buffer);
		fe_observer.if_gain=msg->gains_and_chgrp.if_gain.get();
		break;
	}

	case icd1553::msg_rdr_settings_and_parameters_trait::sa:
	{
		icd1553::msg_rdr_settings_and_parameters_t* msg=reinterpret_cast<icd1553::msg_rdr_settings_and_parameters_t*>(buffer);
		if (ovr_mode)
		{
			if ((fe.if_gain>0) && (fe.if_gain<=127))
				msg->gains_and_chgrp.if_gain.set(fe.if_gain);
		}
		fe_observer.if_gain=msg->gains_and_chgrp.if_gain.get();
		break;
	}

	case icd1553::msg_rdr_operation_command_trait::sa:
	{
		icd1553::msg_rdr_operation_command_t* msg=reinterpret_cast<icd1553::msg_rdr_operation_command_t*>(buffer);
		if (evn_pending)
		{
			des_valid=false;
			switch(evn_pending)
			{
			case 1:
				msg->rdr_mode_command.des_ctrl.set(icd1553::LOCK_ON);
				break;
			case 2:
				msg->rdr_mode_command.des_ctrl.set(icd1553::REJECT);
				break;
			case 0x801:
				msg->rdr_mode_command.des_ctrl.set(icd1553::LOCK_ON_DTT);
				break;
			case 0x802:
				msg->rdr_mode_command.des_ctrl.set(icd1553::TRANS_HPT_BUT);
				break;
			default:
				msg->rdr_mode_command.des_ctrl.set(icd1553::NOT_VALID);
			}
			evn_pending=0;
		}
		else
		{
			if (!des_valid)
				des_valid=msg->rdr_mode_command.des_ctrl.get()==icd1553::NOT_VALID;
			if (!des_valid)
				msg->rdr_mode_command.des_ctrl.set(icd1553::NOT_VALID);
		}
		if (ovr_mode)
		{
			if (fe.mode>=0)
			{
				msg->rdr_mode_command.master_mode.set(static_cast<icd1553::rdr_modes_t>(fe.mode));
				if (fe.submode>=0)
				{
					switch(fe.mode)
					{
					case icd1553::GM:
						msg->param1.gm_submode.set(static_cast<icd1553::gm_submode_t>(fe.submode));
						break;
					case icd1553::ACM:
						msg->param1.acm_submode.set(static_cast<icd1553::acm_submode_t>(fe.submode));
						break;
					default:
						break;
					}
				}
			}
			if ((fe.scale>=0) && (fe.scale<=3))
			{
				msg->param1.range_scale.set(static_cast<icd1553::range_scale_t>(fe.scale));
			}
			if ((fe.width>=0) && (fe.width<=3))
			{
				msg->param1.scan_width.set(static_cast<icd1553::scan_width_t>(fe.width));
			}
			if ((fe.bars>=0) && (fe.bars<=3))
				msg->param1.bars_num.set(static_cast<icd1553::bars_num_t>(fe.bars));

		}
		fe_observer.mode=msg->rdr_mode_command.master_mode.get();
		fe_observer.scale=msg->param1.range_scale.get();
		fe_observer.bars=msg->param1.bars_num.get();

		fe_observer.map=1;
		fe_observer.map_ref_valid=0;
		//memcpy((void*)fe_observer.a2, buffer, 32);
		break;
	}
	}

	if ((obs) && (msgid==icd1553::msg_nav_data_and_cursor_trait::sa))
	{
		b1553_udp_lock();
		fe_observer.marker=flying_engineer_observer_t::obs_marker;
		++fe_observer.updated;
		fe_observer.pending=1;
		b1553_udp_unlock();
	}
}
#endif

