#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifndef WIN32
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
#else
#include "../Common/Linux4Win.h"
#define _USE_MATH_DEFINES
#endif
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#ifdef _USE_PGM
extern "C" {
#include <pgm.h>
}
#endif

#include <cv.h>
#include <cvcompat.h>
#include <highgui.h>


#include "fisheye.h"
#include "image.h"
#include "sad.h"
#include "../Common/sharedmem.h"

#include "define.h"

#define _MAP_VIEW	1

#define SEARCH_INC 3
 
void MapInit(void);
int  MapTracking(unsigned char *rgbbuf);

robot_status *rstat = NULL;
unsigned char gMap[MAP_X*MAP_Y];

int     mypos_x = 0;
int     mypos_y = 0;
double  mypos_r = 0.0;


#ifdef USE_UNDISTORT
void SetUndistortInfo(double in_dF1,
                      double in_dF2,
                      double in_dPrincipalX,
                      double in_dPrincipalY,
                      double in_Dist[4],
                      CvMat *m_CameraMatrix,
                      CvMat *m_DistortionCoeffs
                      )
{
  cvSetReal2D(m_CameraMatrix, 0, 0, in_dF1);
  cvSetReal2D(m_CameraMatrix, 0, 1, 0.0);
  cvSetReal2D(m_CameraMatrix, 0, 2, in_dPrincipalX);
  
  cvSetReal2D(m_CameraMatrix, 1, 0, 0.0);
  cvSetReal2D(m_CameraMatrix, 1, 1, in_dF2);
  cvSetReal2D(m_CameraMatrix, 1, 2, in_dPrincipalY);
  
  cvSetReal2D(m_CameraMatrix, 2, 0, 0.0);
  cvSetReal2D(m_CameraMatrix, 2, 1, 0.0);
  cvSetReal2D(m_CameraMatrix, 2, 2, 1.0);

  cvSetReal1D(m_DistortionCoeffs, 0, in_Dist[0]);
  cvSetReal1D(m_DistortionCoeffs, 1, in_Dist[1]);
  cvSetReal1D(m_DistortionCoeffs, 2, in_Dist[2]);
  cvSetReal1D(m_DistortionCoeffs, 3, in_Dist[3]);
}
#endif


/**
 * @brief 天井マッチングメインループ
 * @attention 永久ループする
 *
 */
void MapLoop(void)
{
	Image vga_img	= _IMGcreate_image(640,480);	// 独自定義画像情報構造体初期化
	Image qvga_img	= _IMGcreate_image(320,240);	// 独自定義画像情報構造体初期化

#ifdef SAVE_TRACK
	unsigned int	count = 130;
	char			fname[256];
//	count = 0;
#endif

#ifdef USE_UNDISTORT
	//IplImage* distort_img = cvCreateImage( cvSize(640, 480), IPL_DEPTH_8U, 1);
	//IplImage* undistort_img = cvCreateImage( cvSize(640, 480), IPL_DEPTH_8U, 1);
	IplImage* undistort_img = NULL;

	// /***************** TV1634M 1.6mm ****************/
	// /* Distortion: -0.254952, 0.036893, -0.004263, 0.001020 */
	// /* Focal Lengths: 271.4525, 269.9548 */
	// /* Principal Point: 299.8679, 253.8837 */
	// /* Camera Intrinsic Matrix: */
	// /* [271.4525 0.0000 299.8679] */
	// /* [0.0000 269.9548 253.8837] */
	// /* [0.0000 0.0000 1.0000] */

	// double
	//   in_dF1 = 271.4525,
	//   in_dF2 = 269.9548;
	// double
	//   in_dPrincipalX = 299.8679,
	//   in_dPrincipalY = 253.8837;
	// double in_Dist[4]={-0.254952,
	//       	     0.036893,
	//       	     -0.004263,
	//       	     0.001020
	// };
	// CvMat *m_CameraMatrix;  
	// CvMat *m_DistortionCoeffs;

	// m_DistortionCoeffs = cvCreateMat(4, 1, CV_64FC1);
	// m_CameraMatrix = cvCreateMat(3, 3, CV_64FC1);

	// SetUndistortInfo(in_dF1, in_dF2, in_dPrincipalX, in_dPrincipalY, in_Dist, 
	//                  m_CameraMatrix, m_DistortionCoeffs);
#endif
cvNamedWindow("Test", CV_WINDOW_AUTOSIZE);
	while(1) {

#ifdef USE_UNDISTORT
		/* スタティック画像読み込み */	// 本番では動画(カメラ画像)入力
		// cvUndistort2(distort_img, undistort_img, m_CameraMatrix, m_DistortionCoeffs);
		if(count > 1061) count = 130;
		sprintf( fname, LOAD_TRACK"cap_%05d.png", count++ );
		undistort_img = cvLoadImage( fname, CV_LOAD_IMAGE_GRAYSCALE);
cvShowImage("Test", undistort_img);
{
	int key;
	key = cvWaitKey(10);
}
		memcpy(vga_img->image,undistort_img->imageData,640*480*sizeof(unsigned char));
#endif
		_IMGhalf(vga_img, qvga_img);	// 画像を1/4にして、水平方向反転する
		MapTracking(qvga_img->image);

#ifdef SAVE_TRACK
#ifdef SAVE_CAPTURE
		sprintf( fname, SAVE_TRACK"cap_%05d.png", count++ );
		cvSaveImage( fname, undistort_img );
#endif
#endif
		usleep(100);
	}
}


int MapTracking(unsigned char *rgbbuf)
{
	int				x,		y;
	int				saa;
	int				dx,		dy;
	double			dr;
	unsigned char	buf[TEMPLATE_SIZE*TEMPLATE_SIZE];
//	unsigned char	buf2[TEMPLATE_SIZE*TEMPLATE_SIZE];
	static int sw = 0; 

#ifdef _DBG_PRINT
	printf("x=%3d y=%3d r=%3.3f\n",mypos_x,mypos_y,mypos_r);
#endif

	if(sw == 0) {
		rstat->x = mypos_x;
		rstat->y = mypos_y;
		rstat->r = mypos_r;
		matt(NULL, rgbbuf, &saa, &dx, &dy, &dr, SEARCH_INC);
		sw = 1;
	}
	else {
		mypos_x = rstat->x;
		mypos_y = rstat->y;
		mypos_r = rstat->r;
		matt(NULL, rgbbuf, &saa, &dx, &dy, &dr, 0);
	}
#ifdef _MAP_VIEW
	FisheyeUnwarp64(rgbbuf, buf, 0.2, M_PI);

	for (y = 0; y < TEMPLATE_SIZE; y++) {
		for (x = 0; x < TEMPLATE_SIZE; x++) {
			rstat->buf[x+y*TEMPLATE_SIZE] = ((int)buf[x+y*TEMPLATE_SIZE]);
		}
	}  
#endif
	if( (mypos_x == rstat->x) && (mypos_y == rstat->y) && (mypos_r == rstat->r) ) {
		mypos_x += dx;
		mypos_y += dy;
		mypos_r += dr;

		while (mypos_r > M_PI)
			mypos_r = -M_PI*2 + mypos_r;
		while (mypos_r < -M_PI)
			mypos_r =  M_PI*2 + mypos_r;

		//printf("%d, %d, %f ( %d )\n", mypos_x, mypos_y, mypos_r, saa);
		rstat->x = mypos_x;
		rstat->y = mypos_y;
		rstat->r = mypos_r;
		rstat->s = saa;
	}

	return 0;
}

/**
 * @brief 地図用マップ画像の読み込み
 *
*/
void MapInit(void)
{
	FILE *ifp0;
	int xsize, ysize, x, y;
#ifdef _USE_PGM
	gray **inp_img0;
	gray maxval;

	ifp0 = pm_openr(MAP);	  // 画像ファイルを開く
	inp_img0 = pgm_readpgm(ifp0, &xsize, &ysize, &maxval);	// 画像ファイル読み込み(2値画像 非グレースケール)
#else
	IplImage*	map_img;
	
	map_img = cvLoadImage(MAP, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);

#endif

#ifdef _DBG_PRINT
	printf("%s x=%3d y=%3d max=%3d\n",MAP,xsize,ysize,maxval);
#endif

	for (y=0; y< MAP_Y; y++){		// 画像の高さ
		for (x=0; x< MAP_X; x++){	// 画像の幅
#ifdef _USE_PGM
			gMap[x+y*MAP_X] = inp_img0[y][x];	// 画像データを1次元配列(グローバル領域)に格納(ってかコピー)
#else
			gMap[x+y*MAP_X] = map_img->imageData[x + y * MAP_X];	// 画像データを1次元配列(グローバル領域)に格納(ってかコピー)
#endif
		}
	}
#ifdef _USE_PGM
	pm_close(ifp0);		// 画像ファイルを閉じる
#endif
}

/**
 * @brief 天井ナビゲーションメイン関数
 *
 */
int main(int argc, char *argv[])
{

	if(argc == 4) {
		mypos_x = atoi(argv[1]);
		mypos_y = atoi(argv[2]);
		mypos_r = atoi(argv[3]);
#ifdef _DBG_PRINT
		printf("x=%3d y=%3d r=%3.3f\n",mypos_x,mypos_y,mypos_r);
#endif
	}

	CamaeraUnwarpMakeTable64();		// ?????????
	MapInit();						// マップ画像読み込み
	rstat = (robot_status *)InitSharedMem(SHM_KEY, sizeof(robot_status));	// 共有メモリ関連・・・(移植時は削除)
	MapLoop();

	return 0;
}
