// ConsoleApplication1.cpp: определяет точку входа для консольного приложения.
//
#define _AFXDLL
#include "stdafx.h"
//#include "afxwin.h"
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <vector>
#include "main.h"
#include <atlstr.h>
#include <atlimage.h>

using namespace std;

typedef unsigned char U8;
typedef signed char S8;
typedef unsigned short U16;
typedef signed short S16;
typedef unsigned int U32;
typedef signed int S32;
typedef bool BIT;

#define POROG 50
#define	M_PI 3.14159265358979323846

//int load_image(_TCHAR *fname, FIFO_ITEM *ff1, U16 &k1);
int load_image2(_TCHAR *fname, FIFO_ITEM *ff1, U16 &k1);
void ssaimage(_TCHAR *fname, int num);
void bcaimage(_TCHAR *fname, int num);
void bcaimage2(_TCHAR *fname, int num);
void eicimage(_TCHAR *fname, int num);
void cmcimage(_TCHAR *fname, int num, U16 x, U16 y);
void cmcimage2(_TCHAR *fname, FIFO_ITEM_f *centr, U16 num, U8 array);

void drawrect(CImage *im, U16 l, U16 t, U16 r, U16 b, COLORREF color);
void drawcircle(CImage *im, U16 x, U16 y, U16 r, COLORREF color);
void drawrectsolid(CImage *im, U16 l, U16 t, U16 r, U16 b, COLORREF color);

static void store_shot_array1(BIT use_extended_field_of_view);
static void store_shot_array2(BIT use_extended_field_of_view);
static void build_clusters_array1(void);
static void build_clusters_array2(void);
static void exclude_intersected_clusters_from_array1(void);
static void exclude_intersected_clusters_from_array2(void);
static void enlarge_clusters_in_array1(void);
static void enlarge_clusters_in_array2(void);
static BIT calculate_mass_centre_by_array1(float *px, float *py);
static BIT calculate_mass_centre_by_array2(float *px, float *py);
static BIT calculate_mass_centre(FIFO_ITEM_f *mass, U16 *num, U8 array);
static BIT cmcentre(U8 array);

static U64 get_time_10us(void);
U16 get_coords_count(void);
BIT pop_coords_item(COORDS_DATA *coords);
BIT push_coords_item(const COORDS_DATA *coords);
static void calculate_angles(float x, float y, S16 *pAngle1, S16 *pAngle2);

__inline void CopyRect(ARM_RECT *dst, const ARM_RECT *src)
{
	dst->left = src->left;
	dst->top = src->top;
	dst->right = src->right;
	dst->bottom = src->bottom;
}
__inline BIT EmptyRect(const ARM_RECT *r)
{
	return !(((S32)r->right - (S32)r->left) > 0 &&
		((S32)r->bottom - (S32)r->top) > 0);
}
__inline BIT IntersectRect(ARM_RECT *r, const ARM_RECT *r1, const ARM_RECT *r2)
{
	ARM_RECT rect;

	CopyRect(&rect, r1);

	if (r2->left   > r1->left) rect.left = r2->left;
	if (r2->top    > r1->top) rect.top = r2->top;
	if (r2->right  < r1->right) rect.right = r2->right;
	if (r2->bottom < r1->bottom) rect.bottom = r2->bottom;

	if (r)
		*r = rect;

	return !EmptyRect(&rect);
}
__inline void make_point(FIFO_ITEM *pi, U8 *pm, THE_POINT *point)
{
	point->pi = pi;
	point->pm = pm;
}
__inline BIT neighbour_points(const THE_POINT *p1, const THE_POINT *p2, U8 length)
{
	return (abs((S16)p2->pi->x - (S16)p1->pi->x) <= length) && (abs((S16)p2->pi->y - (S16)p1->pi->y) <= length);
}
__inline BIT point_has_no_marker(const THE_POINT *p)
{
	return *(p->pm) == NO_MARKER;
}
__inline void copy_marker(THE_POINT *dst, const THE_POINT *src)
{
	*(dst->pm) = *(src->pm);
}
static unsigned short read_u16(FILE *fp)
{
	unsigned char b0, b1;

	b0 = getc(fp);
	b1 = getc(fp);

	return ((b1 << 8) | b0);
}
static unsigned int read_u32(FILE *fp)
{
	unsigned char b0, b1, b2, b3;

	b0 = getc(fp);
	b1 = getc(fp);
	b2 = getc(fp);
	b3 = getc(fp);

	return ((((((b3 << 8) | b2) << 8) | b1) << 8) | b0);
}
static int read_s32(FILE *fp)
{
	unsigned char b0, b1, b2, b3;

	b0 = getc(fp);
	b1 = getc(fp);
	b2 = getc(fp);
	b3 = getc(fp);

	return ((int)(((((b3 << 8) | b2) << 8) | b1) << 8) | b0);
}

vector<FIFO_ITEM> v1, v2;

int _tmain(int argc, _TCHAR* argv[])
//int _tmain(int argc, char* argv[])
{
	USES_CONVERSION;
	if (argc < 3)
	{
		printf("Error! Need two parameters! image1 image2 \r\n");
		return 1;
	}

	string fn1 = W2A(argv[1]);
	string fn2 = W2A(argv[2]);

	CString t1 = argv[1];
	CString t2 = _T("asdfgh");
	CString t3 = t1 + t2;
//	t3.

//	printf("%s %s\r\n", t1, t2);
	cout << "- " << W2A(t3) << " - " << t2 << " -" << endl;

	FIFO_ITEM f1[4096] = { 0 };
	U16 num1 = 0;
	FIFO_ITEM f2[4096] = { 0 };
	U16 num2 = 0;

	load_image2(argv[1], f1, num1);
	load_image2(argv[2], f2, num2);
	
	for (int i = 0; i < num1; i++)
	{
		printf("%d\t- %d.%d\n",i, f1[i].x, f1[i].y);
		v1.push_back(f1[i]);
	}
	cout << "Pixels 1 = " << num1 << endl;
	cout << "Pixels 2 = " << v1.size() << endl;
	for (int i = 0; i < num2; i++)
	{
		printf("%d\t- %d.%d\n", i, f2[i].x, f2[i].y);
		v2.push_back(f2[i]);
	}
	cout << "Pixels 2 = " << num2 << endl;
	cout << "Pixels 2 = " << v2.size() << endl;

	g_config1.nXc = 157;
	g_config1.nYc = 128;
	g_config1.nRs = 120;
	g_config1.nRb = 122;
	g_config1.cLc = 5;
	g_config1.fA = 1.64939992828295E-05;
	g_config1.fB = -0.00147799996193498;
	g_config1.fC = 0.667499005794525;
	g_config2.nXc = 157;
	g_config2.nYc = 128;
	g_config2.nRs = 120;
	g_config2.nRb = 122;
	g_config2.cLc = 5;
	g_config2.fA = 1.64939992828295E-05;
	g_config2.fB = -0.00147799996193498;
	g_config2.fC = 0.667499005794525;

	BIT ok, ok2;
	BIT second_pass_flag;
	S16 angle1, angle2;
	float x = 0, y = 0;
	COORDS_DATA coords;
	U32 start_time = 0;
	U32 cur_time;
	FIFO_ITEM_f centers[10] = { 0 };
	U16 num_shoot = 0;

	// запуск обработки картинок

	second_pass_flag = __FALSE;

	for (int i = 0; i < 2; i++)
	{
		BIT extended_field_of_view = second_pass_flag;

		store_shot_array1(extended_field_of_view);
		store_shot_array2(extended_field_of_view);

		ssaimage(argv[1], 1);
		ssaimage(argv[2], 2);

		build_clusters_array1();
		build_clusters_array2();

		bcaimage(argv[1], 1);
		bcaimage(argv[2], 2);

		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		bool CMOS1_RKS_CMD_REG = true;
		bool CMD_TIMER_CAPTURED_BIT = true;
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

		if (CMOS1_RKS_CMD_REG & CMD_TIMER_CAPTURED_BIT)	// CMOS1 has captured a shot
		{
			enlarge_clusters_in_array2();
			bcaimage2(argv[2], 2);

			exclude_intersected_clusters_from_array1();
			eicimage(argv[1], 1);

			ok = calculate_mass_centre_by_array1(&x, &y);
			cmcimage(argv[1], 1, x, y);

			ok2 = calculate_mass_centre(centers, &num_shoot, 1);
			cmcimage2(argv[1], centers, num_shoot, 1);
			cmcentre(1);
		}
		else											// CMOS2 has captured a shot
		{
			enlarge_clusters_in_array1();
			bcaimage2(argv[1], 1);

			exclude_intersected_clusters_from_array2();
			eicimage(argv[2], 2);

			ok = calculate_mass_centre_by_array2(&x, &y);
			cmcimage(argv[2], 2, x, y);

			ok2 = calculate_mass_centre(centers, &num_shoot, 2);
			cmcimage2(argv[2], centers, num_shoot, 2);
			cmcentre(2);
		}
/*
		if (ok)
		{
			// XY-coordinates have been successfully calculated,
			// now calculate angles
			calculate_angles(x, y, &angle1, &angle2);

			coords.time = (U32)get_time_10us();
			coords.angle1 = angle1;
			coords.angle2 = angle2;

			push_coords_item(&coords);

			break;
		}
*/
		if (ok2)
		{
			calculate_angles(x, y, &angle1, &angle2);
			cout << "Common mass center  = " << angle1 << " " << angle2 << endl;
			for (int i = 0; i < num_shoot; i++)
			{
				calculate_angles(centers[i].x, centers[i].y, &angle1, &angle2);
				cout << "Mass center  = " << angle1 << " " << angle2 << endl;
				coords.time = (U32)get_time_10us();
				coords.angle1 = angle1;
				coords.angle2 = angle2;
				push_coords_item(&coords);
			}

			break;
		}

		// if couldn't calculate mass center, try to do it again using
		// extended field of view in 2-nd processing pass
		second_pass_flag = __TRUE;
	}


	return 0;
}
/*
int load_image(_TCHAR *fname, FIFO_ITEM *ff1, U16 &k1)
{
	USES_CONVERSION;
	FILE * pFile1 = fopen(W2A(fname), "rb");

	// считываем заголовок файла
	cout << "Load " << W2A(fname) << endl;
	//	BITMAPFILEHEADER header;
	BFHEADER header1;

	header1.bfType = read_u16(pFile1);
	header1.bfSize = read_u32(pFile1);
	header1.bfReserved1 = read_u16(pFile1);
	header1.bfReserved2 = read_u16(pFile1);
	header1.bfOffBits = read_u32(pFile1);

	cout << "Size " << header1.bfSize << endl;

	// считываем заголовок изображения
	//	BITMAPINFOHEADER bmiHeader;
	BIHEADER bmiHeader1;

	bmiHeader1.biSize = read_u32(pFile1);
	bmiHeader1.biWidth = read_s32(pFile1);
	bmiHeader1.biHeight = read_s32(pFile1);
	bmiHeader1.biPlanes = read_u16(pFile1);
	bmiHeader1.biBitCount = read_u16(pFile1);
	bmiHeader1.biCompression = read_u32(pFile1);
	bmiHeader1.biSizeImage = read_u32(pFile1);
	bmiHeader1.biXPelsPerMeter = read_s32(pFile1);
	bmiHeader1.biYPelsPerMeter = read_s32(pFile1);
	bmiHeader1.biClrUsed = read_u32(pFile1);
	bmiHeader1.biClrImportant = read_u32(pFile1);

	cout << "Width " << bmiHeader1.biWidth << endl;
	cout << "Height " << bmiHeader1.biHeight << endl;

	RGBQ **rgb1 = new RGBQ*[bmiHeader1.biWidth];
	for (int i = 0; i < bmiHeader1.biWidth; i++)
	{
		rgb1[i] = new RGBQ[bmiHeader1.biHeight];
	}

	for (int i = 0; i < bmiHeader1.biWidth; i++)
	{
		for (int j = 0; j < bmiHeader1.biHeight; j++)
		{
			rgb1[i][j].rgbBlue = getc(pFile1);
			rgb1[i][j].rgbGreen = getc(pFile1);
			rgb1[i][j].rgbRed = getc(pFile1);
		}
		// пропускаем последний байт в строке
		getc(pFile1);
	}

	fclose(pFile1);

//	ff1[4096] = { 0 };
//	k1 = 0;
	// выводим результат
	for (int i = 0; i < bmiHeader1.biWidth; i++)
	{
		for (int j = 0; j < bmiHeader1.biHeight; j++)
		{
			if (rgb1[i][j].rgbRed > POROG && rgb1[i][j].rgbGreen > POROG && rgb1[i][j].rgbBlue > POROG)
			{
//				printf("%d.%d %d %d %d\n", i, j, rgb1[i][j].rgbRed, rgb1[i][j].rgbGreen, rgb1[i][j].rgbBlue);
				ff1[k1].x = i;
				ff1[k1++].y = j;
//				printf("%d.%d\n", ff1[k1-1].x, ff1[k1-1].y);
			}
		}
	}

	cout << "FIFO " << k1 << endl;
	cout << "Load OK " << W2A(fname) << endl;
	return 0;
}

*/
int load_image2(_TCHAR *fname, FIFO_ITEM *ff1, U16 &k1)
{
	USES_CONVERSION;
	CImage ci;
	ci.Load(fname);
	for (int i = 0; i < 100; i++)
	{
		ci.SetPixel(i, i, RGB(255, 0, 0));
	}
	for (int i = 100; i < 200; i++)
	{
		ci.SetPixel(i, i, RGB(0, 255, 0));
	}
	for (int i = 0; i < 100; i++)
	{
		ci.SetPixel(50, i, RGB(0, 255, 255));
	}
	for (int i = 0; i < 100; i++)
	{
		ci.SetPixel(i, 50, RGB(255, 0, 255));
	}

	for (int i = 0; i < 319; i++)
	{
		for (int j = 0; j < 255; j++)
		{
			COLORREF cr = ci.GetPixel(i, j);
			if (GetRValue(cr) > POROG && GetGValue(cr) > POROG && GetBValue(cr) > POROG)
			{
				//				printf("%d.%d %d %d %d\n", i, j, rgb1[i][j].rgbRed, rgb1[i][j].rgbGreen, rgb1[i][j].rgbBlue);
				ff1[k1].x = i;
				ff1[k1++].y = j;
				//				printf("%d.%d\n", ff1[k1-1].x, ff1[k1-1].y);
			}
		}
	}
	ci.Destroy();

	// выводим результат

	cout << "FIFO " << k1 << endl;
	return 0;
}

static void store_shot_array1(BIT use_extended_field_of_view)
{
	U16 i = 0;
	U16 x, y;
	S32 dx, dy, r;

	cout << "store_shot_array1" << endl;
	
	g_shot_count1 = 0;
	while (!v1.empty())
	{
		BIT pixel_is_bad = __FALSE;
		BIT pixel_out_of_view = __FALSE;

		FIFO_ITEM pnt = v1.front();
		v1.erase(v1.begin());
		x = pnt.x + FIFO_X_OFFSET;
		y = pnt.y + FIFO_Y_OFFSET;

		// try to find pixel in bad pixels array
/*
		for (i = 0; i < g_bad_pixels1.count; i++)
		{
			if (x == g_bad_pixels1.array[i].x && y == g_bad_pixels1.array[i].y)
			{
				pixel_is_bad = __TRUE;
				break;
			}
		}
*/
		// check if pixel is out of view
		dx = (S32)x - (S32)g_config1.nXc;
		dy = (S32)y - (S32)g_config1.nYc;

		// select field-of-view radius for processing

		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		bool CMOS1_RKS_CMD_REG = true;
		bool CMD_TIMER_CAPTURED_BIT = true;
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

		if (use_extended_field_of_view == __FALSE)
			r = (CMOS1_RKS_CMD_REG & CMD_TIMER_CAPTURED_BIT ? g_config1.nRs : g_config1.nRb);	// standard field-of-view
		else
			r = (CMOS1_RKS_CMD_REG & CMD_TIMER_CAPTURED_BIT ? g_config1.nRb : g_config1.nRb + 1);	// extended field-of-view
		// check if shot is out of field-of-view
		if ((dx*dx + dy*dy) > r*r)
		{
			pixel_out_of_view = __TRUE;
			printf("%d - pnt %d.%d xy %d.%d dxdy %d.%d outview\r\n", i, pnt.x, pnt.y, x, y, dx, dy);
		}
		else
		{
			printf("%d - pnt %d.%d xy %d.%d dxdy %d.%d\r\n", i, pnt.x, pnt.y, x, y, dx, dy);
		}

		// store pixel if not in bad & not out of view
		if (!pixel_is_bad && !pixel_out_of_view)
		{
			g_shot_array1[g_shot_count1].x = x;
			g_shot_array1[g_shot_count1].y = y;
//			cout << g_shot_count1 << "\t- " << g_shot_array1[g_shot_count1].x << " " << g_shot_array1[g_shot_count1].y << endl;
			g_shot_count1++;
		}
		i++;
	}
	cout << g_shot_count1 << " pix in g_shot_array1" << endl;
}
static void store_shot_array2(BIT use_extended_field_of_view)
{
	U16 i = 0;
	U16 x, y;
	S16 __x, __y;
	S32 dx, dy, r;

	cout << "store_shot_array2" << endl;

	g_shot_count2 = 0;
	while (!v2.empty())
	{
		BIT pixel_is_bad = __FALSE;
		BIT pixel_out_of_view = __FALSE;

		FIFO_ITEM pnt = v2.front();
		v2.erase(v2.begin());
		x = pnt.x + FIFO_X_OFFSET;
		y = pnt.y + FIFO_Y_OFFSET;

		// try to find pixel in bad pixels array
/*
		for (i = 0; i < g_bad_pixels2.count; i++)
		{
			if (x == g_bad_pixels2.array[i].x && y == g_bad_pixels2.array[i].y)
			{
				pixel_is_bad = __TRUE;
				break;
			}
		}
*/
		// check if pixel is out of view
		dx = (S32)x - (S32)g_config2.nXc;
		dy = (S32)y - (S32)g_config2.nYc;
		// select field-of-view radius for processing

		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		bool CMOS2_RKS_CMD_REG = true;
		bool CMD_TIMER_CAPTURED_BIT = true;
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

		if (use_extended_field_of_view == __FALSE)
			r = (CMOS2_RKS_CMD_REG & CMD_TIMER_CAPTURED_BIT ? g_config2.nRs : g_config2.nRb);	// standard field-of-view
		else
			r = (CMOS2_RKS_CMD_REG & CMD_TIMER_CAPTURED_BIT ? g_config2.nRb : g_config2.nRb + 1);	// extended field-of-view

		// check if shot is out of field-of-view
		// convert XY-coords from CMOS2 to CMOS1 sensor coords
		__x = (S16)x - (S16)g_config2.nXc + (S16)g_config1.nXc;
		__y = (S16)y - (S16)g_config2.nYc + (S16)g_config1.nYc;

		if ((dx*dx + dy*dy) > r*r)
		{
			pixel_out_of_view = __TRUE;
			printf("%d - pnt %d.%d xy %d.%d dxdy %d.%d _xy %d.%d outview\r\n", i, pnt.x, pnt.y, x, y, dx, dy);
		}
		else
		{
			printf("%d - pnt %d.%d xy %d.%d dxdy %d.%d _xy %d.%d\r\n", i, pnt.x, pnt.y, x, y, dx, dy, __x, __y);
		}


		// store pixel if not in bad & not out of view
		if (!pixel_is_bad && !pixel_out_of_view && __x > 0 && __y > 0)
		{
			g_shot_array2[g_shot_count2].x = (U16)__x;
			g_shot_array2[g_shot_count2].y = (U16)__y;
//			cout << g_shot_count2 << "\t- " << g_shot_array2[g_shot_count2].x << " " << g_shot_array2[g_shot_count2].y << endl;
			g_shot_count2++;
		}
		i++;
	}
	cout << g_shot_count2 << " pix in g_shot_array2" << endl;
}

static void build_clusters_array1(void)
{
	U8  marker;
	U16 i, j, k, count;
	U16 l, t, r, b;

	cout << "build_clusters_array1" << endl;

	memset(g_cluster_coords1, 0, sizeof(g_cluster_coords1));

	for (i = 0; i < g_shot_count1; i++)
	{
		FIFO_ITEM			*fi = &g_shot_array1[i];
		U16					x = fi->x / CELL_SIZE;
		U16					y = fi->y / CELL_SIZE;
		CLUSTER_COORD		*pcc = &g_cluster_coords1[y*IMAGE_CELL_CX + x];

		pcc->count++;
		pcc->excluded = __FALSE;
		pcc->marker = NO_MARKER;
		pcc->rect.left = (x)* CELL_SIZE;
		pcc->rect.top = (y)* CELL_SIZE;
		pcc->rect.right = (x + 1) * CELL_SIZE;
		pcc->rect.bottom = (y + 1) * CELL_SIZE;
	}

	memset(g_cluster_ptrs1, 0, sizeof(g_cluster_ptrs1));

	g_cluster_count1 = 0;
	for (i = 0; i < IMAGE_CELL_CX*IMAGE_CELL_CY; i++)
	{
		if (g_cluster_coords1[i].count > 0)
			g_cluster_ptrs1[g_cluster_count1++] = &g_cluster_coords1[i];
	}

	marker = NO_MARKER;
	for (i = 0; i < g_cluster_count1; i++)
	{
		CLUSTER_COORD *pcc1 = g_cluster_ptrs1[i];
		S16 x1 = (pcc1->rect.left + pcc1->rect.right) / 2;
		S16 y1 = (pcc1->rect.top + pcc1->rect.bottom) / 2;

		if (pcc1->marker == NO_MARKER)
			pcc1->marker = (++marker);

		for (j = i + 1; j < g_cluster_count1; j++)
		{
			CLUSTER_COORD *pcc2 = g_cluster_ptrs1[j];
			S16 x2 = (pcc2->rect.left + pcc2->rect.right) / 2;
			S16 y2 = (pcc2->rect.top + pcc2->rect.bottom) / 2;

			if (abs(x1 - x2) <= CELL_SIZE && abs(y1 - y2) <= CELL_SIZE)
			{
				if (pcc2->marker == NO_MARKER)
				{
					pcc2->marker = pcc1->marker;
				}
				else
				{
					for (k = 0; k < g_cluster_count1; k++)
					{
						CLUSTER_COORD *pcc3 = g_cluster_ptrs1[k];

						if (pcc3->marker == pcc2->marker)
							pcc3->marker = pcc1->marker;
					}
				}
			}
		}
	}

	memset(g_super_cluster_coords1, 0, sizeof(g_super_cluster_coords1));

	g_super_cluster_count1 = 0;
	for (i = 1; i <= marker; i++)
	{
		count = 0;
		l = 0x7fff, t = 0x7fff, r = 0, b = 0;

		for (j = 0; j < g_cluster_count1; j++)
		{
			CLUSTER_COORD *pcc = g_cluster_ptrs1[j];

			if (pcc->marker == i)
			{
				if (pcc->rect.left   < l) l = pcc->rect.left;
				if (pcc->rect.top    < t) t = pcc->rect.top;
				if (pcc->rect.right  > r) r = pcc->rect.right;
				if (pcc->rect.bottom > b) b = pcc->rect.bottom;

				count += pcc->count;
			}
		}

		if (count > 0)
		{
			printf("marker %d l %d t %d r %d b %d\r\n", i, l, t, r, b);
			g_super_cluster_coords1[g_super_cluster_count1].count = count;
			g_super_cluster_coords1[g_super_cluster_count1].excluded = __FALSE;
			g_super_cluster_coords1[g_super_cluster_count1].marker = NO_MARKER;
			g_super_cluster_coords1[g_super_cluster_count1].rect.left = l;
			g_super_cluster_coords1[g_super_cluster_count1].rect.top = t;
			g_super_cluster_coords1[g_super_cluster_count1].rect.right = r;
			g_super_cluster_coords1[g_super_cluster_count1].rect.bottom = b;
			g_super_cluster_count1++;
		}

	}
}
static void build_clusters_array2(void)
{
	U8  marker;
	U16 i, j, k, count;
	U16 l, t, r, b;

	cout << "build_clusters_array2" << endl;

	memset(g_cluster_coords2, 0, sizeof(g_cluster_coords2));

	for (i = 0; i < g_shot_count2; i++)
	{
		FIFO_ITEM			*fi = &g_shot_array2[i];
		U16					x = fi->x / CELL_SIZE;
		U16					y = fi->y / CELL_SIZE;
		CLUSTER_COORD		*pcc = &g_cluster_coords2[y*IMAGE_CELL_CX + x];
		pcc->count++;
		pcc->excluded = __FALSE;
		pcc->marker = NO_MARKER;
		pcc->rect.left = (x)* CELL_SIZE;
		pcc->rect.top = (y)* CELL_SIZE;
		pcc->rect.right = (x + 1) * CELL_SIZE;
		pcc->rect.bottom = (y + 1) * CELL_SIZE;
	}

	memset(g_cluster_ptrs2, 0, sizeof(g_cluster_ptrs2));

	g_cluster_count2 = 0;
	for (i = 0; i < IMAGE_CELL_CX*IMAGE_CELL_CY; i++)
	{
		if (g_cluster_coords2[i].count > 0)
			g_cluster_ptrs2[g_cluster_count2++] = &g_cluster_coords2[i];
	}

	marker = NO_MARKER;
	for (i = 0; i < g_cluster_count2; i++)
	{
		CLUSTER_COORD *pcc1 = g_cluster_ptrs2[i];
		S16 x1 = (pcc1->rect.left + pcc1->rect.right) / 2;
		S16 y1 = (pcc1->rect.top + pcc1->rect.bottom) / 2;

		if (pcc1->marker == NO_MARKER)
			pcc1->marker = (++marker);

		for (j = i + 1; j < g_cluster_count2; j++)
		{
			CLUSTER_COORD *pcc2 = g_cluster_ptrs2[j];
			S16 x2 = (pcc2->rect.left + pcc2->rect.right) / 2;
			S16 y2 = (pcc2->rect.top + pcc2->rect.bottom) / 2;

			if (abs(x1 - x2) == CELL_SIZE && abs(y1 - y2) == CELL_SIZE)
			{
				if (pcc2->marker == NO_MARKER)
				{
					pcc2->marker = pcc1->marker;
				}
				else
				{
					for (k = 0; k < g_cluster_count2; k++)
					{
						CLUSTER_COORD *pcc3 = g_cluster_ptrs2[k];

						if (pcc3->marker == pcc2->marker)
							pcc3->marker = pcc1->marker;
					}
				}
			}
		}
	}

	memset(g_super_cluster_coords2, 0, sizeof(g_super_cluster_coords2));

	g_super_cluster_count2 = 0;
	for (i = 1; i <= marker; i++)
	{
		count = 0;
		l = 0x7fff, t = 0x7fff, r = 0, b = 0;

		for (j = 0; j < g_cluster_count2; j++)
		{
			CLUSTER_COORD *pcc = g_cluster_ptrs2[j];

			if (pcc->marker == i)
			{
				if (pcc->rect.left   < l) l = pcc->rect.left;
				if (pcc->rect.top    < t) t = pcc->rect.top;
				if (pcc->rect.right  > r) r = pcc->rect.right;
				if (pcc->rect.bottom > b) b = pcc->rect.bottom;

				count += pcc->count;
			}
		}

		if (count > 0)
		{
			printf("marker %d l %d t %d r %d b %d\r\n", i, l, t, r, b);
			g_super_cluster_coords2[g_super_cluster_count2].count = count;
			g_super_cluster_coords2[g_super_cluster_count2].excluded = __FALSE;
			g_super_cluster_coords2[g_super_cluster_count2].marker = NO_MARKER;
			g_super_cluster_coords2[g_super_cluster_count2].rect.left = l;
			g_super_cluster_coords2[g_super_cluster_count2].rect.top = t;
			g_super_cluster_coords2[g_super_cluster_count2].rect.right = r;
			g_super_cluster_coords2[g_super_cluster_count2].rect.bottom = b;
			g_super_cluster_count2++;
		}
	}
}
static void exclude_intersected_clusters_from_array1(void)
{
	U16 i, j;

	for (i = 0; i < g_super_cluster_count1; i++)
	{
		CLUSTER_COORD *c1 = &g_super_cluster_coords1[i];

		for (j = 0; j < g_super_cluster_count2; j++)
		{
			CLUSTER_COORD *c2 = &g_super_cluster_coords2[j];

			if (IntersectRect(NULL, &c1->rect, &c2->rect))
				c1->excluded = __TRUE;
		}
	}
}
static void exclude_intersected_clusters_from_array2(void)
{
	U16 i, j;

	for (i = 0; i < g_super_cluster_count2; i++)
	{
		CLUSTER_COORD *c2 = &g_super_cluster_coords2[i];

		for (j = 0; j < g_super_cluster_count1; j++)
		{
			CLUSTER_COORD *c1 = &g_super_cluster_coords1[j];

			if (IntersectRect(NULL, &c1->rect, &c2->rect))
				c2->excluded = __TRUE;
		}
	}
}
static void enlarge_clusters_in_array1(void)
{
	U16 i;

	cout << "enlarge_clusters_in_array1" << endl;

	for (i = 0; i < g_super_cluster_count1; i++)
	{
		CLUSTER_COORD *pcc = &g_super_cluster_coords1[i];

		if (pcc->rect.left >= CELL_SIZE)	pcc->rect.left -= CELL_SIZE;
		if (pcc->rect.top >= CELL_SIZE)	pcc->rect.top -= CELL_SIZE;
		if (pcc->rect.right <= IMAGE_CX - CELL_SIZE)	pcc->rect.right += CELL_SIZE;
		if (pcc->rect.bottom <= IMAGE_CY - CELL_SIZE)	pcc->rect.bottom += CELL_SIZE;
	}
}
static void enlarge_clusters_in_array2(void)
{
	U16 i;
	cout << "enlarge_clusters_in_array2" << endl;

	for (i = 0; i < g_super_cluster_count2; i++)
	{
		CLUSTER_COORD *pcc = &g_super_cluster_coords2[i];

		if (pcc->rect.left >= CELL_SIZE)	pcc->rect.left -= CELL_SIZE;
		if (pcc->rect.top >= CELL_SIZE)	pcc->rect.top -= CELL_SIZE;
		if (pcc->rect.right <= IMAGE_CX - CELL_SIZE)	pcc->rect.right += CELL_SIZE;
		if (pcc->rect.bottom <= IMAGE_CY - CELL_SIZE)	pcc->rect.bottom += CELL_SIZE;
	}
}
static BIT calculate_mass_centre_by_array1(float *px, float *py)
{
	U16 i, j, c = 0;
	float fx = 0, fy = 0;

	for (i = 0; i < g_super_cluster_count1; i++)
	{
		CLUSTER_COORD *pcc = &g_super_cluster_coords1[i];

		if (pcc->excluded == __FALSE)
		{
			for (j = 0; j < g_shot_count1; j++)
			{
				U16 x = g_shot_array1[j].x;
				U16 y = g_shot_array1[j].y;

				if (x >= pcc->rect.left && x < pcc->rect.right &&
					y >= pcc->rect.top && y < pcc->rect.bottom)
				{
					fx += x;
					fy += y;
					c++;
				}
			}
		}
	}

	if (c > 0)
	{
		*px = fx / (float)c;
		*py = fy / (float)c;

		return __TRUE;
	}

	return __FALSE;
}
static BIT calculate_mass_centre_by_array2(float *px, float *py)
{
	U16 i, j, c = 0;
	float fx = 0, fy = 0;

	for (i = 0; i < g_super_cluster_count2; i++)
	{
		CLUSTER_COORD *pcc = &g_super_cluster_coords2[i];

		if (pcc->excluded == __FALSE)
		{
			for (j = 0; j < g_shot_count2; j++)
			{
				U16 x = g_shot_array2[j].x;
				U16 y = g_shot_array2[j].y;

				if (x >= pcc->rect.left && x < pcc->rect.right &&
					y >= pcc->rect.top && y < pcc->rect.bottom)
				{
					fx += x;
					fy += y;
					c++;
				}
			}
		}
	}

	if (c > 0)
	{
		*px = fx / (float)c;
		*py = fy / (float)c;

		return __TRUE;
	}

	return __FALSE;
}
static void calculate_angles(float x, float y, S16 *pAngle1, S16 *pAngle2)
{
	static float A, B, C;
	static float Xo, Yo;
	static float R, Alpha;
	static float sinB, cosB, sinA;
	static float Fi = 0, Tetta = 0;

	A = g_config1.fA;
	B = g_config1.fB;
	C = g_config1.fC;
	Xo = (float)g_config1.nXc;
	Yo = (float)g_config1.nYc;

	R = sqrt((x - Xo)*(x - Xo) + (y - Yo)*(y - Yo));
	if (R > 0.0)
	{
		float r = R;// * 122.0 / ((float)g_config1.nRs - 10.0);	// default: (Rs - 10.0)

		Alpha = (A*r*r*r + B*r*r + C*r) * M_PI / 180.0;
		if (Alpha > 90.0 * M_PI / 180.0)	Alpha = 90.0 * M_PI / 180.0;
		if (Alpha <  0.0)	Alpha = 0.0;

		sinB = (y - Yo) / R;
		cosB = (x - Xo) / R;
		sinA = sin(Alpha);

		Tetta = asin(sinA * sinB);	//  Tetta: -PI/2...+PI/2

		if (fabs(Tetta) >= M_PI / 2.0)	Fi = 0.0;
		else						Fi = asin(sinA * cosB / cos(Tetta));	// Fi: -PI/2...+PI/2
	}

	*pAngle1 = (S16)rint((-Fi * 180.0 / M_PI) * 60.0);
	*pAngle2 = (S16)rint((-Tetta * 180.0 / M_PI) * 60.0);
}
static BIT calculate_mass_centre(FIFO_ITEM_f *mass, U16 *num, U8 array)
{
	U16 i, j, c = 0;
	float fx = 0, fy = 0;

	if (array == 1)
	{
		for (i = 0; i < g_super_cluster_count1; i++)
		{
			CLUSTER_COORD *pcc = &g_super_cluster_coords1[i];
			if (pcc->excluded == __FALSE)
			{
				for (j = 0; j < g_shot_count1; j++)
				{
					U16 x = g_shot_array1[j].x;
					U16 y = g_shot_array1[j].y;

					if (x >= pcc->rect.left && x < pcc->rect.right &&
						y >= pcc->rect.top && y < pcc->rect.bottom)
					{
						fx += x;
						fy += y;
						c++;
					}
				}
				*num = *num + 1;
			}
			if (*num > 0 && c > 0)
			{
				mass[*num - 1].x = fx / (float)c;
				mass[*num - 1].y = fy / (float)c;
				c = 0;
				fx = 0;
				fy = 0;
				printf("cnt = %d x = %6.2f y = %6.2f\r\n", *num - 1, mass[*num - 1].x, mass[*num - 1].y);
			}
		}
	}
	else
	{
		for (i = 0; i < g_super_cluster_count2; i++)
		{
			CLUSTER_COORD *pcc = &g_super_cluster_coords2[i];
			if (pcc->excluded == __FALSE)
			{
				for (j = 0; j < g_shot_count2; j++)
				{
					U16 x = g_shot_array2[j].x;
					U16 y = g_shot_array2[j].y;

					if (x >= pcc->rect.left && x < pcc->rect.right &&
						y >= pcc->rect.top && y < pcc->rect.bottom)
					{
						fx += x;
						fy += y;
						c++;
					}
				}
				*num = *num + 1;
			}
			if (*num > 0 && c > 0)
			{
				mass[*num - 1].x = fx / (float)c;
				mass[*num - 1].y = fy / (float)c;
				c = 0;
				fx = 0;
				fy = 0;
				printf("cnt = %d x = %6.2f y = %6.2f\r\n", *num - 1, mass[*num - 1].x, mass[*num - 1].y);
			}
		}
	}
	if (*num > 0)
	{
		return __TRUE;
	}
	return __FALSE;
}
static BIT cmcentre(U8 array)
{
	U16 i, j, c = 0, cc = 0;
	float fx = 0, fy = 0;
	S16 angle1, angle2;
	COORDS_DATA coords;

	if (array == 1)
	{
		for (i = 0; i < g_super_cluster_count1; i++)
		{
			CLUSTER_COORD *pcc = &g_super_cluster_coords1[i];
			if (pcc->excluded == __FALSE)
			{
				for (j = 0; j < g_shot_count1; j++)
				{
					U16 x = g_shot_array1[j].x;
					U16 y = g_shot_array1[j].y;

					if (x >= pcc->rect.left && x < pcc->rect.right &&
						y >= pcc->rect.top && y < pcc->rect.bottom)
					{
						fx += x;
						fy += y;
						c++;
					}
				}
				cc = cc + 1;
			}
			if (cc > 0 && c > 0)
			{
				S16 _x = fx / (float)c;
				S16 _y = fy / (float)c;
				calculate_angles(_x, _y, &angle1, &angle2);
				coords.time = (U32)get_time_10us();
				coords.angle1 = angle1;
				coords.angle2 = angle2;
				push_coords_item(&coords);
				c = 0;
				fx = 0;
				fy = 0;
				printf("cnt=%d x=%d y=%d az=%d*%d' um=%d*%d'\r\n", cc - 1, _x, _y, angle1 / 60, abs(angle1 % 60), angle2 / 60, abs(angle2 % 60));
			}
		}
	}
	else
	{
		for (i = 0; i < g_super_cluster_count2; i++)
		{
			CLUSTER_COORD *pcc = &g_super_cluster_coords2[i];
			if (pcc->excluded == __FALSE)
			{
				for (j = 0; j < g_shot_count2; j++)
				{
					U16 x = g_shot_array2[j].x;
					U16 y = g_shot_array2[j].y;

					if (x >= pcc->rect.left && x < pcc->rect.right &&
						y >= pcc->rect.top && y < pcc->rect.bottom)
					{
						fx += x;
						fy += y;
						c++;
					}
				}
				cc = cc + 1;
			}
			if (cc > 0 && c > 0)
			{
				S16 _x = fx / (float)c;
				S16 _y = fy / (float)c;
				calculate_angles(_x, _y, &angle1, &angle2);
				coords.time = (U32)get_time_10us();
				coords.angle1 = angle1;
				coords.angle2 = angle2;
				push_coords_item(&coords);
				c = 0;
				fx = 0;
				fy = 0;
				printf("cnt=%d x=%d y=%d az=%d*%d' um=%d*%d'\r\n", cc - 1, _x, _y, angle1 / 60, abs(angle1 % 60), angle2 / 60, abs(angle2 % 60));
			}
		}
	}
	if (cc > 0)
	{
		return __TRUE;
	}
	return __FALSE;
}

BIT push_coords_item(const COORDS_DATA *coords)
{
	// FIFO buffer is full? exit
	if (g_coords_index == COORDS_FIFO_SIZE)
	{
		return __FALSE;
	}
	g_coords[g_coords_index++] = *coords;
	return __TRUE;
}
BIT pop_coords_item(COORDS_DATA *coords)
{
	// FIFO buffer is empty? exit
	if (g_coords_index == 0)
	{
		return __FALSE;
	}
	*coords = g_coords[--g_coords_index];
	return __TRUE;
}
U16 get_coords_count(void)
{
	U16 count;
	count = g_coords_index;
	return count;
}
static U64 get_time_10us(void)
{
	U64 init_time_10us;
	init_time_10us = g_init_time_10us;
//	return init_time_10us + rtc_GetTickCount_us() / 10;
	return init_time_10us + 0; // rtc_GetTickCount_us() / 10;
}

void ssaimage(_TCHAR *fname, int num)
{
	CImage ci;
	ci.Load(fname);
	CString fn = fname;
	USES_CONVERSION;

	if (num == 1)
	{
		for (int i = 0; i < g_shot_count1; i++)
		{
			ci.SetPixel(g_shot_array1[i].x, g_shot_array1[i].y, RGB(255, 0, 0));
		}
		drawcircle(&ci, 160, 128, g_config1.nRb + 1, RGB(255, 0, 255));
		drawcircle(&ci, g_config1.nXc, g_config1.nYc, g_config1.nRb + 1, RGB(255, 128, 255));
	}
	else
	{
		for (int i = 0; i < g_shot_count2; i++)
		{
			ci.SetPixel(g_shot_array2[i].x, g_shot_array2[i].y, RGB(255, 0, 0));
		}
		drawcircle(&ci, 160, 128, g_config2.nRb + 1, RGB(255, 0, 255));
		drawcircle(&ci, g_config2.nXc, g_config2.nYc, g_config2.nRb + 1, RGB(255, 128, 255));
	}
	fn.Delete(0, 1);
	fn = _T("1") + fn;
	ci.Save(fn);
	ci.Destroy();
}
void bcaimage(_TCHAR *fname, int num)
{
	CImage ci;
	USES_CONVERSION;
	CString fn = fname;
	fn.Delete(0, 1);
	fn = _T("1") + fn;
	ci.Load(fn);
	if (num == 1)
	{
		for (int i = 0; i < g_super_cluster_count1; i++)
		{
			drawrect(&ci,
				g_super_cluster_coords1[i].rect.left,
				g_super_cluster_coords1[i].rect.top,
				g_super_cluster_coords1[i].rect.right,
				g_super_cluster_coords1[i].rect.bottom,
				RGB(0, 255, 0));
		}
	}
	else
	{
		for (int i = 0; i < g_super_cluster_count2; i++)
		{
			drawrect(&ci,
				g_super_cluster_coords2[i].rect.left,
				g_super_cluster_coords2[i].rect.top,
				g_super_cluster_coords2[i].rect.right,
				g_super_cluster_coords2[i].rect.bottom,
				RGB(0, 255, 0));
		}
	}
	fn.Delete(0, 1);
	fn = _T("2") + fn;
	ci.Save(fn);
	ci.Destroy();
}
void bcaimage2(_TCHAR *fname, int num)
{
	CImage ci;
	USES_CONVERSION;
	CString fn = fname;
	fn.Delete(0, 1);
	fn = _T("2") + fn;
	ci.Load(fn);
	if (num == 1)
	{
		for (int i = 0; i < g_super_cluster_count1; i++)
		{
			drawrect(&ci,
				g_super_cluster_coords1[i].rect.left,
				g_super_cluster_coords1[i].rect.top,
				g_super_cluster_coords1[i].rect.right,
				g_super_cluster_coords1[i].rect.bottom,
				RGB(0, 128, 255));
		}
	}
	else
	{
		for (int i = 0; i < g_super_cluster_count2; i++)
		{
			drawrect(&ci,
				g_super_cluster_coords2[i].rect.left,
				g_super_cluster_coords2[i].rect.top,
				g_super_cluster_coords2[i].rect.right,
				g_super_cluster_coords2[i].rect.bottom,
				RGB(0, 128, 255));
		}
	}
	fn.Delete(0, 1);
	fn = _T("3") + fn;
	ci.Save(fn);
	ci.Destroy();
}
void eicimage(_TCHAR *fname, int num)
{
	CImage ci;
	USES_CONVERSION;
	CString fn = fname;
	fn.Delete(0, 1);
	fn = _T("1") + fn;
	ci.Load(fn);
	if (num == 1)
	{
		for (int i = 0; i < g_super_cluster_count1; i++)
		{
			if (g_super_cluster_coords1[i].excluded == __TRUE)
			{
				drawrectsolid(&ci,
					g_super_cluster_coords1[i].rect.left,
					g_super_cluster_coords1[i].rect.top,
					g_super_cluster_coords1[i].rect.right,
					g_super_cluster_coords1[i].rect.bottom,
					RGB(0, 0, 0));
			}
			else
			{
				drawrect(&ci,
					g_super_cluster_coords1[i].rect.left,
					g_super_cluster_coords1[i].rect.top,
					g_super_cluster_coords1[i].rect.right,
					g_super_cluster_coords1[i].rect.bottom,
					RGB(0, 255, 0));
			}
		}
		drawcircle(&ci, 160, 128, g_config1.nRb + 1, RGB(255, 0, 255));
		drawcircle(&ci, g_config1.nXc, g_config1.nYc, g_config1.nRb + 1, RGB(255, 128, 255));
	}
	else
	{
		for (int i = 0; i < g_super_cluster_count2; i++)
		{
			if (g_super_cluster_coords1[i].excluded == __TRUE)
			{
				drawrectsolid(&ci,
					g_super_cluster_coords2[i].rect.left,
					g_super_cluster_coords2[i].rect.top,
					g_super_cluster_coords2[i].rect.right,
					g_super_cluster_coords2[i].rect.bottom,
					RGB(0, 0, 0));
			}
			else
			{
				drawrect(&ci,
					g_super_cluster_coords2[i].rect.left,
					g_super_cluster_coords2[i].rect.top,
					g_super_cluster_coords2[i].rect.right,
					g_super_cluster_coords2[i].rect.bottom,
					RGB(0, 255, 0));
			}
		}
		drawcircle(&ci, 160, 128, g_config2.nRb + 1, RGB(255, 0, 255));
		drawcircle(&ci, g_config2.nXc, g_config2.nYc, g_config2.nRb + 1, RGB(255, 128, 255));
	}
	fn.Delete(0, 1);
	fn = _T("4") + fn;
	ci.Save(fn);
	ci.Destroy();
	ci.Destroy();
}
void cmcimage(_TCHAR *fname, int num, U16 x, U16 y)
{
	CImage ci;
	USES_CONVERSION;
	CString fn = fname;
	fn.Delete(0, 1);
	fn = _T("4") + fn;
	ci.Load(fn);
	if (num == 1)
	{
		for (int i = x - 10; i <= x + 10; i++)
		{
			ci.SetPixel(i, y, RGB(0, 206, 209));
		}
		for (int i = y - 10; i <= y + 10; i++)
		{
			ci.SetPixel(x, i, RGB(0, 206, 209));
		}
	}
	else
	{
		for (int i = x - 10; i <= x + 10; i++)
		{
			ci.SetPixel(i, y, RGB(0, 206, 209));
		}
		for (int i = y - 10; i <= y + 10; i++)
		{
			ci.SetPixel(x, i, RGB(0, 206, 209));
		}
	}
	fn.Delete(0, 1);
	fn = _T("5") + fn;
	ci.Save(fn);
	ci.Destroy();
	ci.Destroy();
}
void cmcimage2(_TCHAR *fname, FIFO_ITEM_f *centr, U16 num, U8 array)
{
	CImage ci;
	USES_CONVERSION;
	CString fn = fname;
	fn.Delete(0, 1);
	fn = _T("5") + fn;
	ci.Load(fn);
	for (int j = 0; j < num; j++)
	{
		if (array == 1)
		{
			for (int i = centr[j].x - 10; i <= centr[j].x + 10; i++)
			{
				ci.SetPixel(i, centr[j].y, RGB(255, 255, 0));
			}
			for (int i = centr[j].y - 10; i <= centr[j].y + 10; i++)
			{
				ci.SetPixel(centr[j].x, i, RGB(255, 255, 0));
			}
		}
		else
		{
			for (int i = centr[j].x - 10; i <= centr[j].x + 10; i++)
			{
				ci.SetPixel(i, centr[j].y, RGB(255, 255, 0));
			}
			for (int i = centr[j].y - 10; i <= centr[j].y + 10; i++)
			{
				ci.SetPixel(centr[j].x, i, RGB(255, 255, 0));
			}
		}
	}
	fn.Delete(0, 1);
	fn = _T("6") + fn;
	ci.Save(fn);
	ci.Destroy();
	ci.Destroy();
}
void drawrect(CImage *im, U16 l, U16 t, U16 r, U16 b, COLORREF color)
{
	for (int i = l; i <= r; i++)
	{
		im->SetPixel(i, t, color);
		im->SetPixel(i, b, color);
	}
	for (int i = t; i <= b; i++)
	{
		im->SetPixel(l, i, color);
		im->SetPixel(r, i, color);
	}
}
void drawcircle(CImage *im, U16 x, U16 y, U16 r, COLORREF color)
{
	U16 cx = 0;
	U16 cy = 0;
	double xLimit = sqrt((double)(r * r) / 2);

	while (cx++ <= xLimit)
	{
		cy = (LONG)floor(sqrt(r * r - cx * cx));
		im->SetPixel(cx + x, cy + y, color);      // 45-90    degrees
		im->SetPixel(cx + x, -cy + y, color);     // 270-315  degrees
		im->SetPixel(-cx + x, cy + y, color);     // 90-135   degrees
		im->SetPixel(-cx + x, -cy + y, color);    // 225-270  degrees
		im->SetPixel(cy + x, cx + y, color);      // 0-45     degrees
		im->SetPixel(cy + x, -cx + y, color);     // 315-360  degrees
		im->SetPixel(-cy + x, cx + y, color);     // 135-180  degrees
		im->SetPixel(-cy + x, -cx + y, color);    // 180-225  degrees
	}
}
void drawrectsolid(CImage *im, U16 l, U16 t, U16 r, U16 b, COLORREF color)
{
	for (int i = l; i <= r; i++)
	{
		for (int j = t; j <= b; j++)
			im->SetPixel(i, j, color);
	}
}