/*
 * SimpleTgtSimInfo.cpp
 *
 */


#include "SimpleTgtSimInfo.h"

#include <math.h>

#ifdef _TMS320C6X
#define DSP_ATTR_COMMON_MEMORY __attribute__((section(".mcsm_common")))
#else
#define DSP_ATTR_COMMON_MEMORY
#endif

static SimpleTgtSimInfo autoMonitorable_simtgt DSP_ATTR_COMMON_MEMORY;

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookupExt(float az, float el, float ac_altitude)
{
	return lookupExtN(az, el, ac_altitude, 0);
}

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookup(float az, float el)
{
	return lookupN(az, el, 0);
}

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookupN(float az, float el, int startIndex)
{
	return lookupExtN(az, el, 0, startIndex);
}

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookupExtN(float az, float el, float ac_altitude, int startIndex)
{
    if (!enabled)
    {
        tgt_index=-1;
        return 0;
    }

    for(int i=startIndex; i<SimpleTgtSimInfo::k_max_tgt; ++i)
    {
        SimpleTgtSimInfo::tgt_t& tgt=t[i];
        if (tgt.enabled)
        {
        	tgt.id=i;
        	float tgt_az_witdh=tgt.az_width ? tgt.az_width : az_width;
        	float tgt_el_witdh=tgt.el_width ? tgt.el_width : el_width;

            float az_start=tgt.az-tgt_az_witdh;
            float az_stop=tgt.az+tgt_az_witdh;

            if ((az>=az_start) && (az<=az_stop))
            {
            	bool tgt_hit=true;

				tgt_start_us=tgt.rng_start;
				if (tgt_start_us==0)
					tgt.range=tgt.rng_meters;
				else
					tgt.range=(tgt_start_us/0.5)*73.5;

				if (tgt.range<10)
				{
					continue;
				}

            	if (features>1)
            	{
            		float tgt_el=tgt.el;
            		if (features>2)
            		{
            			//Use the altitude (relative to A/C
            			tgt_el=asinf((tgt.altitude-ac_altitude)/tgt.range);
            			tgt.el=tgt_el;
            		}

                    float el_start=tgt_el-tgt_el_witdh;
                    float el_stop=tgt_el+tgt_el_witdh;

                    if (!((el>=el_start) && (el<=el_stop)))
                    	tgt_hit=false;
            	}
            	if (tgt_hit)
            	{
					//tgt_start_us=tgt.rng_start;
					tgt_len_us=tgt.rng_len;
					tgt_amplitude=tgt.amplitude;
					tgt_sdr=tgt.sdr;
					++hit;
					++tgt.hit;
					tgt_index=i;
					tgt.just_seen=tgt.already_seen==0;
					tgt.already_seen=lookup_mode<0 ? 0 : 1;
#if 0
					if (tgt_start_us==0)
						tgt.range=tgt.rng_meters;
					else
						tgt.range=(tgt_start_us/0.5)*73.5;
#endif
					return &tgt;
            	}
            }
        }
    }
    tgt_index=-1;
    return 0;
}

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookupNext(float az, float el, tgt_t* previous)
{
	if (previous)
	{
		return lookupN(az, el, previous->id+1);
	}
	return 0;
}

SimpleTgtSimInfo::tgt_t* SimpleTgtSimInfo::lookupNextExt(float az, float el, float ac_altitude, tgt_t* previous)
{
	if (previous)
	{
		return lookupExtN(az, el, ac_altitude, previous->id+1);
	}
	return 0;
}

void SimpleTgtSimInfo::tracksUpdate(float delta_time, bool force_restart)
{
	tracksUpdateExt(delta_time, 0, force_restart);
}

void SimpleTgtSimInfo::tracksUpdateExt(float delta_time, float ac_altitude, bool force_restart)
{
    for(int i=0; i<SimpleTgtSimInfo::k_max_tgt; ++i)
    {
        SimpleTgtSimInfo::tgt_t& tgt=t[i];
        if (tgt.enabled)
        {
        	if ((tgt.rng_meters>0) || (!force_restart))
        	{
            	float r=tgt.rng_rate*delta_time;
            	if (tgt_move)
            	{
            		tgt.rng_meters-=r;
            		tgt.range=tgt.rng_meters;
            	}
        		if (features>2)
        		{
        			//Use the altitude (relative to A/C
        			float tgt_el=asinf((tgt.altitude-ac_altitude)/tgt.range);
        			tgt.el=tgt_el;
        		}
        		//tgt.az+=tgt.heading*delta_time;
        	}
    		if ((tgt.rng_meters<10) || (force_restart))
    		{
    			tgt.restarted=1;
    			tgt.rng_meters=tgt.rng_start_meters;
				tgt.range=tgt.rng_start_meters;
				tgt.az=tgt.az_start;
				tgt.el=tgt.el_start;
				tgt.altitude=tgt.altitude_start;
    		}
        }
    }
}

void SimpleTgtSimInfo::resetScan()
{
	for(int i=0; i<SimpleTgtSimInfo::k_max_tgt; ++i)
	{
		t[i].already_seen=0;
	}
}

SimpleTgtSimInfo::SimpleTgtSimInfo():
   enabled(0),
   features(0),
   sim_mp_hint(1),
   tgt_move(0),
   single_det_hint(1),
   tgt_start_us(0),
   tgt_len_us(0),
   tgt_amplitude(0),
   tgt_sdr(0),
   hit(0),
   tgt_index(-1),
   lookup_mode(0),
   space_invaders(0)
{
	az_width=0.042;
	el_width=0.042;

	for(int i=0; i<SimpleTgtSimInfo::k_max_tgt; ++i)
	{
		t[i].id=i;
		t[i].already_seen=0;
	}

	//t[0].az_width=0.05;
	t[0].rng_start_meters=8*1852; //us
	//t[1].az_width=0.05;
	t[1].rng_start_meters=10*1852; //us
	//t[2].az_width=0.05;
	t[2].az_start=0.1745;	//10 deg
	t[2].rng_start_meters=20*1852; //us
}

SimpleTgtSimInfo& SimpleTgtSimInfo::instance()
{
    return autoMonitorable_simtgt;
}


void SimpleTgtSimInfo::setLookupMode(int mode) //0=normal A/A, 1=A/S, -1=RBM
{
	lookup_mode=mode;
}

