#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifdef WIN32
#define _USE_MATH_DEFINES
#endif
#include <math.h>
#include "sad.h"
#include "fisheye.h"

#include "define.h"

#ifdef _DEBUG
#ifdef WIN32
#include <crtdbg.h>
#endif
#endif

#define MAGROT 0.15
//#define MAGROT 0.20
#define SEARCH_D 3
#define SEARCH_R 2


/* for map 暫定 */
extern int    mypos_x;
extern int    mypos_y;
extern double mypos_r;
extern unsigned char gMap[MAP_X*MAP_Y];


// sad for map at etl
// 64x64のパターンに対して
/**
 *
 * @param glmap 二値化済み地図画像
 *		buf2 2値化・64×64補正済み、現在地画像
 * @return ６４×６４範囲内の差分値
 *			0 = 完全一致(かも知れない)
 *
 */
int sad64(unsigned char *glmap, unsigned char *buf2, int dx, int dy)
{
	int x, y;
	int s, a;

	s = 0;
	for (y=0; y< 64; y++) {
		for (x=0; x< 64; x++) {
/*			s += abs(glmap[(x+dx)+(y+dy)*MAP_X] - buf2[x+y*64]);  */
			a = (glmap[(x+dx)+(y+dy)*MAP_X] - buf2[x+y*64]);

/*			if (a>=0)   a=0; */
/*			else        a=abs(a); */

/*			s += a; */
			if (a<0)
				s -= a;
		}
	}
	return(s);
}

/* matching 5.0 res for map */
/**
 * @brief ???
 * @param bf 使用していない(常にNULLがくる)
 *		rgbbf_a1 入力画像
 *		saa ???
 *		dx ???
 *		dy ???
 *		dr ???
 *		search_inc ???
 *
 */
void matt(unsigned char *bf, unsigned char *rgbbf_a1, int *saa, int *dx, int *dy, double *dr, int search_inc)
{
	unsigned char	buf[64*64];
	int				min;
	double			mx=0,	my=0,	mr=0;
	int				i,		s;
	int				x,		y;

/*	min = 255*48*48; */
	FisheyeUnwarp64(rgbbf_a1, buf, 0.2, mypos_r);
	min = sad64(gMap, buf, mypos_x, mypos_y);

	mx = 0;
	my = 0;
	mr = 0.0;

/*	for (i=-(2+search_inc); i<=(2+search_inc); i++) */
/*	{  */
/*		FisheyeUnwarp64(rgbbf_a1, buf, 0.2, mypos_r+(double)(i*M_PI/36)); */
/*		for (x=-(2+search_inc); x<=(2+search_inc); x++) */
/*		{ */
/*			for (y=-(2+search_inc); y<=(2+search_inc); y++) */
/*			{ */

	// ±８(*5)度の範囲
	for (i= -(SEARCH_PAD + search_inc); i <= (SEARCH_PAD + search_inc); i++) { 

		FisheyeUnwarp64(rgbbf_a1, buf, 0.2, mypos_r + (double)( i * M_PI / 36 ));
		// ±８ピクセル X方向
		for (x = -(SEARCH_PAD + search_inc); x <= (SEARCH_PAD + search_inc); x++) {

			// ±８ピクセル Y方向
			for ( y = -(SEARCH_PAD + search_inc); y <= (SEARCH_PAD + search_inc); y++) {
				s = sad64(gMap, buf, x + mypos_x, y + mypos_y);

				/*
					±８×８の範囲内でかつ、±８(*5)度以内にさらに最適な解がある場合は現在点情報を上書きする
				*/
				if (s < min) {
					min = s;
					mx = x;
					my = y;
					mr = (double)i;
				}
			}
		}
	}
	*saa = min; *dx = mx; *dy = my; *dr = mr * M_PI / 36.0;
}
