#include "softscanconverter.h"

#include "gc_geometry.h"

#include "gc_math.h"

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

#include <math.h>

#define USE_RTLOG

#ifdef USE_RTLOG
#include "rtlog.h"
#include "rtlog_time_measure.h"

#include "mex_ext_rtlog.h"

#include "mex.h"

#else
enum rtlog_severity_enum_t
{
  rtlog_debug = -20,
  rtlog_trace = -10,
  rtlog_info = 0,

  rtlog_breaking_news = 1,  /*note: this is a modifier, applicable to all the other level, as in "rtlog_report|rtlog_breaking_news"*/

  rtlog_success = 10,
  rtlog_report = 20,

  rtlog_warning = 30,

  /*Here begin the errors log. Typically, you log level will be always >= of rtlog_assert*/
  rtlog_assertion = 40,
  rtlog_error = 50,

  rtlog_fatal = 60, /*!!! unrecoverable error !!!!*/

  rtlog_data = 70,

  rtlog_severity_cadinality_
};

static void rtlog_printf(const char*, unsigned int, const char* fmt, ...)
{
}
#endif

//#include <QDebug>
//#define NO_ANGULAR_INTERPOLATION
//#define NO_RANGE_INTERPOLATION

#ifndef M_PI
#define SC_PI 3.14159265358979323846
#else
#define SC_PI M_PI
#endif

const float k_pi=SC_PI;
const float k_2_pi=2*SC_PI;

static float angle_bound_pi(float x)
{
  while(x>k_pi)
    x-=k_2_pi;
  while(x<-k_pi)
    x+=k_2_pi;
  return x;
}

template<typename T_> static inline void sc_swap(T_ a, T_ b)
{
    T_ tmp=a;
    a=b;
    b=tmp;
}

//#include <QDebug>
#define USE_MUTEX

#ifdef USE_MUTEX
#include "mrk.h"

class  ScMutex
{
public:
	mrk_mutex_t mutex;

	bool acquire()
	{
		mrk_sts_t r=mrk_mutex_acquire(&mutex, 100);
		return r==mrk_ok ? true : false;
	}
	void release()
	{
		mrk_mutex_release(&mutex);

	}
	bool acquireTry()
	{
		mrk_sts_t r=mrk_mutex_acquire(&mutex, mrk_no_wait);
		return r==mrk_ok ? true : false;
	}
};

#else
class  ScMutex
{
public:

	inline void acquire()
	{

	}
	inline void release()
	{
	}
};
#endif

static ScMutex scMutex;
//static ScMutex scDbgMutex;

#define MAX_RBIN    4096 //2048

#if 0 //GC_GEOMETRY_MFD==484

#define MAX_PIXEL   512

#define SC_STRIDE	512
#define SC_SRCSHIFT  9

#else
#define MAX_PIXEL   gc_cfg::XY_MAX_LOGICAL_DISPLAY_SIZE  /*gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION*/ /*(640)*/
#define CV_TAB_DX	1024
#define CV_TAB_DY	MAX_PIXEL

#define SC_STRIDE	1024  /*512*/
#define SC_SRCSHIFT  10   /*9*/
#endif

typedef float real_coeff_t;

#define SC_FX_A_SHIFT	14
#define SC_FX_A_LSB 16384

#define SC_SMALL_CV_TAB  //try to reduce tab dimension for better change hit
#ifdef SC_SMALL_CV_TAB
typedef int cv_tab_index_t;
typedef int cv_tab_rng_t;
typedef int cv_tab_real_angle_t;

#define FX_ANG(x_)	((x_)>>SC_FX_A_SHIFT)

static inline cv_tab_real_angle_t cv_tab_toang(float v)
{
	return (v*SC_FX_A_LSB+0.5f);
}

#define FX_RNG(x_)	((x_)>>8)
#define FX_RNG2(x_)	((x_)>>16)
#define FX_RNGF(x_)	(float(x_)/256.0f)

static inline cv_tab_rng_t cv_tab_torng(float v)
{
	return (v*256+0.5f);
}

#else

#define FX_ANG(x_)	(x_)
#define FX_RNG(x_)	(x_)
#define FX_RNG2(x_)	(x_)

typedef int cv_tab_index_t;
typedef float cv_tab_rng_t;
typedef float cv_real_angle_t

static inline cv_real_angle_t cv_tab_toang(float v)
{
	return v;
}

static inline cv_tab_rng_t cv_tab_torng(float v)
{
	return v;
}

#endif

struct cv_pixel_t
{
    //real_coeff_t coeff;
    //int rng_index;
	cv_tab_index_t angle_index;
	cv_tab_rng_t rng;
	cv_tab_real_angle_t real_angle;
};

struct cv_table_t
{
    cv_pixel_t tab[CV_TAB_DY][CV_TAB_DX];
};

#ifdef __arm__
#define ATTRIBUTE_NOLOAD __attribute__((section (".bss.data_noload")))
#else
#define ATTRIBUTE_NOLOAD
#endif

static cv_table_t cvt ATTRIBUTE_NOLOAD;

#define MAX_SAMPLES 4096

struct sweep_t
{
    typedef uint8_t sample_type_t;
    sample_type_t samples[MAX_SAMPLES];

    float real_angle;
    //float scale;
    int num_samples;
    //sample_type_t samples[MAX_SAMPLES];
};

#define SWEEP_BUFFER_LEN	(2048) //(4096)
#define SWEEP_BUFFER_LEFT	(-1024) //(-2048)
#define SWEEP_BUFFER_RIGHT	(1024) //(2047)

#define SWEEP_RESOLUTION	((2*SC_PI)/float(SWEEP_BUFFER_LEN))

//#define SWEEP_RESOLUTION    0.002
//#define SWEEP_BUFFER_LEN    int(2*SC_PI/SWEEP_RESOLUTION)

static inline int a2si(float angle)
{
    //angle=angle_bound_pi(angle+reference);
	float a=angle/SWEEP_RESOLUTION;	//assuming angle already reduced to +-pi
	int i=a<0 ? a-0.5 : a+0.5;
	if (i<SWEEP_BUFFER_LEFT)
		i=SWEEP_BUFFER_LEN+i;
	else if (i>SWEEP_BUFFER_RIGHT)
		i=(-SWEEP_BUFFER_LEN)+i;
    return i;
}

struct sweep_buffer_t
{
    sweep_t spweep[SWEEP_BUFFER_LEN];

    //int association[SWEEP_BUFFER_LEN];
};

class sweep_list
{
public:
    typedef uint8_t index_t;
    enum { BUF_SIZE = 64, BUF_MASK=BUF_SIZE-1}; //UINT8_MAX+1}; //should be BUF_SIZE = 256

    enum {SWEEP_INDEX_LIMIT=SWEEP_BUFFER_LEN/2};

    sweep_t sweeps[BUF_SIZE];
    sweep_t null_sweep;

    index_t index;
    sweep_t* association_store[2][SWEEP_BUFFER_LEN];
    sweep_t** sweep_index;
    sweep_t** p_sweep_index;
    float ia_factor[SWEEP_BUFFER_LEN];
    float* iaf;
    float last_angle;
    float reference;
    int last_index;
    int max_pixel;
    float scale;
    int num_samples;
    bool interpolation_active;

    sweep_list()
    {
        last_index=0;

        sweep_index=&association_store[0][SWEEP_BUFFER_LEN/2];
        p_sweep_index=&association_store[1][SWEEP_BUFFER_LEN/2];
        iaf=&ia_factor[SWEEP_BUFFER_LEN/2];

        memset(&null_sweep, 0, sizeof null_sweep);
        for(int i=0; i<MAX_SAMPLES; ++i)
            null_sweep.samples[i]=0;
        null_sweep.real_angle=0; //1000;
        null_sweep.num_samples=128;
        num_samples=0;
        interpolation_active=false;
        //null_sweep.scale=128/float(MAX_PIXEL);
        setPixel(MAX_PIXEL);
        clear();
    }
    void setPixel(int n)
    {
        max_pixel=n;
        scale=128.0f/max_pixel;
        //null_sweep.scale=scale;
    }

    void clear()
    {
        for(int i=0; i<SWEEP_BUFFER_LEN; ++i)
        {
            association_store[0][i]=&null_sweep;
            association_store[1][i]=&null_sweep;
            ia_factor[i]=1;
        }
    }

    void reinit(float ref)
    {
        //clear();
        reference=ref;
    }

    void setReference(float ref)
    {
        reference=ref;
    }

    bool add(float angle, float* data, int n=MAX_SAMPLES)
    {
    	bool hole_detected=false;
        index_t id=index;
        ++index;

        id=id & BUF_MASK;

        sweep_t* s=&sweeps[id]; //lst.back();

        s->real_angle=angle;
       	scale=n/float(max_pixel);

        //s->scale=scale;;
        s->num_samples=n;
        for(int i=0; i<n; ++i)
        {
           int x=data[i];
#if 0 	//Performance: assuming already limited
           if (x>255)
        	   x=255;
           if (x<0)
        	   x=0;
#endif
           s->samples[i]=x; //data[i];
        }
        //last_angle=angle;
        int si=a2si(angle);
        sweep_index[si]=s;
        p_sweep_index[si]=s;
        iaf[si]=1;

        rtlog_printf("SC", rtlog_trace, "SW+: %f (%d) -> %f (%d)", angle, si, last_angle, last_index);

        num_samples=n;
#if 1
        float delta_angle=angle-last_angle;
        delta_angle=gc_math::angle_bound_pi(delta_angle);
        if (delta_angle<0)
        	delta_angle=-delta_angle;
        if (delta_angle>(0.1745329251994329/2))	//10deg
        {
        	last_index=si;
        	hole_detected=true;
        }
#endif

        if (si!=last_index)
        {
            //float sweep_angle=si*SWEEP_RESOLUTION;
            sweep_t* p_sweep=sweep_index[last_index];

            p_sweep_index[si]=p_sweep;

//#ifdef NO_ANGULAR_INTERPOLATION
            int middle_index=a2si(last_angle+((angle-last_angle)/2.0));
#ifndef NO_ANGULAR_INTERPOLATION
            float dx=fabsf(s->real_angle-last_angle);
            iaf[si]=fabsf(s->real_angle-si*SWEEP_RESOLUTION)/dx;

            float angle_inc=SWEEP_RESOLUTION/dx;
#endif

            if (si<last_index)
            {
                --last_index;
                float angle_base=(s->real_angle-last_index*SWEEP_RESOLUTION)/dx;

                for(int i=last_index; i>si; --i)
                {
#ifdef NO_ANGULAR_INTERPOLATION
                    //float delta=fabs(i*SWEEP_RESOLUTION-sweep_index[i]->real_angle);
                    if (i<middle_index) //max_delta<delta)
                        sweep_index[i]=s;
                    else
                        sweep_index[i]=p_sweep;
#else
                    if (interpolation_active)
                    {
                    	sweep_index[i]=s;
                    	p_sweep_index[i]=p_sweep;
                    	iaf[i]=fabsf(angle_base); //fabsf(s->real_angle-i*SWEEP_RESOLUTION)/dx;
                    	angle_base+=angle_inc;
                    }
                    else
                    {
                    	if (i<middle_index) //max_delta<delta)
                    		sweep_index[i]=s;
                    	else
                    		sweep_index[i]=p_sweep;
                    	iaf[i]=1;
                    }
#endif
                }
            }
            else
            {
                ++last_index;
                float angle_base=(s->real_angle-last_index*SWEEP_RESOLUTION)/dx;

                for(int i=last_index; i<si; ++i)
                {
#ifdef NO_ANGULAR_INTERPOLATION
                    //float delta=fabs(i*SWEEP_RESOLUTION-sweep_index[i]->real_angle);
                    if (i>middle_index)
                        sweep_index[i]=s;
                    else
                        sweep_index[i]=p_sweep;
#else
                    if (interpolation_active)
                    {
                    	sweep_index[i]=s;
                    	p_sweep_index[i]=p_sweep;
                    	iaf[i]=fabsf(angle_base); //fabsf(s->real_angle-i*SWEEP_RESOLUTION)/dx;
                    	angle_base-=angle_inc;
                    }
                    else
                    {
                    	if (i>middle_index) //max_delta<delta)
                    		sweep_index[i]=s;
                    	else
                    		sweep_index[i]=p_sweep;
                    	iaf[i]=1;
                    }
#endif
                }
            }
        }
        last_index=si;
        last_angle=angle;
        return hole_detected;
    }
    sweep_t* lookup(float angle)
    {
        float delta=100000;
        index_t nx=index;
        index_t n=nx-1;
        sweep_t* s=0;
        for(; n!=nx; --n)
        {
            sweep_t* i=&sweeps[n];
            float d=fabs(angle-i->real_angle);
            if (d<delta)
            {
                s=i;
                delta=d;
            }
        }
        if (s==0)
            return 0;
        return s;
    }
    struct sweep2_t
    {
        sweep_t* s[2];
        float af;
    };

    inline sweep2_t lookup2(int id, int /*direction*/)
    {
        sweep2_t s2;
#if 0
       s2.s[0]=sweep_index[id];
       int id2=id-direction;
       if (id2>=SWEEP_INDEX_LIMIT)
           id2=id;
       else if (id2<=-SWEEP_INDEX_LIMIT)
           id2=id;

       s2.s[1]=sweep_index[id2];
#endif
       s2.s[0]=sweep_index[id];
       s2.s[1]=p_sweep_index[id];
       s2.af=iaf[id];
       return s2;
    }

    inline sweep_t* lookup(int id)
    {
    	return sweep_index[id];
    }
/*
    inline sweep_t* lookup(int id)
    {
       return sweep_index[id];
    }
*/
};

static sweep_list slist;

static void precalculate(int n, int cx, int cy)
{
    cv_table_t& t=cvt;

    //const int cx=255;
    //const int cy=511;
    //const int n=512;

    //const float rng_scale=float(MAX_SAMPLES)/float(MAX_PIXEL);

    for(int y=0; y<n; ++y)
    {
        int py=cy-y;
        for(int x=0; x<n; ++x)
        {
            int px=x-cx;

            float angle=atan2(-px, py);
            //angle+=M_PI;
            //angle=-angle;

            t.tab[y][x].real_angle=cv_tab_toang(angle);
            t.tab[y][x].angle_index=a2si(angle); //(angle/SWEEP_RESOLUTION);

            float rng=sqrt(px*px+py*py);
            //t.tab[x][y].rng_index=rng*rng_scale;
            t.tab[y][x].rng=cv_tab_torng(rng);
        }
    }
}

struct sc_context_t
{
    int current_index;
    int dimension;
    int size;
    int stride;
    int cx;
    int cy;

    float last_angle;
    float current_angle;
    bool hole;
    //float rng_scale;
    float reference;
    float current_reference;
    float scan_center;
    float left_limit;
    float right_limit;
    int direction;
    int inversion;

    //bool angular_interpolation;
    bool range_interpolation;
    bool ang_interpolation;

    float tx_accumulator;
    float ty_accumulator;
    bool first_sweep;

    uint8_t* buffer[2];    

    rtlog_time_measure_t sc_time;
    rtlog_time_measure_t sw_time;

    int zoom_factor;
    float zoom_ox;
    float zoom_oy;
    int zoom_px;
    int zoom_py;

    int old_update;

    sc_context_t():
        current_index(0),
        dimension(0),        
        size(0),
        stride(0),
        cx(0),
        cy(0),
        last_angle(0),
        current_angle(0),
        hole(false),
        //rng_scale(0),
        reference(0),
        current_reference(0),
        scan_center(0),
        left_limit(0),
        right_limit(0),
        direction(0),
        inversion(0),
        //angular_interpolation(false),
        range_interpolation(false),
        ang_interpolation(false),
        tx_accumulator(0),
        ty_accumulator(0),
    	first_sweep(true),
    	zoom_factor(1),
    	zoom_ox(0),
    	zoom_oy(0),
    	zoom_px(0),
    	zoom_py(0),
    	old_update(0)

    {
        buffer[0]=0;
        buffer[1]=0;

        rtlog_time_measure_reset(&sc_time);
        rtlog_time_measure_register(&sc_time, "SCTIME", 0);

        mex_mt_object_t* obj=new mex_mt_object_t;
        mex_ext_rtlog_register_time_measure(obj, &sc_time);

        rtlog_time_measure_reset(&sw_time);
        rtlog_time_measure_register(&sw_time, "SCSW", 0);

        obj=new mex_mt_object_t;
        mex_ext_rtlog_register_time_measure(obj, &sw_time);


    	slist.interpolation_active=true;
    	ang_interpolation=slist.interpolation_active;
    	range_interpolation=slist.interpolation_active;

#ifdef __MINGW32__
        old_update=false;
        range_interpolation=true;
        ang_interpolation=true;
        slist.interpolation_active=true;
#endif
    }

    void clear()
    {
    	clear(0);
    	clear(1);
    	current_index=0;
    	first_sweep=true;
    	slist.clear();
    	tx_accumulator=0;
    	ty_accumulator=0;
    	hole=false;
    }

    void clear(int index)
    {
        memset(buffer[index], 0, size);
    }

    void initialize(int phy, int dx, int x0, int y0)
    {
    	stride=SC_STRIDE;
        //stride=stride_<dx ? dx : stride_;

        if (dimension<dx)
        {
            if (buffer[0])
                delete[] buffer[0];
            if (buffer[1])
                delete[] buffer[1];

            buffer[0]=new uint8_t[phy*stride];
            buffer[1]=new uint8_t[phy*stride];
        }
        dimension=phy; //dx;
        size=phy*stride;

        cx=x0;
        cy=y0;

        clear(0);
        clear(1);


        //qDebug()<<"buffers:"<<size<<", table:"<<sizeof(cv_table_t)<<", store:"<<sizeof(slist.sweeps)<<", ass:"<<sizeof(slist.association_store);
        slist.setPixel(dx);
        precalculate(dx, x0, y0);
        //setScale(MAX_SAMPLES);

        zoom_ox=0; //x0;
        zoom_oy=0; //y0;

    	tx_accumulator=0;
    	ty_accumulator=0;
    	hole=false;
    }
#if 0
    void setScale(int rbins)
    {
       rng_scale=float(rbins)/slist.max_pixel; //float(MAX_PIXEL);
    }
#endif

    void update_new(float vx_nav, float vy_nav, float delta_t, bool hole);

    void update(float vx_nav, float vy_nav, float delta_t, bool hole)
    {
    	//if (!old_update)
    	{
    		update_new(vx_nav, vy_nav, delta_t, hole);
    		return;
    	}
    }
    static void __monitor_invokable_swinterp(void) __attribute__((used));
};

//sc_context_t scx;
sc_context_t autoMonitorable_sccx;
#define scx autoMonitorable_sccx

void sc_context_t::__monitor_invokable_swinterp(void)
{
	slist.interpolation_active=!slist.interpolation_active;
	scx.ang_interpolation=slist.interpolation_active;
	scx.range_interpolation=slist.interpolation_active;
	if (slist.interpolation_active)
		mex_printf("Interpolation ACTVIE\n");
	else
		mex_printf("Interpolation NOT ACTVIE\n");
}

struct frame_buffer_t
{
    uint8_t* m;
    int dxy;
    int stride;

    uint8_t* row(int r) { return &m[stride*r];}
};

SoftScanConverter::SoftScanConverter()
{
}

void SoftScanConverter::initialize(int phy_y, int dxy, int x0, int y0, int /*stride*/)
{
    scx.initialize(phy_y, dxy, x0, y0);
    slist.setPixel(dxy);
    //precalculate();
}

void SoftScanConverter::addSweep(float angle, float* data, int samples, int direction, float f_center, float scan_center, float left_limit, float right_limit)
{
    rtlog_time_measure_start(&scx.sw_time);

#if 0
    int sindex=angle/SWEEP_RESOLUTION;
    if ((sindex<0) ||(sindex>=SWEEP_BUFFER_LEN))
        return;

    for(int i=0; i<MAX_SAMPLES; ++i)
    {
        sweep_buffer.spweep[sindex].samples[i]=data[i];
    }
#endif

    if (scMutex.acquire())
    {
		f_center=angle_bound_pi(f_center);
		angle=angle_bound_pi(angle+f_center);
		left_limit=angle_bound_pi(left_limit);
		right_limit=angle_bound_pi(right_limit);

		//scMutex.acquire();
		bool hole_detected=slist.add(angle, data, samples);
		scx.hole=scx.hole || hole_detected;

		//scx.current_angle=angle;
		scx.direction=direction;
		scx.reference=f_center;

		scx.scan_center=scan_center;
	#if 0
		left_limit=angle_bound_pi(left_limit+scan_center);
		right_limit=angle_bound_pi(right_limit+scan_center);
		if (left_limit>right_limit)
			sc_swap(left_limit, right_limit);
	#endif
		scx.left_limit=left_limit;
		scx.right_limit=right_limit;

		scMutex.release();
    }
    else
    	rtlog_const_message("SC", rtlog_warning|rtlog_breaking_news, "addSweep(): mutex timed out");
    rtlog_time_measure_stop(&scx.sw_time);
}

const void* SoftScanConverter::update(float delta_t, float vx, float vy)
{
	if (delta_t<=0)
	{
		scx.tx_accumulator=0;
		scx.ty_accumulator=0;
		scx.first_sweep=true;
		return 0;
	}

	if (scMutex.acquire())
	{
		scx.current_angle=slist.last_angle;
		bool hole=scx.hole;
		scx.hole=false;
		scMutex.release();
		scx.update(vx, vy, delta_t, hole);
	}
	else
		rtlog_const_message("SC", rtlog_warning|rtlog_breaking_news, "update(): mutex timed out");

    return current_buffer();
}

void SoftScanConverter::block(unsigned int /*timeout*/)
{
	scMutex.acquire();
}

void SoftScanConverter::unblock()
{
	scMutex.release();
}

void* SoftScanConverter::current_buffer()
{
    return scx.buffer[scx.current_index];
}

int SoftScanConverter::buffer_stride()
{
	return scx.stride;
}

void* SoftScanConverter::next_buffer()
{
	int next_index=scx.current_index ^ 1;
    return scx.buffer[next_index];
}

void SoftScanConverter::swap_buffers()
{
	scx.current_index=scx.current_index ^ 1;
}


void SoftScanConverter::setZoom(int ox, int oy, float factor)
{
	scx.zoom_factor=factor<1 ? 1 : factor;
	scx.zoom_ox=ox;
	scx.zoom_oy=oy;
}

void SoftScanConverter::setAngularInterpolation(bool enable)
{
    slist.interpolation_active=enable;//scx.angular_interpolation=enable;
}

void SoftScanConverter::setRangeInterpolation(bool enable)
{
    scx.range_interpolation=enable;
}

void SoftScanConverter::clear()
{
	scx.clear();

	//scx.clear(0);
	//scx.clear(1);
}



//Simulation

void SoftScanConverter::simulateSweep(float azimuth, float *data, int samples, int direction, float platform_azimuth, float map_reference_azimuth, float left_limit, float right_limit)
{
	const int max_sim_smaples=3*1024; //MAX_SAMPLES
	static float fdata[3*1024];
	const int rbins=samples>max_sim_smaples ? max_sim_smaples : samples;

	if (data)
	{
		memcpy(fdata, data, rbins*sizeof(fdata[0]));
	}
	else
	{
		int deg_angle=gc_math::rad_to_deg(azimuth);
		if (deg_angle<0)
			deg_angle=-deg_angle;
		deg_angle=deg_angle/5;
		int poffset=(deg_angle & 1) ? 16 : 0;
		for(int i=0; i<rbins; ++i)
		{
			static const int pattern[16]={0, 0, 64, 64, 70, 70, 80, 100, 100, 80, 70, 70, 64, 64, 0, 0};
			//int v=128+((i+poffset) & 0x7F);
			int v=128+pattern[i & 0x0F]+poffset;
			if (v>255)
				v=255;
			fdata[i]=v;
			//int r=rbins/32;
			//if (r & 1)
			//	fdata[i]=200;
		}
	}
	addSweep(azimuth, fdata, rbins, direction, platform_azimuth, map_reference_azimuth, left_limit, right_limit);
}



//Expermental optimized update
#define SC_FIX_SC_HOLE 0

void sc_context_t::update_new(float vx_nav, float vy_nav, float delta_t, bool hole)
{
    static int clear_extra=0;
    static int clear_pipe=0;
    static bool zoom_reset;
    static float last_scale=-1;

    float angle_rot=0;

    if (first_sweep)
    {
    	last_angle=current_angle;
    	first_sweep=0;
    	return;
    }

    rtlog_time_measure_start(&sc_time);

    reference=angle_bound_pi(reference);

   	//float vc=cosf(-reference);
   	//float vs=sinf(-reference);

    //vx_nav=vx_nav*delta_t;
    //vy_nav=vy_nav*delta_t;

   	float vmod=sqrtf(vy_nav*vy_nav+vx_nav*vx_nav);
   	float gta=atan2f(vy_nav, vx_nav);
   	float dgta=angle_bound_pi(gta+reference);

   	float vc=cosf(dgta);
   	float vs=sinf(dgta);

    float pix_scale=slist.scale;

   	float vx_=vmod*vs;
   	float vy_=-vmod*vc;
   	//vx_=vx_*(1/(pix_scale));//*75));
    //vy_=vy_*(1/(pix_scale));//*75));


    float vxp=-vx_*delta_t;
    float vyp=vy_*delta_t;

    tx_accumulator+=vxp;
    ty_accumulator+=vyp;

    int vx=0;
    int vy=0;

    if (fabsf(tx_accumulator)>1)
    {
        vx=tx_accumulator;
        tx_accumulator-=vx;
    }
    if (fabsf(ty_accumulator)>1)
    {
        vy=ty_accumulator;
        ty_accumulator-=vy;
    }

    //Too fast: if too big velocity enter from 1553, accumulators could go to INF
    if ((vx>1024) || (vx<-1024) || (vy>1024) || (vy<-1024))
    {
    	vx=0;
    	vy=0;
    	tx_accumulator=0;
    	ty_accumulator=0;
    }

    if (direction!=inversion)
    {
        angle_rot=current_reference-reference;
        slist.reinit(current_reference);
        if (direction)
        {
        	clear_pipe=1;
            if (direction>0)
               clear_extra=1;
            else
                clear_extra=-1;
        }
        inversion=direction;
        current_reference=reference;
    }
    else
    {
    	if (clear_pipe>0)
    	{
    		--clear_pipe;
    	}
    	else
    		clear_extra=0;
    }

    float r_limit=angle_bound_pi(right_limit+current_reference);
    float l_limit=angle_bound_pi(left_limit+current_reference);

    //Rotation center
    //const int x0=cx;
    //const int y0=cy;

    int dindex=current_index ^ 1;

    const uint8_t* row_in =buffer[current_index];

    cv_tab_real_angle_t hole_start=100000;
    cv_tab_real_angle_t hole_end=-100000;

	if (hole)
	{
		rtlog_printf("SC", rtlog_info|rtlog_breaking_news, "SC Hole: %f->%f", last_angle, current_angle);
		float h_start=last_angle;
		float h_stop=direction<0 ? r_limit : l_limit;
		if (h_stop<h_start)
		{
			float tmp=h_start;
			h_start=h_stop;
			h_stop=tmp;
		}
		hole_start=cv_tab_toang(h_start);
		hole_end=cv_tab_toang(h_stop);
		last_angle=current_angle;
	}


    int istart=a2si(last_angle); //last_angle/SWEEP_RESOLUTION;
    int istop =a2si(current_angle); //current_angle/SWEEP_RESOLUTION;
    if (istop<istart)
    {
    	//sc_swap(istart, istop);
        int tmp=istart;
        istart=istop;
        istop=tmp;
    }

    cv_table_t& t=cvt;

    bool rotation=angle_rot!=0;

    float cosangle = 1;
    float sinangle = 0;

    if (rotation)
    {
      cosangle = cosf(angle_rot);
      sinangle = sinf(angle_rot);
    }

    //uint8_t* row_out=&buffer[dindex][0];
    uint8_t* row_out1=&buffer[current_index][0];

    //const cv_pixel_t* pb=&t.tab[0][0];

    rtlog_printf("SC", rtlog_trace, "SW*: %f (%d) -> %f (%d)", current_angle, istop, last_angle, istart);

    int num_samples=slist.num_samples-1;//TODO: remove -1. So far used for the range interpolation

    //float zf=1/zoom_factor;
    int zf=zoom_factor>=1 ? zoom_factor : 1;

    int zox=0;
    int zoy=0;

    int half_dimension=dimension>>1;
	int z_dimension=half_dimension/zf;

	int crs_ox=zoom_ox;
	int crs_oy=zoom_oy;

    if (zoom_factor!=1)
    {
    	int z_limit=dimension-z_dimension;

    	if (crs_ox<z_dimension)
    		crs_ox=z_dimension;
    	else if (crs_ox>z_limit)
    		crs_ox=z_limit;
    	if (crs_oy<z_dimension)
    		crs_oy=z_dimension;
    	else if (crs_oy>z_limit)
    		crs_oy=z_limit;

    	if (zoom_reset)
    	{
    		vx=0;
    		vy=0;
    	}
    	else
    	{
    		vx=(crs_ox-zoom_px)*zf; //0;
    		vy=(crs_oy-zoom_py)*zf; //0;
    	}
    	rotation=false;
        zox=crs_ox-(z_dimension); ///zf;
        zoy=crs_oy-(z_dimension); ///zf;
        zoom_px=crs_ox;
        zoom_py=crs_oy;
        zoom_reset=false;
    }
    else
    {
    	crs_ox=dimension;
    	crs_oy=0;
    	zoom_reset=true;
    }

    int z_shift=0;
    switch(zf)
    {
    case 2: z_shift=1; break;
    case 4: z_shift=2; break;
    case 8: z_shift=3; break;
    default:
    	break;
    }


    {
#if 0
#define SC_DX	484
#define SC_DY	484
#define SC_CX	241
#define SC_CY	483
#define SC_STRIDE 512
#define SC_UVCLIP	~(512-1)
#else
    	const int SC_DX=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION; ////#define SC_DX	gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION
#define SC_DY	SC_DX
    	const int SC_CX=((SC_DX/2)-1); //#define SC_CX	((SC_DX/2)-1) //241
#define SC_CY	(SC_DY-1) //483
//#define SC_STRIDE MAX_PIXEL
//#define SC_UVCLIP	~((SC_STRIDE)-1)
#endif
//#define SC_SRCSHIFT  9
#define SC_FX_SHIFT	14
#define SC_FX_LSB 16384

		//int srcW=SC_DX;
		//int srcH=SC_DY;
		//int srcDelta=SC_STRIDE;
		float fDstCX=SC_CX;
		float fDstCY=SC_CY;
		float fSrcCX=SC_CX+vx;
		float fSrcCY=SC_CY+vy;

		float fScale=1; //Take in account the range scale change?
		if (last_scale!=slist.scale)
		{
			if (last_scale>0)
			{
				//TODO: verify resize winslist.scale=0!!
				fScale=last_scale/slist.scale;
				rtlog_printf("SC", rtlog_info|rtlog_breaking_news, "Scale Changed: %f -> %f (%f)", last_scale, slist.scale, fScale);
			}
			last_scale=slist.scale;
		}
		//float fAngle=rotation_angle;
		//fAngle+=0.005;
		//if (fAngle>3.14/2)
		//    fAngle=0;
		//srcDelta /= sizeof(WORD);
		//dstDelta /= sizeof(WORD);

		float duCol = sinangle * (1.0f / fScale);
		float dvCol = cosangle * (1.0f / fScale);
		float duRow = dvCol;
		float dvRow = -duCol;
		//float dvRowStride=dvRow*SC_STRIDE;
		float startingu = fSrcCX - (fDstCX * dvCol + fDstCY * duCol);
		float startingv = fSrcCY - (fDstCX * dvRow + fDstCY * duRow);

		float rowu = startingu-duRow;
		float rowv = startingv-dvRow;

		int fx_rowu=(rowu*SC_FX_LSB)+0.5;
		int fx_rowv=(rowv*SC_FX_LSB)+0.5;
		int fx_duRow=(duRow*SC_FX_LSB)+0.5;
		int fx_dvRow=(dvRow*SC_FX_LSB)+0.5;
		int fx_duCol =(duCol*SC_FX_LSB)+0.5;
		int fx_dvCol =(dvCol*SC_FX_LSB)+0.5;

		const uint8_t* __restrict pSrcBase=row_in;
		uint8_t* __restrict row=&buffer[dindex][0];

		const cv_pixel_t* tt=&t.tab[zoy][zox];

		int fx_num_samples=cv_tab_torng(float(num_samples-1)/pix_scale);
		int fx_pix_scale=cv_tab_torng(pix_scale);

		cv_tab_real_angle_t fx_l_limit=cv_tab_toang(l_limit);
		cv_tab_real_angle_t fx_r_limit=cv_tab_toang(r_limit);

		bool interpolate=slist.interpolation_active && ang_interpolation && range_interpolation;

		const float rng_fix_scale=1.0f/(1<<16);

		for(int y=0; y<SC_DY; ++y)
		{
			int fx_u=fx_rowu; //float u = rowu;
			int fx_v=fx_rowv; //float v = rowv;


	    	int zy=y>>z_shift;
#if 0
	    	zy+=zoy;
	    	const cv_pixel_t* pb=&t.tab[zy][0];
#endif
	    	const cv_pixel_t* pb=&tt[zy*CV_TAB_DX];

			//const int zy=(y>>z_shift)+zoy;

			//int ymt=y+vy;
			//const uint8_t* t_row_in =&row_in[ymt*stride];

			//const uint8_t* __restrict pSrc=pSrcBase[int(u)];

			for(int x=0; x<SC_DX; ++x)
			{
				//const int zx=(x>>z_shift)+zox;

				fx_u+=fx_duRow; //u += duRow;
				fx_v+=fx_dvRow; //v += dvRow;

				//if (false)
				{
		        	int zx=x>>z_shift; //float(zf);
#if 0
		        	zx+=zox;

		        	bool outside=((zy<0) || (zy>=dimension) || (zx<0) || (zx>=dimension));
		        	if (outside)
		        		continue;

		        	const cv_pixel_t& p=pb[zx];
#endif
		        	bool inside=!((zy<0) || (zy>=SC_DY) || (zx<0) || (zx>=SC_DX));


		        	if (inside)
		        	{
		        		const cv_pixel_t& p=pb[zx];
		        		auto r_angle=p.real_angle; //angle_bound_pi(p.real_angle-current_reference);

		        		//const cv_pixel_t& p=pb[zx];
		        		//auto r_angle=p.real_angle; //angle_bound_pi(p.real_angle-current_reference);

		        		if (r_angle<fx_l_limit)
		        		{
							if (clear_extra>0)
							{
								row[x]=0;
								row_out1[x]=0;
							}
							//skip=true;
							continue;
						}
						else if (r_angle>fx_r_limit)
						{
							if (clear_extra<0)
							{
								row[x]=0;
								row_out1[x]=0;
							}
							//skip=true;
							continue;
						}

						//float rng=p.rng*pix_scale;
						//int rng_index=rng; //s->scale; //rng_scale;
						//if (rng_index>=num_samples) //s->num_samples)
						//	continue;
		        		if (p.rng>=fx_num_samples)
		        		{
							row[x]=0;
		        			continue;
		        		}

						if ((p.angle_index>=istart) && (p.angle_index<=istop))
						{
#define SC_RANGE_INTERPOLATION
#ifdef SC_RANGE_INTERPOLATION
							if (interpolate)
							{
								int fx_rng=p.rng*fx_pix_scale;
								int rng_index=FX_RNG2(fx_rng);
								float rng=float(fx_rng)*rng_fix_scale; //-ARM does not have div instruction /(1<<16); //256.0f;
								sweep_list::sweep2_t sk=slist.lookup2(p.angle_index, direction);

#if 0
								//float32x2_t x0=vld1_f32(&sk.s[0]->samples[rng_index]);
								//float32x2_t x1=vld1_f32(&sk.s[1]->samples[rng_index]);
								float v=optimized_interpolate(&sk.s[0]->samples[rng_index], &sk.s[1]->samples[rng_index], sk.af, (rng-float(rng_index));


#else
								float x00=sk.s[0]->samples[rng_index];
								float x10=sk.s[1]->samples[rng_index];

								//const float f0=sk.af;
								//const float f0_1=1-f0;
								const float f0=fabsf(sk.af);
								const float f0_1=1-f0;

#if 0
								if (!range_interpolation)
								{
									float v=x00*(1-f0)+x10*(f0);
									row[x]=v;
								}
								else
#endif
								{
									float x01=sk.s[0]->samples[rng_index+1];
									float x11=sk.s[1]->samples[rng_index+1];

									const float r0=(rng-float(rng_index));
									float r0_1=1-r0;
									//float v=x00*(1-f0)*(1-r0)+x10*f0*(1-r0)+x01*(1-f0)*(r0)+x11*(f0)*r0;

									//float v=x00*f0_1*r0+x10*f0*r0_1+x01*f0_1*r0_1+x11*f0*r0;
									float x0y1=x00*f0_1+x10*f0;
									float x0y2=x01*f0_1+x11*f0;

									float v=x0y1*r0_1+x0y2*r0;
#endif
									//if (v>255) v=255;
									row[x]=v;
								}
							}
							else
#endif
							{
								int fx_rng=p.rng*fx_pix_scale;
				        		int rng_index=FX_RNG2(fx_rng);
								sweep_t* s=slist.lookup(p.angle_index); //slist.lookup(nav_angle); //p.real_angle);
								sweep_t::sample_type_t v=s->samples[rng_index]; //sweep_buffer.spweep[p.angle_index].samples[p.rng_index];
								row[x]=v;
							}
							//row[x]=v;
							continue;
						}
#if SC_FIX_SC_HOLE
						else if ((r_angle>hole_start) && (r_angle<hole_end))
						{
							row[x]=0;
							row_out1[x]=0;
							continue;
						}
#endif
		        	}
				}

				//row[x]=0;

				{//translation and rotation of previous buffer
					int ux=fx_u>>SC_FX_SHIFT; //int ux=u;
					int vy=fx_v>>SC_FX_SHIFT; //int vy=v;

					bool outside_src=((ux<0) || (ux>=SC_DX) || (vy<0) || (vy>SC_DY));
					//bool outside_src=((ux & SC_UVCLIP) || (vy & SC_UVCLIP));

					//if (outside_src)
					//	row[x]=0;
					//else
					if (!outside_src)
					{
						//uint8_t pixel=pSrcBase[int(v*srcDelta+u)];
						//uint8_t pixel=pSrcBase[vy*srcDelta+ux];
#if 1
						uint8_t pixel=pSrcBase[(vy<<SC_SRCSHIFT)+ux];
						row[x]=pixel;
#else
						row[x]=pSrcBase[y*SC_STRIDE+x];
#endif
					}
					else
						row[x]=0;
				}
				//u += duRow;
				//v += dvRow;
			}


			fx_rowu += fx_duCol; //rowu += duCol;
			fx_rowv += fx_dvCol; //rowv += dvCol;

			row+=SC_STRIDE; //simple add, supposed faster then dst->d[y] (could required a add + mul)
			row_out1+=SC_STRIDE;
		}
    }

    last_angle=current_angle;
    current_index=dindex;

    rtlog_time_measure_stop(&sc_time);
}
