/*-----------------------------------------
 *
 *-----------------------------------------*/

/*-------------include files-----------------*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include "nearest.h"

//#include <ssm_common.h>
//#include <sensor/URG_SCIP2.h>

PointIndex g_points_table[SIZE_X][SIZE_Y];
PointXY g_points[MAX_POINTS_NUM];
int g_points_num;

void init_table(void){
  int i,j;
  for(i = 0;i<SIZE_X;i++){
    for(j = 0;j<SIZE_Y;j++){
      g_points_table[i][j].num = 0;
    }  
  }
  g_points_num=0;
}


void add_point(double x, double y){
  if(g_points_num >= MAX_POINTS_NUM)return;

  g_points[g_points_num].x= x;
  g_points[g_points_num].y= y;

  g_points_num++;
}

void add_point_table(int n){
  //PointIndex table[SIZE_X][SIZE_Y],Point *points, int n){
  int ix,iy;

  ix = SIZE_X/2+g_points[n].x/(ROBOT_RADIUS*2.0);
  iy = SIZE_Y/2+g_points[n].y/(ROBOT_RADIUS*2.0);

  if(ix < 0 || ix >= SIZE_X)return;
  if(iy < 0 || iy >= SIZE_Y)return;

  if(MAXPOINT_IN_CELL<=g_points_table[ix][iy].num)return;
  g_points_table[ix][iy].points[g_points_table[ix][iy].num]=n;
  g_points_table[ix][iy].num++;

}


//テーブルを利用した最近傍点の探索
double search_min_dist(double x,double y){
  int ix,iy,n,nearest,i,j,k;
  double dist,min_dist;

  //  ix = SIZE_X/2+(x-0.5)/(ROBOT_RADIUS*2.0);
  //  iy = SIZE_Y/2+(y-0.5)/(ROBOT_RADIUS*2.0);
  ix = SIZE_X/2+ x/(ROBOT_RADIUS*2.0);
  iy = SIZE_Y/2+ y/(ROBOT_RADIUS*2.0);

  //探索範囲分を考慮し、IDをクリッピング
  if(ix < 1 || ix >= SIZE_X-2)return 0;
  if(iy < 1 || iy >= SIZE_Y-2)return 0;

  min_dist = 1e100;
  nearest = 0;
  
  //-1 から1じゃなくていいのか？
  for(i = -1;i <= 1; i++){
    for(j = -1;j <= 1; j++){
      //テーブルに登録されている点に大して距離を計算
      for(k = 0; k <g_points_table[ix+i][iy+j].num; k++){

	//テーブルからID読み込み
	n = g_points_table[ix+i][iy+j].points[k];
	
	//距離を計算
	dist = (g_points[n].x - x)*(g_points[n].x - x)
	  + (g_points[n].y - y )*(g_points[n].y - y );

	//最小値を計算
	if(dist < min_dist ){
	  min_dist = dist;
	  nearest = n;
	}
      }
    }
  }

  //平方根をとって最小値を出力
  return sqrt(min_dist);
}


//障害物をテーブルに登録
int regist_points(void){
  double min_dist;
  int j,min_id,edge_num;
  char edge[1200];
  double bx,by;
  double dist,d;

  //エッジおよびロボットのサイズ未満になるような間隔に点を間引く
  min_dist = 1e100;
  min_id = 0;

  edge[0]=0;
  //順方向
  for(j = 1;j < g_points_num;j++){ 
    edge[j]= 0;

    //距離計算
    dist = g_points[j].x*g_points[j].x+g_points[j].y*g_points[j].y;
    if(min_dist > dist){
      min_dist = dist;
      min_id = j;
    }

    d = (g_points[j-1].x - g_points[j].x)*(g_points[j-1].x - g_points[j].x)
      + (g_points[j-1].y - g_points[j].y)*(g_points[j-1].y - g_points[j].y);

    if(d > ROBOT_RADIUS*ROBOT_RADIUS ){
      edge[j]=2;
      edge[j-1]=2;
      min_dist = 1e100;
    }
    if(min_id == j-10){
      edge[min_id]++;
      min_dist = 1e100;
    }
  }
  //逆方向
  min_dist = 1e100;
  for(j = g_points_num-2;j >= 0;j--){ 
    dist = g_points[j].x*g_points[j].x+g_points[j].y*g_points[j].y;
    if(min_dist >= dist){
      min_dist = dist;
      min_id = j;
    }
    if(min_id == j+10 ){
      edge[min_id]++;
      min_dist = 1e100;
    }
  }
  
  //エッジをテーブルに登録
  bx = 0;
  by = 0;
  edge_num = 0;

  for(j = 0;j < g_points_num; j++){ 
    d = (bx - g_points[j].x)*(bx - g_points[j].x)
      + (by - g_points[j].y)*(by - g_points[j].y);
    if(edge[j]>=2  //順方向と逆方向どちらでもエッジであった、または
       || d > ROBOT_RADIUS*ROBOT_RADIUS //間隔がロボットの半径以上であるか
       ){
      add_point_table(j);//登録
      bx = g_points[j].x;
      by = g_points[j].y;
      edge_num++;
    }
  }
  
  return edge_num;
}


/*------------------------------------------------
 *                 functions
 *------------------------------------------------*/
/*int  main(int argc ,char *argv[])
{
  int j;

  SSM_sid URG_sid;
  URG_data urg;
  URG_property urg_prop;
  double urg_time;
  FILE *range_file,*gnuplot,*circle_file,*peak_file;
  int id,i,k,min_id;
  char file_name[30],edge[1200];
  Point points[1200];
  double time,dist[5][20];
  double o,x,y,theta,v,w,d;
  double bx,by,min_dist;
  
  gnuplot = popen("/usr/bin/gnuplot","w");
  fprintf(gnuplot,"set xrange [-4:4]\n");
  fprintf(gnuplot,"set yrange [-2:4]\n");
  fprintf(gnuplot,"set term x11\n");

  if(argc > 1)id = strtol(argv[1],0,10);

  initSSM();
  URG_sid = openSSM(URG_NAME,id,0);  
  get_propertySSM(URG_NAME,id,(char*)&urg_prop);  

  // printf("%f %d-%d\n",urg_prop.res,urg_prop.amin,urg_prop.amax);  
  sprintf(file_name,"range%d",id);

  while(1){
    readSSM(URG_sid, (char*)&urg, &urg_time, -1);
    
    init_table(g_point_table);
    range_file =fopen(file_name,"w");
    peak_file =fopen("peak","w");
    //printf("%d %d\n",urg_prop.amin,  urg_prop.amax);
    

    for(j = urg_prop.amin;j < urg_prop.amax;j++){ 
      points[j].x = urg.data[j]*cos((double)(j-urg_prop.afrt)*urg_prop.res*M_PI/180.0)/1000.0;
      points[j].y = urg.data[j]*sin((double)(j-urg_prop.afrt)*urg_prop.res*M_PI/180.0)/1000.0;

      add_point(points[j].x,points[j].y);
      fprintf(range_file,"%f %f\n0 0\n\n",
	      points[j].x, points[j].y);     
    }
    time = gettimeSSM();

    points_regist();

    fclose(range_file);
    fclose(peak_file);


    for(i=0,v=0.0;i<5;i++,v+=0.2){
      for(j=0,w=-0.2;j<20;j++,w+=0.02){
	x = 1;
	y = 0;
	theta=0;
	d =0;
	for(k = 0;k < 50;k++){
	  x+= v*cos(theta)*0.1;
	  y+= v*sin(theta)*0.1;
	  theta+= w*0.1;
	  d += v*0.1;  
	  if(search_min_dist(x,y)<0.3)break;
	}
	dist[i][j] = d; 
      }
    }
    printf("%f\n",(gettimeSSM()-time)*1000.0);
   
    circle_file =fopen("circle","w");
    for(i=0,v=0.0;i<5;i++,v+=0.2){
      for(j=0,w=-0.2;j<20;j++,w+=0.02){
	x = 1;
	y = 0;
	theta=0;
	d =0;
	for(d = 0;d < dist[i][j];d+=0.1){
	  x+= v*cos(theta)*0.1;
	  y+= v*sin(theta)*0.1;
	  theta+= w*0.1;
	  fprintf(circle_file,"%f %f\n",x,y);
	}
	fprintf(circle_file,"\n");
      }
    }

    fclose(circle_file);
    
    //fprintf(gnuplot,"plot 'range%d', 'range%d' w l,'circle' w l\n",id,id);
    fprintf(gnuplot,"plot 'range%d','peak' w l,'circle' w l\n",id);
    fflush(gnuplot);
    
    usleep(50000);
  }
  
  pclose(gnuplot);   
  
  return 1;
} 
*/
