/*
 * cnv_d1553.cpp
 *
 *  Created on: 11/mag/2022
 *      Author: chessaa
 */

#include <stdio.h>
#include <stdint.h>

#include <math.h>
#ifndef M_PI
#define M_PI		3.14159265358979323846
#endif

#include "gtl_cmdio.h"

//
struct avionics_adaptor_message_store_t
{
	uint32_t tag;
	//RX
	uint32_t att[9];
	uint16_t a1[32];//icd1553::msg_rdr_settings_and_parameters_trait::wc];
	uint16_t a2[32];//icd1553::msg_rdr_operation_command_trait::wc];
	uint16_t a3[32];//icd1553::msg_graphic_setting_trait::wc];
	uint16_t a4[32];//icd1553::msg_nav_data_and_cursor_trait::wc];
	uint16_t a5[32];//icd1553::msg_inu_high_speed_trait::wc];
	uint16_t a7[32];//icd1553::msg1_data_link_target_trait::wc];
	uint16_t a8[32];//icd1553::msg2_data_link_target_trait::wc];
	uint16_t x1[32];//icd1553::msg_dev0_reserved_trait::wc];

	uint32_t btt[32];

	uint16_t b1[32];//icd1553::tws_status_and_targets_01_02_trait::wc];
	uint16_t b2[32];//icd1553::tws_status_and_targets_03_04_05_trait::wc];
	uint16_t b3[32];//icd1553::tws_status_and_targets_06_07_08_trait::wc];
	uint16_t b4[32];//icd1553::spt_target_message_trait::wc];
	uint16_t b5[32];//icd1553::hpt_target_message_trait::wc];

	uint16_t b6[32];//icd1553::msg_settings_tellback_trait::wc];
	uint16_t b7[32];//icd1553::msg_rdr_status_tellback_trait::wc];
	uint16_t b8[32];//icd1553::bit_report_message_trait::wc];

	uint16_t b9[32];//icd1553::msg_b09_trait::wc];
	uint16_t b10[32];//icd1553::msg_b10_trait::wc];

	uint16_t b11[32];//icd1553::msg_b10_trait::wc];
	uint16_t b12[32];//icd1553::msg_b10_trait::wc];
	uint16_t b13[32];//icd1553::msg_b10_trait::wc];
	uint16_t b14[32];//icd1553::msg_b10_trait::wc];
	uint16_t b15[32];//icd1553::msg_b10_trait::wc];
	uint16_t b16[32];//icd1553::msg_b10_trait::wc];
	uint16_t b17[32];//icd1553::msg_b10_trait::wc];
	uint16_t b18[32];//icd1553::msg_b10_trait::wc];

    //AvionicsBusDriver::message_descriptor_t msglist[32];
};


#define ICD1553_GEOPOS_SC_LSB	(4.65661E-10)
#define ICD1553_GEOPOS_DEG_LSB	(ICD1553_GEOPOS_SC_LSB*180)

#define ICD1553_SEMICIRCLE_LSB	(3.05176E-05f)
#define ICD1553_SEMICIRCLE_DEG_LSB	(180*ICD1553_SEMICIRCLE_LSB)

#define GC_K_METERS_TO_FEETS (3.280839895f)
#define GC_K_FEETS_TO_METERS (1.0f/GC_K_METERS_TO_FEETS)

#define ICD1553_BARO_ALT_FEET_LSB	(4.0)
#define ICD1553_BARO_ALT_METERS_LSB	(ICD1553_BARO_ALT_FEET_LSB*GC_K_FEETS_TO_METERS)

#define ICD1553_VELOCITY_FEET_LSB (3.81470E-06)
#define ICD1553_VELOCITY_METERS_LSB	(ICD1553_VELOCITY_FEET_LSB*GC_K_FEETS_TO_METERS)

enum rdr_modes_t
{
  RWS=0,
  VS,
  ACM,
  TWS,
  MODE_SPARE_4=4,
  GM,
  SEA_LOW,
  SEA_HIGH,
  GMTI,
  BCN,
  AGR,
  TA,
  WA,
  TS,
  MODE_SPARE_14,
  RDR_DLY  //this is actually only a tell-back, a not command-able mode
};

typedef enum
{
    idle_master_mode,
    int_bit_master_mode,
    gm_master_mode,
    dbs_master_mode,
    rws_master_mode,
    vs_master_mode,
    acm_master_mode,
    tws_master_mode,
    sea_low_master_mode,
    sea_high_master_mode,
    gmti_master_mode,
    bcn_master_mode,
    sam_master_mode,
    ta_master_mode,
    wa_master_mode,
    stt_master_mode,
    dtt_master_mode,
    sstt_master_mode,
    acq_master_mode,
    ftt_master_mode,
    agr_master_mode,

    sar_master_mode,

    invalid_master_mode_,

    xtst_dummy_mode,                 /*to be used only as a defualt mode for prototypal development*/
    xtst_hw_validation_mode,    /*dedicated to special REP test for HW validation*/

    boot_master_mode,

    master_mode_id_cardinality_
} master_mode_t;


static FILE* fgpx=0;
static FILE* fd=0;

static void d1553_gpx_close()
{
	if (fgpx)
	{
		fprintf(fgpx, "</trkseg>\n</trk>\n</gpx>\n");
		fclose(fgpx);
	}
	if (fd)
		fclose(fd);
}

static float w2f(uint16_t w0, uint16_t w1, float lsb)
{
	int32_t tmp32=((unsigned int)w0)<<16 | ((unsigned int)w1);
	return tmp32*lsb;
}

const char* mk_fname(const char* prefix, const char* ext);

extern unsigned int n_batch;
extern unsigned int r_mode;

char time_of_mission[128];
const char* lbl_mode;

void d1553_extract(const void* data, int gps_save_mode)
{
	static unsigned ss=0;
	static unsigned int t_start;
	static double plat;
	static double plon;

	const avionics_adaptor_message_store_t* x=(const avionics_adaptor_message_store_t*)&(((uint32_t*)data)[36]);

	if (data==0)
	{
		d1553_gpx_close();
		return;
	}

	if (gps_save_mode<0)
		return;

	if (fd==0)
	{
		const char* fname=mk_fname("track", "txt"); //"ftrack.txt"; //make_fname("ftrak")
		fd=fopen(fname, "w");
		setvbuf(fd, 0,  _IOFBF, 100*1024);
		t_start=x->tag;

		//const char* csv_prologue=
		//		"tt64;dttt64;time;mode;sub;scale;lat;lon;alt(m);thead;mhead;ptaz";
		//fprintf(fd, csv_prologue);
		fprintf(fd, "%32s;%8s;%8s;%8s;%19s;%2s;%4s;%2s;%6s;%11s;%11s;%7s;%11s;%11s;%11s;%11s;%11s;%11s\n",
				"file", "batch", "a5tt", "tt64","dtt64","time","M","Mode", "S","SC","lat","lon","alt(m)","thead","mhead","ptaz","vx[m/s]","vy[m/s]","vz[m/s]");

		const char* gpx_fname=mk_fname("track", "gpx"); //"ftrack.gpx";
		const char* gpx_prologue=
				"<?xml version=\"1.0\" encoding=\"utf-8\" standalone=\"yes\"?>\n"
				"<gpx version=\"1.1\" creator=\"g_reconvert\" xmlns=\"http://www.topografix.com/GPX/1/1\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\">\n"
				"<trk>\n"
				"<name>FT-%s</name>\n"
				"<trkseg>\n";

		char gpx_buffer[8*1024];
		sprintf(gpx_buffer, gpx_prologue, fname); //outname);

		fgpx=fopen(gpx_fname, "w");
		setvbuf(fgpx, 0,  _IOFBF, 100*1024);

		fprintf(fgpx, gpx_buffer);
	}
	if (!fd)
		return;

	const char* proc_file=cmdio_get_proc_file(0);

	uint16_t a5_tt=x->a5[1];

	//char time_of_mission[128];
	double lat=0;
	double lon=0;
	uint16_t date=x->a1[5];
	uint16_t time=x->a1[6];
	unsigned int tt=time*2;
	unsigned int h=tt/3600;
	unsigned int m=(tt-h*3600)/60;
	unsigned int s=(tt-(h*3600+m*60));

	int tmp32=((unsigned int)x->a4[23])<<16 | ((unsigned int)x->a4[24]);
	lat=tmp32*ICD1553_GEOPOS_DEG_LSB;

	//tmp32=((unsigned int)x->a4[25])<<16 | ((unsigned int)x->a4[26]);
	//lon=tmp32*ICD1553_GEOPOS_DEG_LSB;
	lon=w2f(x->a4[25], x->a4[26], ICD1553_GEOPOS_DEG_LSB);

	float ele=x->a4[9]*ICD1553_BARO_ALT_METERS_LSB;

	//unsigned int mode=x->b7[0]>>12;
	//unsigned int sub=(x->b7[1]>>10) & 1;
	unsigned int scale=(x->b7[1]>>6) & 0x3;
	//unsigned int des=(x->b7[0]>>9) & 0x7;

	float thead=((signed short)x->a4[2])*ICD1553_SEMICIRCLE_DEG_LSB;
	float mhead=((signed short)x->a4[3])*ICD1553_SEMICIRCLE_DEG_LSB;

	float ptaz=((signed short)x->a5[8])*ICD1553_SEMICIRCLE_DEG_LSB;

	float vx=w2f(x->a5[2], x->a5[3], ICD1553_VELOCITY_METERS_LSB);
	float vy=w2f(x->a5[4], x->a5[5], ICD1553_VELOCITY_METERS_LSB);
	float vz=w2f(x->a5[6], x->a5[7], ICD1553_VELOCITY_METERS_LSB);

	float gta=atan2(vx,vy)*(180/M_PI);

	bool gps_update=false;

	if (gps_save_mode==0)
		gps_update=true;
	else if (gps_save_mode==1)
	{
		if (s!=ss)
			gps_update=true;
	}
	else
	{
		if ((s!=ss) || (plat!=lat) || (plon!=lon))
			gps_update=true;
	}

	ss=s;
	plat=lat;
	plon=lon;

	if (!gps_update)
		return;

	int m_scale=10;
	switch(scale)
	{
	default:
		break;
	case 0:
		m_scale=80;
		break;
	case 1:
		m_scale=40;
		break;
	case 2:
		m_scale=20;
		break;
	}
	m_scale=m_scale*1852;

	lbl_mode="???";

	static unsigned int p_r_mode;

	int mm=r_mode; //rws_master_mode;
	switch(mm)
	{
	default:
		break;
	case tws_master_mode:
		lbl_mode="TS";
		break;
	case rws_master_mode:
		lbl_mode="RWS";
		break;
	case vs_master_mode:
		lbl_mode="VS";
		break;
	case stt_master_mode:
		lbl_mode="STT";
		break;
	case gm_master_mode:
		lbl_mode="GM";
		break;
	case dbs_master_mode:
		lbl_mode="DBS";
		break;
	case wa_master_mode:
		lbl_mode="WA";
		break;
	case acm_master_mode:
		lbl_mode="ACM";
		break;
	case int_bit_master_mode:
		lbl_mode="IBIT";
		break;
	case sea_low_master_mode:
		lbl_mode="SEA1";
		break;
	}

	if (r_mode!=p_r_mode)
	{
		p_r_mode=r_mode;
		cmdio_pinfo("Mode=%s (%d)", lbl_mode, r_mode);
	}

#if 0
	if (mode==GM)
	{
		if (sub==1)
			mm=dbs_master_mode, lbl_mode="DBS";
		else
			mm=gm_master_mode, lbl_mode="RBM";
	}
	else if (mode==VS)
		mm=vs_master_mode, lbl_mode="VS";
	else if (mode==WA)
		mm=wa_master_mode, lbl_mode="WA";
	else if (mode==ACM)
		mm=acm_master_mode, lbl_mode="ACM";
	else if (mode==RWS)
	{
		if (des==0)
			mm=stt_master_mode, lbl_mode="STT";
	}
#endif

	unsigned int batch=n_batch;

	sprintf(time_of_mission, "%04u-%02u-%02uT%02u:%02u:%02u",
			2022, //((date>>1) & 0x1F)+2000,
			(date>>6) & 0xF,
			(date>>10) & 0x3F,
			h, m, s);

	fprintf(fd, "%32s;%8u;%8u;%8u;%8u;%s;%2u;%4s;%2u;%6u;%12.8f;%12.8f;%7.0f;%11.6f;%11.6f;%11.6f;%11.6f;%11.6f;%11.6f;%11.6f\n",
			proc_file,
			batch,
			a5_tt,
			x->tag,
			x->tag-t_start,
			time_of_mission,
			mm, lbl_mode, //mode, sub,
			scale, m_scale,
			lat, lon,
			ele,
			thead, mhead,
			ptaz,
			vx, vy, vz, gta
			);

	//fflush(fd);

	fprintf(fgpx, "  <trkpt lat=\"%f\" lon=\"%f\"><ele>%f</ele><time>%s.0</time></trkpt>\n", lat, lon, ele, time_of_mission);
	//fflush(fgpx);

	//cmdio_pinfo("TOM: %2u:%2u:%2u,  lat=%f, lon=%f", h, m, s, lat, lon);

}



