/*
 * dsp_ris_mex.cpp
 *
 */

#include "dsp_ris_internals_.h"

#include "mex.h"
#include "gc_math.h"

struct ris_rfloat_t
{
	float value;
	bool relative;
};


extern "C" int mex_mt_helper_show_float(mex_mt_printf_style_function_t f, float value, int width, int precision);

static int mt_show_rfloat(const struct mex_mt_tag_t* const mt,
    mex_mt_printf_style_function_t f, const void* address, int width, int mode, const mex_mt_reference_t* ref)
{
  (void) mt;
  (void) mode;
  (void) ref;
  return mex_mt_helper_show_float(f, *(float*) address, width, -1);
}

extern int mex_mt_helper_read_float(float* f, const char** str);

static mex_cmd_param_parse_result_t mt_read_rfloat(
    const struct mex_mt_tag_t* const /*mt*/, void* address, const char** str, const mex_mt_reference_t* /*ref*/)
{
  ris_rfloat_t* result=reinterpret_cast<ris_rfloat_t*>(address);
  result->relative=false;
  result->value=0;
  if (**str=='%')
  {
	  result->relative=true;
	  ++(*str);
  }
  int nok=mex_mt_helper_read_float(&(result->value), str);
  if (nok)
	  return mex_cmd_param_parse_error;

  return mex_cmd_param_parse_ok;
}

static const mex_mt_methods_t ris_mt_relfloat_methods =
{ 0, mt_show_rfloat, mt_read_rfloat, 0, 0, 0};

const mex_mt_t ris_mt_relfloat =
{ "rfloat", sizeof(float), &ris_mt_relfloat_methods };


static float from_angle(float v)
{
	return gc_math::rad_to_deg(v); //v*cnv->angles;
}

struct target_params_t
{
	int id;
	ris_rfloat_t range;
	ris_rfloat_t az;
	ris_rfloat_t heading;
	ris_rfloat_t speed;
	ris_rfloat_t altitude;

	int restart;
	int start;
	int az_abs;
	int traceable;

	int altitude_is_el;

	struct par_validity_t
	{
		bool set_az;
		bool set_range;
		bool set_speed;
		bool set_heading;
		bool set_altitude;
		bool set_elevation;
		int set_id;
		int start;
		int restart;
	} validity;
	ris_rfloat_t generic_value;
};

static target_params_t tgt_params;

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

static const mex_cmd_param_descriptor_t target_table[] =
{
  { "id", "target id", 0, &tgt_params.id, &mex_mt_int},
  { "rng", "range (NM)", 0, &tgt_params.range, &ris_mt_relfloat},
  { "az", "azimuth (deg)", 0, &tgt_params.az, &ris_mt_relfloat},
  { "velocity", "speed (feet/s)", 0, &tgt_params.speed, &ris_mt_relfloat},
  { "heading", "heading (deg)", 0, &tgt_params.heading, &ris_mt_relfloat},
  { "altitude", "altitude (feet)", 0, &tgt_params.altitude, &ris_mt_relfloat},

  { "r", "restart", mex_cmd_qualifier, &tgt_params.restart },
  { "s", "start", mex_cmd_qualifier, &tgt_params.start },
  { "t", "traceable", mex_cmd_qualifier, &tgt_params.traceable },
  { "e", "set elevation instead of altitude (experimental)", mex_cmd_qualifier, &tgt_params.altitude_is_el },

  { 0 }
};


static void RisTgtTakeMutex()
{
}

static void RisTgtReleaseMutex()
{
}

static void RisTargetsRestart(int i)
{
	auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];

	t.az=t.az_start;
	t.ground_range=t.rng_start_meters;
	t.altitude=t.altitude_start;
	t.el=t.el_start;
	t.speed=t.speed_start;
	++t.unique_id;
}


static bool angle_convert(float& az, float v)
{
	az=0;
	if ((v>361) || (v<-361))
		return false;
	az=gc_math::angle_bound_pi(gc_math::deg_to_rad(v));
	return true;
}

static bool speed_convert(float& s, float v, bool relative)
{
	s=0;
	if (v>4000)
		return false;
	if (relative)
	{
		if (v<-4000)
			return false;
	}
	else if (v<0)
		return false;

	s=gc_math::feet_to_meters(v);
	return true;
}

static bool range_convert(float& r, float v)
{
	r=0;
	if ((v<0) || (v>200))
		return false;
	r=gc_math::nm_to_meters(v);
	return true;
}

static bool altitude_convert(float& r, float v)
{
	r=0;
	if ((v<-40000) || (v>40000))
		return false;
	r=gc_math::feet_to_meters(v);
	return true;
}

class SimpleTgtSimInfo
{
public:
	enum { k_max_tgt=DSP_RIS_MAX_TGT};
};

static mex_res_t Cmd_TargetApply(int mode)
{
	//RadarInterfaceSimulator& s=RadarInterfaceSimulator::instance();
	int start=tgt_params.validity.start; //mex_param_is_present(&tgt_params.stop);
	bool restart=tgt_params.validity.restart; //mex_param_is_present(&tgt_params.restart);


	int t_start=0;
	int t_stop=SimpleTgtSimInfo::k_max_tgt;
	bool only_active=start==0;
	if (tgt_params.validity.set_id) //mex_param_is_present(&tgt_params.id))
	{
		if ((tgt_params.id<0) || (tgt_params.id>=SimpleTgtSimInfo::k_max_tgt))
			return mex_err_invalid_range;
		t_start=tgt_params.id;
		t_stop=t_start+1;
		only_active=false;
	}

	float az=0;
	float el=0;
	float range=0;
	float heading=0;
	float altitude=0;
	float speed=0;

	bool set_az=tgt_params.validity.set_az;//mex_param_is_present(&tgt_params.az);
	bool set_range=tgt_params.validity.set_range;//mex_param_is_present(&tgt_params.range);
	bool set_speed=tgt_params.validity.set_speed;//mex_param_is_present(&tgt_params.speed);
	bool set_heading=tgt_params.validity.set_heading;//mex_param_is_present(&tgt_params.heading);
	bool set_altitude=tgt_params.validity.set_altitude;//mex_param_is_present(&tgt_params.altitude);
	bool set_elevation=tgt_params.validity.set_elevation;//mex_param_is_present(&tgt_params.altitude_is_el);

	if (set_az)
	{
		bool ok=angle_convert(az, tgt_params.az.value);
		if (!ok)
		{
			mex_msg_printf(mex_msg_error, "TGT", "invalid azimuth (%f)", tgt_params.az.value);
			return mex_ok;
		}
	}
	if (set_heading)
	{
		bool ok=angle_convert(heading, tgt_params.heading.value);
		if (!ok)
		{
			mex_msg_printf(mex_msg_error, "TGT", "invalid heading (%f)", tgt_params.heading.value);
			return mex_ok;
		}
	}
	if (set_speed)
	{
		bool ok=speed_convert(speed, tgt_params.speed.value, tgt_params.speed.relative);
		if (!ok)
		{
			mex_msg_printf(mex_msg_error, "TGT", "invalid velocity (%f)", tgt_params.speed.value);
			return mex_ok;
		}
	}
	if (set_range)
	{
		bool ok=false;
		if (tgt_params.range.relative)
		{
			range=tgt_params.range.value;
			ok=(tgt_params.range.value>=0) && (tgt_params.range.value<200.0);
		}
		else
			ok=range_convert(range, tgt_params.range.value);
		if (!ok)
		{
			mex_msg_printf(mex_msg_error, "TGT", "invalid range (%f)", tgt_params.range.value);
			return mex_ok;
		}
	}
	if (set_altitude)
	{
		if (set_elevation)
		{
			bool ok=angle_convert(el, tgt_params.heading.value);
			if (!ok)
			{
				mex_msg_printf(mex_msg_error, "TGT", "invalid elevation (%f)", tgt_params.altitude.value);
				return mex_ok;
			}
			set_altitude=false;
		}
		else
		{
			bool ok=altitude_convert(altitude, tgt_params.altitude.value);
			if (!ok)
			{
				mex_msg_printf(mex_msg_error, "TGT", "invalid altitude (%f)", tgt_params.altitude);
				return mex_ok;
			}
		}
	}


	{
		RisTgtTakeMutex();
		//AvionicsNavigationData* avn=AvionicsDataStore::getData()->getItsNavigationData();
		//ac_data.actualize(avn->getInu(), avn->getNavBlended());
		//RisData* d=RadarInterfaceSimulator::instance().risData();
		//ac_data.actualize(d->nav);
		RisTgtReleaseMutex();
	}

	const iddtypes::inu_data_t& inu=DspRisTgtInuData();

	if (tgt_params.range.relative)
	{
		//AvionicsControlData::settings_t& cmd=AvionicsDataStore::getData()->getItsControlData()->getBasicSettings();
		//float scale=cmd.range_scale_m;
		//range=(range/100)*scale;
	}

	if (tgt_params.speed.relative)
	{
		float v=dsp_ris_tgt_db.vxy; //sqrtf(inu.own_ship_velocity.vxac_data.vx*ac_data.vx+ac_data.vy*ac_data.vy);
		speed+=v;
	}

	if (tgt_params.heading.relative)
	{
		heading+=inu.own_ship_attitude.vector.platform_azimuth; //ac_data.platform_azimuth;
	}

	//bool abs_mode=mex_param_is_present(&tgt_params.az_abs);

	float reference_az=tgt_params.az.relative ? inu.own_ship_attitude.vector.platform_azimuth : 0;
	float reference_altitude=tgt_params.altitude.relative ? inu.inertial_altitude.data : 0;


	az+=reference_az;
	altitude+=reference_altitude;

	//SimpleTgtSimInfo& si=SimpleTgtSimInfo::instance();

	int traceable=mex_param_is_present(&tgt_params.traceable);

	RisTgtTakeMutex();
	for(int i=t_start; i<t_stop; ++i)
	{
		auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];

		if (start>0)
		{
			//if (t.rng_start_meters)
				t.enabled=1;
		}
		else if (start<0)
			t.enabled=0;

		if (only_active && !t.enabled)
			continue;

		if (mode==0)
		{
			if (set_az)
				t.az_start=az;
			if (set_range)
				t.rng_start_meters=range;
			if (set_heading)
				t.heading_start=heading;
			if (set_speed)
			{
				t.speed_start=speed;
				//t.rng_rate=0;
			}

			if (set_altitude)
			{
				t.altitude_start=altitude;
				t.altitude_invalid=false;
			}
			if (set_elevation)
			{
				t.el_start=el;
				t.altitude_invalid=true;
			}
		}//end only init

		if (traceable>0)
			t.not_traceable=0;
		else if (traceable<0)
			t.not_traceable=1;

		if (restart || ((start>0) && (mode==0)))
		{
#if 0
			t.az=t.az_start;
			t.range=t.rng_start_meters;
			t.speed=t.speed_start;
			t.heading=t.heading_start;
			t.altitude=t.altitude_start;
			t.el=t.el_start;
			if (mode==0)
			{
				t.restarted=1;
			}
#endif
			RisTargetsRestart(i);
		}

		if (mode==1)
		{
			if (set_az)
				t.az=az;
			if (set_range)
			{
				//t.rng_meters=range;
				t.ground_range=range;
			}
			if (set_heading)
				t.heading=heading;
			if (set_speed)
			{
				t.speed=speed;
			}
			if (set_altitude)
			{
				t.altitude=altitude;
				t.altitude_invalid=false;
			}
			if (set_elevation)
			{
				t.el=el;
				t.altitude_invalid=true;
			}
			if (restart)
				t.enabled=1;
			t.restarted=restart;
		}
	}
	RisTgtReleaseMutex();

	if (mode==0)
	{
		for(int i=t_start; i<t_stop; ++i)
		{
			auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];

			if (only_active && !t.enabled)
				continue;
			mex_printf(
				"%c%c%u: range=%7.3f, az=%8.3f, el=%8.3f vel=%5.3f, heading=%8.3f, altitude=%5.0f\n",
				t.enabled ? '+' : '-',
				t.not_traceable ? '-' : '+',
				i,
				gc_math::meters_to_nm(t.rng_start_meters), from_angle(t.az_start), from_angle(t.el_start), gc_math::meters_to_feet(t.speed_start), from_angle(t.heading_start), gc_math::meters_to_feet(t.altitude_start));
		}
		return mex_ok;
	}

	mex_page_loop_init(1);
	do
	{
	    mex_page_loop_top();

		mex_printf("      %3.3s %7.7s %6.6s %8.8s %8.8s %8.8s  %8.8s %8.8s %8.8s %8.8s %8.8s %s  \n",
				"TRK", "GR[NM]", "(R[m])", "AZ^", "VEL[FT]", "HEAD", "ALT[FT]", "(EL)", "(RR[ms])", "(RV[ms])", "(hit)", "(cc)");

		for(int i=t_start; i<t_stop; ++i)
		{
			auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];

			//mex_printf("%c%c%2u: %3d range=%7.3f, az=%8.3f, el=%8.3f, vel=%5.3f, heading=%8.3f, altitude=%5.0f\n",
			mex_printf("%c%c%2u: %3d %7.2f %6d %8.3f %8.3f %8.3f%c %8.1f %8.3f %8.3f %8.3f %8u %5u  \n",
					t.enabled ? '+' : '-',
					t.not_traceable ? '-' : '+',
					i,
					(t.unique_id & 0x0FF), //t.unique_id<0 ? -1 : (t.unique_id & 0x0FF),
					gc_math::meters_to_nm(t.ground_range),
					int(t.slant_range),
					from_angle(t.az),
					gc_math::meters_to_feet(t.speed),
					from_angle(t.heading),

					((t.turn_duration>0) && (t.turn)) ? ((t.turn>0) ? '>' : '<') : '=',

					gc_math::meters_to_feet(t.altitude),
					from_angle(t.el),
					t.range_rate,
					t.rad_vel,
					t.hits,
					t.ext_updated & 0x0FFFF
					);
		}
	} while (mex_page_loop_end(0, 0) != mex_page_event_quit);

	return mex_ok;
}


static mex_res_t Cmd_Target(int mode)
{
	tgt_params.validity.set_az=mex_param_is_present(&tgt_params.az);
	tgt_params.validity.set_range=mex_param_is_present(&tgt_params.range);
	tgt_params.validity.set_speed=mex_param_is_present(&tgt_params.speed);
	tgt_params.validity.set_heading=mex_param_is_present(&tgt_params.heading);
	tgt_params.validity.set_altitude=mex_param_is_present(&tgt_params.altitude);
	tgt_params.validity.set_elevation=mex_param_is_present(&tgt_params.altitude_is_el);
	tgt_params.validity.start=mex_param_is_present(&tgt_params.start);
	tgt_params.validity.restart=mex_param_is_present(&tgt_params.restart);
	tgt_params.validity.set_id=mex_param_is_present(&tgt_params.id);

	return Cmd_TargetApply(mode);
}

static mex_res_t Cmd_TgtInit()
{
	return Cmd_Target(0);
}

static mex_res_t Cmd_TgtSet()
{
	return Cmd_Target(1);
}

static float turn_deg;
static float turn_time;

static const mex_cmd_param_descriptor_t target_turn_table[] =
{
  { "id", "target id", 0, &tgt_params.id, &mex_mt_int},
  { "deg", "deg/s", 0, &turn_deg, &mex_mt_float},
  { "time", "turn duration", 0, &turn_time, &mex_mt_float},
  { 0 }
};

static mex_res_t Cmd_TgtTurn()
{
	int t_start=0;
	int t_stop=SimpleTgtSimInfo::k_max_tgt;
	if (tgt_params.validity.set_id) //mex_param_is_present(&tgt_params.id))
	{
		if ((tgt_params.id<0) || (tgt_params.id>=SimpleTgtSimInfo::k_max_tgt))
			return mex_err_invalid_range;
		t_start=tgt_params.id;
		t_stop=t_start+1;
	}

	bool set_turn=mex_param_is_present(&turn_deg);
	bool set_time=mex_param_is_present(&turn_time);

	turn_deg=gc_math::deg_to_rad(turn_deg);

	for(int i=t_start; i<t_stop; ++i)
	{
		auto& t=dsp_ris_tgt_db.t[i]; //SimpleTgtSimInfo::tgt_t& t=si.t[i];

		if (set_turn)
			t.turn=turn_deg;
		if (set_time)
		{
			t.turn_duration=turn_time;
		}
	}

	return mex_ok;
}


extern const mex_cmd_descriptor_t cmd_tgtinit=
{ "tgtset", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtInit, target_table};


extern const mex_cmd_descriptor_t cmd_tgtset =
{ "tgtset", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtSet, target_table};


extern const mex_cmd_descriptor_t cmd_tgtturn =
{ "tgtturn", "Manipulate targets", (mex_cmd_handler_t) Cmd_TgtTurn, target_turn_table};