#ifndef WIN32
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <unistd.h>
#include <linux/types.h>
#include <linux/videodev.h>
#include <pthread.h>
#else
#define _USE_MATH_DEFINES
#endif
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "fisheye.h"
#include "sad.h"

#ifdef _DEBUG
#ifdef WIN32
#include <crtdbg.h>
#endif
#endif
     
#define VWIDTH	320
#define VWIDTH2	64
#define VHEIGHT	240
#define SIZE	64
#define SIZEH	(SIZE / 2)

/* Fisheye Camera */
#define CENTER_X	160
#define CENTER_Y	127
#define FISHEYE1	70
#define FISHEYE2	135

//#define MAG		0.20
#define MAG		0.30
//#define MAG		0.28
#define BW_TH	240		/* 2値化の閾値 */
//#define BW_TH	200		/* 2値化の閾値 */



int gFisheyeUnwarpTable320[VHEIGHT][VWIDTH];
int gTable64[73][SIZE][SIZE];


/* 48x48 unwarp 5.0 res */
/**
 * @brief グレースケール画像を2値化する
 * @attention 元画像を回転した結果を格納する
 * @param in 入力画像(グレースケール)
 *		  out 出力画像(回転済み、2値化画像)
 *		  mag 使用しない
 *		  rot 回転角度(ラジアン)
 * @note 元画像は640×480だが、生成される二値画像は64×64になる
 */
void FisheyeUnwarp64(unsigned char *in, unsigned char *out, double mag, double rot)
{
	int x, y;
	int r;

	if (rot > M_PI)		// 180度を越えている場合、マイナスに変更する
		rot = -M_PI * 2 + rot;
	else if (rot < -M_PI)	// -180を下回る場合、プラスに変更する
		rot = M_PI * 2 + rot;

	r = (int)(rot / ( M_PI / 36 ) ) + 36;	// ラジアン->度 変換

	for (x=0; x < SIZE; x++) {
		for (y=0; y < SIZE; y++) {
			if (gTable64[r][x][y] >= 0 && gTable64[r][x][y] < 320 * 240)
				out[x+y*64] = in[gTable64[r][x][y]];	// 回転した場合の座標位置にあるデータを取り出す
			else
				out[x+y*64] = 0;

			if (out[x+y*64] > BW_TH )	// 2値化の閾値判定を行い、白/黒に変更する
				out[x+y*64] = 255;
			else
				out[x+y*64] = 0;
		}
	}
}

#ifdef _NOT_USE	// Add FSI
/* 48x48 unwarp 5.0 res */
void FisheyeUnwarp64test(unsigned char *in, unsigned char *out, double mag, double rot)
{
  int x, y;
  int r;

  if (rot > M_PI)
    rot = -M_PI*2 + rot;
  else if (rot < -M_PI)
    rot = M_PI*2 + rot;
  r = (int)(rot/(M_PI/36))+36;

  for (x=0; x<SIZE; x++)
    {
      for (y=0; y<SIZE; y++)
	{
	  if (gTable64[r][x][y] >= 0 &&
	      gTable64[r][x][y] < 320*240)
	    out[x+y*64] = in[gTable64[r][x][y]];
	  else
	    out[x+y*64] = 0;
	}
    }
}
#endif

#ifdef _NOT_USE	// Add FSI
/* 320x240 unwarp */
void FisheyeUnwarp320(unsigned char *inPic, unsigned char *outPic)
{
  int x, y;

  for (y = 0; y < VHEIGHT; y++)
    {
      for (x = 0; x < VWIDTH; x++)
	{
	  outPic[x + y*(VWIDTH)+0] = inPic[gFisheyeUnwarpTable320[y][x]];
	}
    }
}
#endif

#ifdef _NOT_USE	// Add FSI
/* 320x240 unwarp table */
void FisheyeUnwarpMakeTable320(void)
{
  int x, y;
  double cx, cy, x1, y1, x2, y2;
  double f, ff;
  double d, dd;

  cx = 160;
  cy = 127;
  f = 70; /*70*/
  ff = 135; /*small*/

  for (y = 0; y < VHEIGHT; y++)
    {
      for (x = 0; x < VWIDTH; x++)
	{
	  x1 = x - cx;
	  y1 = y - cy;
	  y1 = y1/0.95;

	  d = sqrt(x1*x1+y1*y1);
	  dd = ff*atan(d/f);

	  if (d == 0) d=1;
	  x2 = x1*(dd/d)+cx;
	  y2 = y1*(dd/d)+cy;

	  if (x2 < 0 ) x2 = 0;
	  if (x2 > 319 ) x2 = 319;
	  if (y2 < 0 ) {y2 = 0; x2=0;}
	  if (y2 > 239 ) {y2 = 239; x2=0;}

	  gFisheyeUnwarpTable320[y][x] = (int)x2+(int)y2*VWIDTH;
	}
    }
}
#endif

#ifdef _NOT_USE		// Add FSI
/*														*/
/* 変換テーブル作成(魚眼レンズ補正、回転、スケール変換)	*/
/*														*/
/* 64x64 unwarp 5.0 res table */
void FisheyeUnwarpMakeTable64(void)
{
  int i;
  int x, y;
  int x2, y2;
  int sx, sy;
  double cx, cy, dx1, dy1, dx2, dy2;
  double f, d;
  double ff, dd;

  cx = CENTER_X;
  cy = CENTER_Y;
  f  = FISHEYE1;
  ff = FISHEYE2;

  sx = cx - (SIZEH/MAG);
  sy = cy - (SIZEH/MAG);

  for (y=0; y<SIZE; y++)
    {
      for (x=0; x<SIZE; x++)
	{
	  for (i=-36; i<=36; i++)
	    {
	      x2 = sx + ((double)(x-SIZEH)*cos(i*M_PI/36) - (double)(y-SIZEH)*sin(i*M_PI/36)+SIZEH)/MAG;
	      y2 = sy + ((double)(x-SIZEH)*sin(i*M_PI/36) + (double)(y-SIZEH)*cos(i*M_PI/36)+SIZEH)/MAG;

	      dx1 = -(x2 - cx);
	      dy1 = -(y2 - cy);

	      d = sqrt(dx1*dx1+dy1*dy1);
	      dd =ff*atan(d/f);

	      if (d == 0) d=1;
	      dx2 = dx1*(dd/d);
	      dy2 = dy1*(dd/d);

	      dx2 += cx;
	      dy2 += cy;

	      if (dx2 < 0 ) dx2 = 0; 
	      if (dx2 > 319 ) dx2 = 319; 
	      if (dy2 < 0 ) {dy2 = 0; dx2=0;}
	      if (dy2 > 239 ) {dy2 = 239; dx2=0;}

	      gTable64[i+36][x][y] = (int)((int)dx2+(int)dy2*VWIDTH);
	      /* 離散座標に変換にしてからでないと... */
	    }
	}
    }
}
#endif		// Add FSI

/**
 * @brief 各座標点の回転後の座標？を算出する
 * @note 5度単位に角度をずらしてテーブルを構築している。
 *		180 -> 36(*5)
 *
 */
void CamaeraUnwarpMakeTable64(void)
{
	int		i;
	int		x,	y;
	int		x2,	y2;
	int		sx,	sy;
	double	cx,	cy, dx1, dy1;

	cx = CENTER_X;	// カメラセンター設定
	cy = CENTER_Y;	// カメラセンター設定

	sx = cx - (int)((double)SIZEH / MAG);
	sy = cy - (int)((double)SIZEH / MAG);

	for (y = 0; y < SIZE; y++){
		for (x = 0; x < SIZE; x++){
			for (i = -36; i <= 36; i++){
				x2 = y2 = 0;

				x2 = sx + ( (double)((double)( x - SIZEH )) * cos((double)i * M_PI / 36.0) - 
							(double)((double)( y - SIZEH )) * sin((double)i * M_PI / 36.0) + (double)SIZEH ) / MAG;

				y2 = sy + ( (double)((double)( x - SIZEH )) * sin((double)i * M_PI / 36.0) +
							(double)((double)( y - SIZEH )) * cos((double)i * M_PI / 36.0) + (double)SIZEH ) / MAG;


/*				/\* 離散座標に変換にしてからでないと... *\/ */

				dx1 = x2;
				dy1 = y2;

				if (dx1 <   0 ) dx1 = 0;
				if (dx1 > 319 ) dx1 = 319;
				if (dy1 <   0 ) {dy1 = 0; dx1=0;}
				if (dy1 > 239 ) {dy1 = 239; dx1=0;}

				gTable64[i+36][x][y] = (int)((int)dx1+(int)dy1*VWIDTH);
			}
		}
	}
}


