/*
 * SoftScanConverter.cpp
 *
 *  Created on: 22/feb/2018
 *      Author: chessaa
 */

#include "SoftScanConverterDriver.h"

#include "SoftScanConverter.h"

#include "image_resampling.h"
#include "image_serializer.h"

#include "gc_math.h"
#include "gc_geometry.h"

#include "rtlog.h"

#include "mrk.h"

#include "gfd.h"
#include "gfd_grifo_fonts.h"

#include "gc_color_theme.h"

#include "mex_hobbit_register.h"

#include <string.h>

#include "rtg_high_resolution_timer.h"

#include "GcEnvAdaptor.h"
#include "GcDisplayExtraInfo.h"

#define MAX_SAMPLES	4096

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

#define SAR_MAX_DIM	2048
#define SAR_16TO8
struct sar_image_buffer_t
{
	bool sar_mode;

	float heading;
	float lat;
	float lon;
	float resolutions;

	float zoom;
	int zx;
	int zy;

	unsigned int bpp;
	int dx;
	int dy;
	int stride;

	uint8_t* raw_img;

	union
	{
		uint8_t image[512][512];
#ifdef SAR_16TO8
		uint8_t hr_image[2048*2048];
#endif
	} store;
};

static sar_image_buffer_t sar_image ATTRIBUTE_NOLOAD;


class SoftScanConverterDriver::ScImplementation
{
public:
	enum {wf_max=4, k_msg_max_len=64};

	struct wf_t
	{
		enum {max_samples=2048};
		bool visible;
		unsigned int n_samples;
		char samples[max_samples];
	};
	wf_t wf[wf_max];

	enum
	{
		stack_size=4096,
		thread_affinity_mask=2
	};
	volatile bool enabled;
	int fb_pitch;
	int fby;

	volatile float vx;
	volatile float vy;

	volatile bool image_mode;
	volatile bool dbs_mode;

	int scan_volume_source;
	float scan_limit_min;
	float scan_limit_max;

	int dx;
	int dy;

	volatile bool update_completed;

	gfd_framebuffer_t fb;

	bool dbg_wf_show;
	bool dbg_show;
    bool dbg_msg_show;

    int ts_y;
    int ts_m;
    int ts_d;
    int ts_s;
    unsigned int ts_tt;

    unsigned int tt;
	char msg[4][k_msg_max_len];

	float sar_rot_simulate;

	ScImplementation():
		enabled(false),
		fb_pitch(0),
		fby(0),
		vx(0),
		vy(0),
		image_mode(false),
		dbs_mode(false),
		scan_volume_source(0)
	{
		update_completed=true;
		sar_rot_simulate=0;
		gfd_fb_init(&fb, 512, 512, 512, &sar_image.store.image[0][0]);

		msgClear();

		dbg_wf_show=false;
		dbg_show=false;
		dbg_msg_show=false;
		thread_start();

		//GcEnvAdaptor::ggSetPostOperativeHandler(postOperativeDraw, this);
		GcDisplayExtraInfo::engInstance().addVideoFrameHandler(postOperativeDraw, this);
	}

	typedef void (*handler_t)(GcDisplayExtraInfo& d, GcDisplayExtraInfo& ed, void* cookie);

	static int postOperativeDraw(GcDisplayExtraInfo& /*d*/, GcDisplayExtraInfo& ed, void* cookie) //void* cookie, const uint8_t* src, unsigned int rows, unsigned int cols, unsigned int pitch)
	{
		gfd_framebuffer_t* sfb=ed.frameBuffer();

		ScImplementation* obj=reinterpret_cast<ScImplementation*>(cookie);
		if (!cookie || !sfb)
			return 0;

		if (obj->dbg_show)
		{//Overload Oscilloscope and messages
			//uint8_t* p=(uint8_t*)sfb->buffer;
			//gfd_fb_init(&sfb, cols, rows, pitch, (unsigned char*)src);
			//gfd_fb_init(&sfb, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION, 512, pitch, &p[pitch*gc_cfg::Y_LOGICAL_DISPLAY_OFFSET+gc_cfg::X_LOGICAL_DISPLAY_OFFSET]);

			if (obj->dbg_msg_show)
			{
				int ox=50+8*8;
				int oy=38;
				if (gc_cfg::X_LOGICAL_DISPLAY_OFFSET<16)
				{
					ox=2;
					oy=100;
				}
				for(int n_msg=0; n_msg<4; ++n_msg)
				{
					if (obj->msg[n_msg][0])
						gfd_fb_printf_cs(sfb, &gfd_font256_PC_8x8, ox, oy+n_msg*10, gc_color_theme::gc_color_dbg/*64*/, obj->msg[n_msg]);
				}
			}

			if (obj->dbg_wf_show)
			{
				for(int i=0; i<ScImplementation::wf_max; ++i)
				{
					ScImplementation::wf_t& w=obj->wf[i];
					if (w.visible)
						obj->oscilloscopeShowWf((uint8_t*)sfb->buffer, i, w.n_samples, w.samples, sfb->dx, sfb->dy, sfb->pitch, gc_color_theme::gc_color_red);
				}
			}
			//obj->oscilloscopeShowWf((uint8_t*)sfb.buffer, 0, 100, 0, pitch, gc_color_theme::gc_color_red);
		}
		return 0;
	}

#if 0
	static void postOperativeDraw(void* cookie, const uint8_t* src, unsigned int rows, unsigned int cols, unsigned int pitch)
	{
		(void)rows;
		(void)cols;

		ScImplementation* obj=reinterpret_cast<ScImplementation*>(cookie);
		if (!cookie || !src)
			return;

		if (obj->dbg_show)
		{//Overload Oscilloscope and messages
			gfd_framebuffer_t sfb;
			uint8_t* p=(uint8_t*)src;
			//gfd_fb_init(&sfb, cols, rows, pitch, (unsigned char*)src);
			gfd_fb_init(&sfb, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION, 512, pitch, &p[pitch*gc_cfg::Y_LOGICAL_DISPLAY_OFFSET+gc_cfg::X_LOGICAL_DISPLAY_OFFSET]);

			if (obj->dbg_msg_show)
			{
				for(int n_msg=0; n_msg<4; ++n_msg)
				{
					if (obj->msg[n_msg][0])
						gfd_fb_printf_cs(&sfb, &gfd_font256_PC_8x8, 50+8*8, 38+n_msg*10, gc_color_theme::gc_color_dbg/*64*/, obj->msg[n_msg]);
				}
			}

			if (obj->dbg_wf_show)
			{
				for(int i=0; i<ScImplementation::wf_max; ++i)
				{
					ScImplementation::wf_t& w=obj->wf[i];
					if (w.visible)
						obj->oscilloscopeShowWf((uint8_t*)sfb.buffer, i, w.n_samples, w.samples, pitch, gc_color_theme::gc_color_red);
				}
			}
			//obj->oscilloscopeShowWf((uint8_t*)sfb.buffer, 0, 100, 0, pitch, gc_color_theme::gc_color_red);
		}


	}
#endif

	void msgClear()
	{
		msg[0][0]=0;
		msg[1][0]=0;
		msg[2][0]=0;
		msg[3][0]=0;
	}

	mrk_thread_t thread;
	mrk_event_t evn;
	uint64_t stack[stack_size];

	bool setImage(unsigned int bpp, int ox, int oy, int dx, int dy, int stride, int rows, const void* p, float heading=0, float lat=0, float lon=0, float resolutions=0)
	{
#if 0
		sar_image.bpp=bpp;
		sar_image.dx=dx;
		sar_image.dy=dy;
		sar_image.stride=stride;
		sar_image.raw_img=(uint8_t*)p;
#endif
		sar_image.sar_mode=dx>512;
		sar_image.heading=heading;
		sar_image.lon=lon;
		sar_image.lat=lat;
		sar_image.resolutions=resolutions;

		if (sar_image.sar_mode)
		{
			GrifoImageSerializer::rectifiedImageCommit(bpp, (uint8_t*)p /*sar_image.raw_img*/, stride, dy, heading, lat, lon, resolutions);
#ifdef SAR_16TO8
			//image_resampling(sar_image.store.hr_image, SAR_MAX_DIM, SAR_MAX_DIM, SAR_MAX_DIM, sar_image.dx/2, sar_image.dy/2, 0, 1, bpp, (image_resampling_src_type_t*)p, sar_image.dx, sar_image.dy, sar_image.stride);

			image_simple_resampling_16to8(
					sar_image.store.hr_image, SAR_MAX_DIM, SAR_MAX_DIM, SAR_MAX_DIM,
					(image_resampling_src_type_t*)p, dx, dy, stride);

			sar_image.bpp=1;
			sar_image.dx=SAR_MAX_DIM;
			sar_image.dy=SAR_MAX_DIM;
			sar_image.stride=SAR_MAX_DIM;
			sar_image.raw_img=sar_image.store.hr_image;

			//GrifoImageSerializer::rectifiedImageCommit(1, sar_image.raw_img, stride, dy, heading, lat, lon, resolutions);
#else
			sar_image.bpp=bpp;
			sar_image.dx=dx;
			sar_image.dy=dy;
			sar_image.stride=stride;
			sar_image.raw_img=(uint8_t*)p;
#endif
			return true;
		}

		sar_image.bpp=bpp;
		sar_image.dx=dx;
		sar_image.dy=dy;
		sar_image.stride=stride;
		sar_image.raw_img=(uint8_t*)p;

		int ey=oy+rows;
#if 1
		const uint8_t* src=reinterpret_cast<const uint8_t*>(p);
		int R=rows;
		const int IDX=sizeof sar_image.store.image[0];
		if (ey>IDX)
			R=rows-(ey-IDX);
		int NX=dx>IDX ? IDX : dx;

		for(int r=0; r<R; ++r)
		{
			memcpy(&sar_image.store.image[oy+r][ox], &src[stride*r], NX);
		}
		return true;

#else
		if ((ey<=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION) && (rows<=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION) && (stride<=512))
		{
			int ex=ox+dx;
			if (ex<512)
			{
				const uint8_t* src=reinterpret_cast<const uint8_t*>(p);
				for(int r=0; r<rows; ++r)
				{
					memcpy(&sar_image.image[oy+r][ox], &src[stride*r], dx);
				}
				return true;
			}
		}
		return false;
#endif
	}
    void clearImage()
    {
		memset(&sar_image.store.image[0][0], 0, sizeof sar_image.store.image);
		//msgClear();
    }

	void oscilloscopeSetWaveformNumber(unsigned int /*n*/)
	{
	}

	void oscilloscopeShowWaveform(unsigned int wfn, unsigned int samples, char* udata)
	{
		if (wfn<wf_max)
		{
			wf_t& w=wf[wfn];
			w.visible=samples>0;
			w.n_samples=samples<=wf_t::max_samples ? samples : static_cast<unsigned int>(wf_t::max_samples);
			memcpy(w.samples, udata, w.n_samples);
		}
	}

	void oscilloscopeShowWf(uint8_t* fb, unsigned int wfn, unsigned int samples, char* udata, unsigned int dx, unsigned int dy, unsigned int pitch, unsigned char color=255)
	{
		if (samples<1)
			return;

		auto pixelset = [](uint8_t& p, uint8_t v)
		{
			if ((p==0) || (p>31))
				p=v;
		};

		//unsigned int stride;
		const int start_offset=76;
		const int wf_num=wf_max;

		unsigned char yellow=gc_color_theme::gc_color_dbg;

		const signed char* data=reinterpret_cast<signed char*>(udata);

		if (udata==0)  //test
		{
			for(int y=0; y<(unsigned int)dy; ++y)
			{
				for(int x=0; x<(unsigned int)dx; ++x)
				{
					pixelset(fb[y*pitch+x], color);
				}
			}
			return;
		}
		const unsigned int x_dim=dx; //(unsigned int)gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;
		const unsigned int y_dim=dy; //(unsigned int)gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;

		unsigned int wf_space=(y_dim-start_offset)/wf_num;
		float base=float(wf_space)/2.0f;
		float f_width=base; //(base/2)-1;
		float scale_y=128.0/(f_width-2); //(base/2);
		float scale_x=float(x_dim)/float(samples);
		//if (wfn==0)
		//	memset(&sar_image.image[0][0], 0, sizeof sar_image.image);
		int py=0;
		base=start_offset+base+(wfn*wf_space);
		//if (wfn>0)
		//	base+=4;
		base+=wfn*2;

		uint8_t* fb_zero=&fb[pitch*int(base)];

		int pix_width=int(f_width);

		float scale_x_inv=1/scale_x;
		float scale_y_inv=1/scale_y;

		for(unsigned int i=0; i<x_dim; ++i)
		{
			unsigned index=i*scale_x_inv; //i/scale_x;
			int v=data[index];
			float f=v*scale_y_inv; //v/scale_y;
			if (f>f_width)
				f=f_width;
			if (f<-f_width)
				f=-f_width;
			int y=f+base;


			uint8_t* fbi=&fb[i];

			pixelset(fbi[y*pitch/*+i*/], color); //sar_image.image[y][i]=255;
			if (i>0)
			{
				if (y>py)
				{
					++py;
					for(int j=py; j<y; ++j)
						pixelset(fbi[j*pitch/*+i*/], color); //sar_image.image[j][i]=255;
				}
				else
				{
					--py;
					for(int j=py; j>y; --j)
						pixelset(fbi[j*pitch/*+i*/], color); //sar_image.image[j][i]=255;
				}
			}
			if ((i & 0x10)==0)
			{
				fb_zero[pitch+i]=yellow;
			}
			if (i<10)
			{
				pixelset(fb_zero[pix_width*pitch+i], yellow);
				pixelset(fb_zero[(-pix_width)*pitch+i], yellow);
			}
			py=y;
		}
	}

	static void exec()
	{
	    intptr_t sc_arg=static_cast<intptr_t>(mrk_thread_get_arg());
        ScImplementation& sc=*reinterpret_cast<ScImplementation*>(sc_arg);	    //ScImplementation& sc=*reinterpret_cast<ScImplementation*>(mrk_thread_get_arg());

        mex_hobbit_register_instance("ssc", sc);

		for(;;)
		{
			sc.update_completed=true;
			mrk_event_wait(&sc.evn, mrk_wait_forever);

			if (!sc.image_mode)
			{
				sc.update(sc.enabled);
				continue;
			}

			//Image:
			if (false)
			{
				uint8_t* dst=reinterpret_cast<uint8_t*>(SoftScanConverter::next_buffer());

				for(int r=0; r<gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION; ++r)
				{
					uint8_t* p=sar_image.store.image[r];
					for(int c=0; c<gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION; ++c)
					{
						uint8_t v=p[c];
						v=gc_color_theme::sc_color_index_convert(v);//v=(v>>1)+128;
						dst[c]=v;
					}
					dst+=512;
				}
				SoftScanConverter::swap_buffers();
			}
		}
	}

	void update(bool ena)
	{
		rtlog_const_message("SC", rtlog_trace, "Conversion Begin");

		const float scale=1; //gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/float(80*1875);

		float dt=0.04;

#ifdef __MINGW32__
    	{
    		static rtg_hrt_ticks_t hrt_previous;
    		auto hrt=rtg_hrt_get();
    		rtg_hrt_ticks_t hrt_delta=hrt-hrt_previous;
    		rtg_hrt_microseconds_t ms_delta=rtg_hrt_to_microseconds_longlong(hrt_delta);
    		if (hrt_previous<hrt)
    			dt=double(ms_delta)*1.0E-6;
    		hrt_previous=hrt;
    	}
#endif

		if (ena)
			SoftScanConverter::update(dt, vx*scale, vy*scale);
		else
			SoftScanConverter::update(0, 0, 0);

		rtlog_const_message("SC", rtlog_trace, "Conversion End");
	}

	void go()
	{
		if (!update_completed)
		{
			rtlog_const_message("SC", rtlog_warning, "SC overload");
		}
		else
			mrk_event_pulse(&evn);
	}

	void thread_start()
	{
		mrk_event_create(&evn, "SC");

		//mrk_thread_create(&thread, "SC", exec, reinterpret_cast<mrk_opaque_t>(this), mrk_std_priority_below_normal, sizeof stack, &stack, 0);
		mrk_thread_create_with_affinity(&thread, "SC", exec, reinterpret_cast<mrk_opaque_t>(this), mrk_std_priority_low/*mrk_std_priority_below_normal*/, sizeof stack, &stack, thread_affinity_mask, 0);
	}
};

SoftScanConverterDriver::SoftScanConverterDriver(SoftScanConverterDriver::ScImplementation& imp):
		p_(imp)
{
	//SoftScanConverter::initialize(0, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION, 512);

	SoftScanConverter::initialize(gc_cfg::XY_MAX_LOGICAL_DISPLAY_SIZE/*XY_LOGICAL_DISPLAY_DIMENSION*/, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION-1, 512);
}

void SoftScanConverterDriver::resize(int dxy, int cx, int cy)
{
	SoftScanConverter::initialize(gc_cfg::XY_MAX_LOGICAL_DISPLAY_SIZE, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION-1, 512);
}

void SoftScanConverterDriver::block(unsigned int timeout)
{
	SoftScanConverter::block(timeout);
}

void SoftScanConverterDriver::unblock()
{
	SoftScanConverter::unblock();
}

void SoftScanConverterDriver::setZoom(int ox, int oy, float factor)
{
	sar_image.zoom=factor;
	sar_image.zx=ox;
	sar_image.zy=oy;

	SoftScanConverter::setZoom(ox, oy, factor);
}

void SoftScanConverterDriver::setInterpolation(bool enable)
{
	SoftScanConverter::setAngularInterpolation(enable);
}

void SoftScanConverterDriver::enable(bool ena)
{
  if (ena!=p_.enabled)
	  SoftScanConverter::clear();
  p_.enabled=ena;
}

void SoftScanConverterDriver::reset()
{
	  SoftScanConverter::clear();
	  p_.clearImage();
}

void SoftScanConverterDriver::setScanVolumeFromSweep(bool from_sweep)
{
	p_.scan_volume_source=from_sweep ? 0 : 1;
}

void SoftScanConverterDriver::setScanVolume(float scan_center, float scan_width)
{
	float tmp=scan_center-scan_width;
	p_.scan_limit_min=gc_math::angle_bound_pi(tmp);
	tmp=scan_center+scan_width;
	p_.scan_limit_max=gc_math::angle_bound_pi(tmp);

}

void SoftScanConverterDriver::setDbsMode(bool enable)
{
	p_.dbs_mode=enable;
}
void SoftScanConverterDriver::setNavigation(float range_scale, float vx, float vy)
{
	float scale=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/range_scale;
	p_.vx=vx*scale;
	p_.vy=vy*scale;
}

void SoftScanConverterDriver::addSweep(const SweepHeader& s)
{
  static float fdata[MAX_SAMPLES];
  const int rbins=s.rbins>MAX_SAMPLES ? MAX_SAMPLES : s.rbins;
  for(int i=0; i<rbins; ++i)
  {
	  uint8_t v=s.data[i];
	  //v=v>>1;
	  //v+=128;
	  v=gc_color_theme::sc_color_index_convert(v);//v=(v>>1)+128;
	  fdata[i]=v;
  }
  float limit_min=s.scan_limit_min; //gc_math::deg_to_rad(-60)-s.map_reference_azimuth; //s.scan_limit_min;
  float limit_max=s.scan_limit_max; //gc_math::deg_to_rad(60)-s.map_reference_azimuth; //s.scan_limit_min;

  if (p_.scan_volume_source)
  {
	  limit_max=p_.scan_limit_max;
	  limit_min=p_.scan_limit_min;
  }

  //p_.vx=s.vx;
  //p_.vy=s.vy;

  SoftScanConverter::addSweep(s.azimuth, fdata, rbins, s.direction, s.platform_azimuth, s.map_reference_azimuth, limit_min, limit_max);

  p_.image_mode=false;
  //p_.addSweep(s);
}


void SoftScanConverterDriver::simulateSweep(float angle, float* data, int samples, int direction, float f_center, float scan_center, float left_limit, float right_limit)
{
	  p_.image_mode=false;
	  if (p_.scan_volume_source)
	  {
		  right_limit=p_.scan_limit_max;
		  left_limit=p_.scan_limit_min;
	  }

	  SoftScanConverter::simulateSweep(angle, data, samples, direction, f_center, scan_center, left_limit, right_limit);
}


void SoftScanConverterDriver::setTimestamp(int y, int m, int d, int s, unsigned int tt)
{
	p_.ts_y=y;
	p_.ts_m=m;
	p_.ts_d=d;
	p_.ts_s=s;
	p_.ts_tt=tt;

}

void SoftScanConverterDriver::updateMessage(int n, const char* msg)
{
	if ((n<0) || (n>3))
		return;
	p_.msg[n][0]=0;

	if (!msg || !msg[0])
	{
		p_.msg[n][0]=0;
		return;
	}

	p_.msg[n][ScImplementation::k_msg_max_len-1]=0;
	memcpy(p_.msg[n], msg, (sizeof p_.msg[0]-1));

	//gfd_fb_printf(&p_.fb, &gfd_font256_PC_8x8, 50, 38+n*10, 255, msg);
}

void SoftScanConverterDriver::displayMessages(bool show)
{
	p_.dbg_msg_show=show;
}

void SoftScanConverterDriver::showWaveforms(unsigned int fwn, unsigned int samples, char* data)
{
	//p_.image_mode=true;
	p_.oscilloscopeSetWaveformNumber(2);
	p_.oscilloscopeShowWaveform(fwn, samples, data);
}

bool SoftScanConverterDriver::setImage(unsigned int bpp, int ox, int oy, int dx, int dy, int stride, int rows, const void* p, float heading, float lat, float lon, float resolutions)
{
	p_.image_mode=true;
	return p_.setImage(bpp, ox, oy, dx, dy, stride, rows, p, heading, lat, lon, resolutions);
}


void SoftScanConverterDriver::enableMapDbg(bool enable)
{
	p_.dbg_show=enable;
}

void SoftScanConverterDriver::displayWaveforms(bool show)
{
	p_.dbg_wf_show=show;
}

#include "DbsScanHelper.h"

//#define SC_FAST_UNALIGNED_COPY
#ifdef SC_FAST_UNALIGNED_COPY
//Well... this is slower then library memcpy...
static void display_unaligned_copy(uint8_t* dst, const uint8_t* src, unsigned int len) __attribute__((noinline));
static void display_unaligned_copy(uint8_t* dst, const uint8_t* src, unsigned int len)
{
	unsigned int len32=len>>2;
	uint16_t* d=reinterpret_cast<uint16_t*>(dst);
	const uint32_t* s=reinterpret_cast<const uint32_t*>(src);
	for(unsigned int i=0; i<len32; i+=1)
	{
		uint32_t x=s[i];
		d[0]=x & 0x0FFFF;
		d[1]=x>>16;
		d+=2;
#if 0
		x=s[i+1];
		d[2]=x & 0x0FFFF;
		d[3]=x>>16;
#endif
	}
}
#endif


void SoftScanConverterDriver::generateDebug(void* fb)
{
#if 0
	if (p_.dbg_show)
	{//Overload Oscilloscope and messages
		uint8_t* p=reinterpret_cast<uint8_t*>(fb);
		gfd_framebuffer_t sfb;
		gfd_fb_init(&sfb, 512, 512, p_.fb_pitch, &p[p_.fb_pitch*gc_cfg::Y_LOGICAL_DISPLAY_OFFSET+gc_cfg::X_LOGICAL_DISPLAY_OFFSET]);
#if 0
		if (p_.dbs_mode)
		{
			const float dx=(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2);

			DbsScanHelper& dbs=DbsScanHelper::instance();
			DbsScanHelper::dbs_scan_volume_t& v=dbs.scanVolume();
			float angle=(v.ground_track+v.platform_azimuth);
			float y=dx*cosf(angle);
			float x=dx*sinf(angle);

			gfd_fb_draw_line(&sfb,
					(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)-x, (gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION)-y,
					gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION,
					gc_color_theme::gc_color_dbg/*64*/);

			for(int i=0; i<v.fullVolume.bar_num; ++i)
			{
				const float dx=(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)+100;
				angle=v.b[i].hard_int_lim;
				float y=dx*cosf(angle);
				float x=dx*sinf(angle);
				gfd_fb_draw_line(&sfb,
						(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)-x, (gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION)-y,
						gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION,
						gc_color_theme::gc_color_dbg/*64*/);
			}
		}
#endif

		if (p_.dbg_msg_show)
		{
			for(int n_msg=0; n_msg<4; ++n_msg)
			{
				if (p_.msg[n_msg][0])
					gfd_fb_printf(&sfb, &gfd_font256_PC_8x8, 50, 38+n_msg*10, gc_color_theme::gc_color_dbg/*64*/, p_.msg[n_msg]);
			}
		}

		if (p_.dbg_wf_show)
		{
			for(int i=0; i<ScImplementation::wf_max; ++i)
			{
				ScImplementation::wf_t& w=p_.wf[i];
				if (w.visible)
					p_.oscilloscopeShowWf((uint8_t*)sfb.buffer, i, w.n_samples, w.samples, p_.fb_pitch, gc_color_theme::gc_color_red);
			}
		}
	}
#endif
}

void SoftScanConverterDriver::generate(void* fb)
{
	static bool clear_need=false;


	if (p_.enabled)
	{
		clear_need=true;
#if 0
		if (0)
		{
			rtlog_const_message("GC", rtlog_info, "SC CV Begin");

			const float scale=1; //gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/float(80*1875);

			const uint8_t* sc=reinterpret_cast<const uint8_t*>(SoftScanConverter::update(0.04, p_.vx*scale, p_.vy*scale)); //10, 10));

			rtlog_const_message("GC", rtlog_info, "SC CV End");
		}
#endif
		const uint8_t* sc=reinterpret_cast<const uint8_t*>(SoftScanConverter::current_buffer());
		int sc_stride=SoftScanConverter::buffer_stride();

		uint8_t* p=reinterpret_cast<uint8_t*>(fb);

		const int DXY=gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION;
		const int PY=gc_cfg::Y_PHYSICAL_DISPLAY_DIMENSION-DXY;
		const int x_offset=gc_cfg::X_LOGICAL_DISPLAY_OFFSET;
		const int y_offset=gc_cfg::Y_LOGICAL_DISPLAY_OFFSET;

		if (!p_.image_mode)
		{
			p_.go();

			//uint8_t* p=reinterpret_cast<uint8_t*>(fb);
			unsigned int fill_size_right=p_.fb_pitch-(x_offset-DXY); //gc_cfg::X_LOGICAL_DISPLAY_OFFSET+gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION);
			if ((fill_size_right+DXY)>gc_cfg::X_MAX_PHYSICAL_DISPLAY_DIMENSION)
			{
				fill_size_right=1;
			}

			//p=&p[p_.fb_pitch*gc_cfg::Y_LOGICAL_DISPLAY_OFFSET];
			for(int r=0; r<y_offset; ++r)
			{
				memset(&p[0], 0, p_.fb_pitch);
				p+=p_.fb_pitch;
			}
			uint8_t* pu=&p[p_.fb_pitch*y_offset+x_offset];

			for(int r=0; r<DXY; ++r)
			{
				memset(&p[0], 0, x_offset);
#ifdef SC_FAST_UNALIGNED_COPY
				display_unaligned_copy(pu, &sc[r*512], gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION);
#else
				memcpy(pu, &sc[r*sc_stride], DXY); //gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION);
#endif
				memset(&pu[DXY], 0, fill_size_right);
				//memcpy(fb, &image[0][0], 512*512);
				p+=p_.fb_pitch;
				pu+=p_.fb_pitch;
			}

			int trailing_rows=(PY)-y_offset;
			if (trailing_rows>0)
			{
				memset(&p[0], 0, p_.fb_pitch*trailing_rows);
			}
#if 0
			for(int r=0; r<trailing_rows; ++r)
			{
				memset(&p[0], 0, p_.fb_pitch);
				p+=p_.fb_pitch;
			}
#endif
		}
		else
		{
			//TODO: make code common with polar scan converter buffer?
			//uint8_t* p=reinterpret_cast<uint8_t*>(fb);
			unsigned int fill_size_right=p_.fb_pitch-(gc_cfg::X_LOGICAL_DISPLAY_OFFSET+gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION);
			uint8_t* pu=&p[p_.fb_pitch*y_offset+x_offset];

			for(int r=0; r<y_offset; ++r)
			{
				memset(&p[0], 0, p_.fb_pitch);
				p+=p_.fb_pitch;
			}

			if (sar_image.sar_mode)
			{
				{
					auto pleft=p;
					auto pright=pu;

					for(int r=0; r<DXY; ++r)
					{
						memset(&pleft[0], 0, x_offset);

						memset(&pright[DXY], 0, fill_size_right);
						pleft+=p_.fb_pitch;
						pright+=p_.fb_pitch;
					}
				}

				float zoom=DXY/float(sar_image.dx);
				if (sar_image.zoom>1)
					zoom=zoom*2; //sar_image.zoom;
#ifdef __MINGW32__
				//zoom*=2;
#endif
				if (sar_image.raw_img)
				{
					float sar_rotation=p_.sar_rot_simulate;
					image_resampling(
							&pu[0], DXY, DXY, p_.fb_pitch,

							sar_image.dx/2, sar_image.dy/2, sar_rotation, zoom, //0.5,

							sar_image.bpp, (image_resampling_src_type_t*)sar_image.raw_img, sar_image.dx, sar_image.dy, sar_image.stride);
				}
			}
			else
			{

				for(int r=0; r<DXY; ++r)
				{
					const uint8_t* __restrict img=sar_image.store.image[r];
					uint8_t* __restrict dst=&pu[0];
					memset(&p[0], 0, x_offset);

					for(int c=0; c<DXY; ++c)
					{
						uint8_t v=img[c];
						v=gc_color_theme::sc_color_index_convert(v);//v=(v>>1)+128;
						dst[c]=v;
					}
					//memset(&p[gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION], 0, fill_size);
					memset(&pu[DXY], 0, fill_size_right);
					p+=p_.fb_pitch;
					pu+=p_.fb_pitch;
				}
			}

			int trailing_rows=(gc_cfg::Y_PHYSICAL_DISPLAY_DIMENSION-gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION)-gc_cfg::Y_LOGICAL_DISPLAY_OFFSET;
			if (trailing_rows>0)
			{
				uint8_t* p=reinterpret_cast<uint8_t*>(fb);
				p=&p[p_.fb_pitch*DXY];
				memset(&p[0], 0, p_.fb_pitch*trailing_rows);
			}
		}
		//memcpy(fb, &image[0][0], 512*512);
	}
	else
	{
		if (clear_need)
		{
			SoftScanConverter::clear();
			clear_need=false;
		}
		memset(fb, 0, p_.fby*p_.fb_pitch); //512*512);
	}
#if 0
	if (p_.dbg_show)
	{//Overload Oscilloscope and messages
		uint8_t* p=reinterpret_cast<uint8_t*>(fb);
		gfd_framebuffer_t sfb;
		gfd_fb_init(&sfb, 512, 512, p_.fb_pitch, &p[p_.fb_pitch*gc_cfg::Y_LOGICAL_DISPLAY_OFFSET+gc_cfg::X_LOGICAL_DISPLAY_OFFSET]);
#if 0
		if (p_.dbs_mode)
		{
			const float dx=(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2);

			DbsScanHelper& dbs=DbsScanHelper::instance();
			DbsScanHelper::dbs_scan_volume_t& v=dbs.scanVolume();
			float angle=(v.ground_track+v.platform_azimuth);
			float y=dx*cosf(angle);
			float x=dx*sinf(angle);

			gfd_fb_draw_line(&sfb,
					(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)-x, (gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION)-y,
					gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION,
					gc_color_theme::gc_color_dbg/*64*/);

			for(int i=0; i<v.fullVolume.bar_num; ++i)
			{
				const float dx=(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)+100;
				angle=v.b[i].hard_int_lim;
				float y=dx*cosf(angle);
				float x=dx*sinf(angle);
				gfd_fb_draw_line(&sfb,
						(gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2)-x, (gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION)-y,
						gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION/2, gc_cfg::XY_LOGICAL_DISPLAY_DIMENSION,
						gc_color_theme::gc_color_dbg/*64*/);
			}
		}
#endif

		if (p_.dbg_msg_show)
		{
			for(int n_msg=0; n_msg<4; ++n_msg)
			{
				if (p_.msg[n_msg][0])
					gfd_fb_printf(&sfb, &gfd_font256_PC_8x8, 50, 38+n_msg*10, gc_color_theme::gc_color_dbg/*64*/, p_.msg[n_msg]);
			}
		}

		if (p_.dbg_wf_show)
		{
			for(int i=0; i<ScImplementation::wf_max; ++i)
			{
				ScImplementation::wf_t& w=p_.wf[i];
				if (w.visible)
					p_.oscilloscopeShowWf((uint8_t*)sfb.buffer, i, w.n_samples, w.samples, p_.fb_pitch, gc_color_theme::gc_color_red);
			}
		}
	}
#endif
}

void SoftScanConverterDriver::setGeometry(unsigned int dx, unsigned int dy, unsigned int pitch)
{
	//p_.setupGeometry(dx, dy, pitch);
	p_.fb_pitch=pitch;
	p_.fby=dy;
	p_.dx=dx;
	p_.dy=dy;
}

#if 0
SoftScanConverterDriver::ScBuffer SoftScanConverterDriver::buffer()
{
	return p_.img;
}
#endif


SoftScanConverterDriver& SoftScanConverterObject::instance()
{
	static SoftScanConverterDriver::ScImplementation scImp;
	static SoftScanConverterDriver sc(scImp);

	return sc;
}


