/* map.c: compile with xcc地図描画  */
#include <stdio.h>
#include <math.h>
#include "Xtc.h"
#include "xtcextend.c"

#define CrossingNumber 100  /* 交差点数=100 */
#define MaxName         50  /* 最大文字数50文字(半角) */

/* 座標変換関数の定義 */

double W_X          = 900;   /* WINDOWSIZEを定義*/
double W_Y          = 1000;
double HW_X         = 450;   /* WINDOWSIZE/2を定義*/
double HW_Y         = 500;
double ORIGIN_X     = 450;   /* グローバル変数*/
double ORIGIN_Y     = 390;
double RR           = 1.0001;
double RR0          = 1.0001;
double ROT          = 0;
int passed[CrossingNumber];
int    T            = 0;
int    I            = 0;
int    start        = 0;
int    goal         = 0;
int    S            = -1;
int    G            = -1;
int    L            = 0;
#define R               10
#define ROT_X            0
#define ROT_Y            0
#define gx(x, y) ((int)(ORIGIN_X + ( (x - ROT_X) * cos(ROT/180.0*M_PI) - (y - ROT_Y) * sin(ROT/180.0*M_PI) )*50/RR))
#define gy(x, y) ((int)(ORIGIN_Y - ( (x - ROT_X) * sin(ROT/180.0*M_PI) + (y - ROT_Y) * cos(ROT/180.0*M_PI) )*50/RR))

typedef struct{
  double x,y;
}
Position;                /* 位置を表す構造体 */

/* データ構造の定義 */
typedef struct {
  int id;                /* 交差点番号 */
  Position pos;           /* 位置を表す構造体 */
  double wait;           /* 平均待ち時間 */
  char jname[MaxName];   /* 日本語交差点名 */    /* 名前が二つに*/
  char ename[MaxName];   /* ローマ字交差点名 */  /* なってます */
  int points;            /* 交差道路数 */
  int next[5];           /* 隣接する交差点番号 */
  double distance;       /* 基準交差点までの距離：追加 */
} Crossing;

/* データの宣言 */
Crossing cross[CrossingNumber];

/* ファイルの読み込み */
int map_read(char *filename)/* 今週の readfile.c 用意してあります  */
{
  FILE *fp;
  int i, j;
  int crossing_number;                          /* 交差点数 */

  fp = fopen(filename, "r");
  if (fp == NULL) {
    printf("File %s is not created\n", filename);
    return;
  }
  /* はじめに交差点数を読み込む */
  fscanf(fp, "%d", &crossing_number);
  if((crossing_number<1)||
     (crossing_number>=CrossingNumber))
    {
      printf("Illegal data number (%d)\n");
      return 0;
    }
  for (i = 0; i < crossing_number; i++) {
    fscanf(fp, "%d,%lf,%lf,%lf,%[^,],%[^,],%d",
   &(cross[i].id), &(cross[i].pos.x), &(cross[i].pos.y),
   &(cross[i].wait), cross[i].jname, cross[i].ename,
   &(cross[i].points));

    for(j=0; j<cross[i].points; j++)
      fscanf(fp, ",%d", &(cross[i].next[j]));
  }
  fclose(fp);
  return crossing_number;
}

/* アニメーションに必要な時間待ち関数(この通り作れば動きます) */
void uwaitloop(void)
{   xflush();   usleep(120000);    return ;  }

/* 距離を表示できるよう改造してます */
void print_cross(int i)
{  int j ;  printf("%s ( %s ),\n",cross[i].jname,cross[i].ename); }

void print_cross_list(int num)  /* num個表示 = 表示数制限可能 */
{  int i;for(i=0;i<num;i++) print_cross(i);  }

/* 交差点 a と b の間の「距離」を与える */
double distance(int a, int b)
{return hypot(cross[a].pos.x-cross[b].pos.x,cross[a].pos.y-cross[b].pos.y);}

/*ダイクストラ法*/
void dijkstra(int crossing_number,int target)
{
  int i,j,n,min_cross;
  double d,min_distance;          /* 例によって「最小」を探すための変数 */
  int done[CrossingNumber];     /* 確定済み:1 未確定:0 を入れるフラグ */

  for(i=0;i<crossing_number;i++)/* 初期化 */
    {
      cross[i].distance=1e100;  /* 初期値は有り得ないくらい大きな値 */
      done[i]=0;                /* 全交差点未確定 */
    }

  /* ただし、基準の交差点は 0 */
  cross[target].distance=0;
 
  for(i=0;i<crossing_number;i++) /* crossing_number回やれば終わるはず */
    {
      /* 最も距離数値の小さな未確定交差点を選定 → サーチのところ参照 */
      min_distance=1e100;
      for(j = 0 ; j < crossing_number ; j++)
{
  if(done[j]==0){
    if(cross[j].distance < min_distance){
      min_distance = cross[j].distance;
      min_cross = j;
    }
  }
}

      /* 交差点 min_cross は 確定できる */
      done[min_cross]=1;  /* 確定 */
      /* 確定交差点周りで距離の計算 */
      for(j=0;j<cross[min_cross].points;j++)
{
  n=cross[min_cross].next[j];    /* 長ったらしいので置き換え(だけ) */
  /* 評価指標「距離」 */
  d=distance(min_cross,n)+cross[min_cross].distance;
  /* 現在の暫定値と比較して、短いなら更新 */
  if(cross[n].distance > d)
    cross[n].distance = d;
}
    }
}

/*最短経路検索*/
int pickup_path(int crossing_number,int start,int goal,
int path[],int maxpath)
{
  int c=start;         /* 現在いる交差点 */
  int i,j,n,k;
  double min_distance,D;
  int min_cross;
  path[0]=start;
  i=1;k=0;D=1e100;min_distance=1e100;
  c=start;             /* 現在値を start に設定 */
  while(c!=goal)
    {
     for(j=0;j < cross[c].points;j++)
       {
         k=cross[c].next[j];
         D = distance(c,k) + cross[k].distance;

         if(D<min_distance){
           min_distance=D;
           min_cross = cross[c].next[j];
         }
       }
      c=min_cross;     /* 次に進むべき交差点 */
      path[i]=c;
      i++;L++;
    }
  path[i]=-1;   /* おしまいのマーク */
  return 0;
}


/* 道路網の表示1 ( 新しく作成する部分 ) */
void map_show(int crossing_number)
{
  int i,j,t;
  double x0,y0,x,y,r,x1,y1,X,Y;
  for(i=0;i<crossing_number; i++)
    {  if(i!=G || i!=S ){
       x0=cross[i].pos.x; y0=cross[i].pos.y;
       setlinestyle(SOLID_LINE,4);
       setcolor(LIGHTGRAY);
       for(j=0;j<cross[i].points; j++)
{  x=cross[cross[i].next[j]].pos.x;
    y=cross[cross[i].next[j]].pos.y;
    line(gx(x0,y0),gy(x0,y0),gx(x,y),gy(x,y));
}
       setcolor(LIGHTRED);
       for(t=1; t < 5; t++)
{    circle(gx(cross[i].pos.x,cross[i].pos.y),gy(cross[i].pos.x,cross[i].pos.y),t);}
       setcolor(WHITE);
        outtextxy(gx(x0,y0),gy(x0,y0),cross[i].ename);
    }}uwaitloop();
       if(G>0)  { 
       setcolor(LIGHTMAGENTA)  ;
       x1=cross[G].pos.x;y1=cross[G].pos.y;
       outtextxy(gx(x1+0.5,y1+0.5),gy(x1+0.5,y1+0.5),"GOAL");
       for(r=0;r<7;r++)  {circle(gx(x1,y1),gy(x1,y1),r);}
       outtextxy(gx(x1,y1),gy(x1,y1),cross[G].ename);xflush();
     }
       if(S>0)  { 
       setcolor(LIGHTGREEN)  ;
       x1=cross[S].pos.x;y1=cross[S].pos.y;
       outtextxy(gx(x1+0.5,y1+0.5),gy(x1+0.5,y1+0.5),"START");
       for(r=0;r<7;r++)  {circle(gx(x1,y1),gy(x1,y1),r);}
       outtextxy(gx(x1,y1),gy(x1,y1),cross[S].ename);xflush();
     }
 
  uwaitloop();
}

/* 道路網の表示2 ( 新しく作成する部分 ) */
void map_show2(int crossing_number)
{
  int i,j,t;
  double x0,y0,x,y,r;
  for(i=0;i<crossing_number; i++)
    { 
       x0=cross[i].pos.x; y0=cross[i].pos.y;
       setlinestyle(SOLID_LINE,4);
       setcolor(DARKGRAY);
       for(j=0;j<cross[i].points; j++)
{  x=cross[cross[i].next[j]].pos.x;
    y=cross[cross[i].next[j]].pos.y;
    line(gx(x0,y0),gy(x0,y0),gx(x,y),gy(x,y));
}
setcolor(LIGHTRED);
       for(t=1; t < 4; t++)
{    circle(gx(cross[i].pos.x,cross[i].pos.y),gy(cross[i].pos.x,cross[i].pos.y),t);}

    }
}

/* 通過道路色がえ*/
void path_color()
{
  int i,n,j,r,k;
  double x1,x2,y1,y2,x0,y0,xe,ye,dh;dh=0.1;
  setwritemode( COPY_PUT );

 /*未通過道を灰色未通過点を赤*/
 for(j=T+1;j<I-1;j++){
  if(passed[j]>0){
    x1 = cross[ passed[j] ].pos.x ; y1 = cross[ passed[j] ].pos.y ;
    x2 = cross[passed[j+1]].pos.x ; y2 = cross[passed[j+1]].pos.y ;
    setcolor(LIGHTGRAY);setlinestyle(SOLID_LINE,5);
      line(gx(x2,y2),gy(x2,y2),gx(x1,y1),gy(x1,y1));
      for(r=0;r<5;r++){
setcolor(LIGHTBLUE);
circle(gx(x1,y1),gy(x1,y1),r);
}
  }
if(T==-1) uwaitloop();
 }

 for(j=0;j<T;j++){/*通過道をきいろで通過点を赤*/
  if(passed[j]>0){
    x1 = cross[ passed[j] ].pos.x ; y1 = cross[ passed[j] ].pos.y ;
    x2 = cross[passed[j+1]].pos.x ; y2 = cross[passed[j+1]].pos.y ;
    setcolor(YELLOW);setlinestyle(SOLID_LINE,5);
      line(gx(x2,y2),gy(x2,y2),gx(x1,y1),gy(x1,y1));
      for(r=0;r<5;r++){
setcolor(LIGHTCYAN);
circle(gx(x2,y2),gy(x2,y2),r);
      }
  }
 }
    x0 = cross[ passed[0] ].pos.x ; y0 = cross[ passed[0] ].pos.y ;
      setcolor(LIGHTGREEN) ;for(r=0;r<7;r++) circle(gx(x0,y0),gy(x0,y0),r);

 for(j=0;j<I;j++){/*最初と最後以外の地名しろと円あお*/
  if(passed[j]>0){
    x1 = cross[ passed[j] ].pos.x ; y1 = cross[ passed[j] ].pos.y ;
    x2 = cross[passed[j+1]].pos.x ; y2 = cross[passed[j+1]].pos.y ;
    if(j>0){
      if(j<I-1) { setcolor(WHITE);
        outtextxy(gx(x1+0.1,y1+0.1),gy(x1+0.1,y1+0.1),cross[passed[j]].ename);
      }
    }   
  }
if(T==-1) uwaitloop();

 }
    xe = cross[ passed[I-1] ].pos.x ; ye = cross[ passed[I-1] ].pos.y ;
    setcolor(LIGHTMAGENTA);  for(r=0;r<7;r++) circle(gx(xe,ye),gy(xe,ye),r);

  /*最初最後の地名とスターとゴール表示と特別円
    x0 = cross[ passed[0] ].pos.x ; y0 = cross[ passed[0] ].pos.y ;
    xe = cross[ passed[I-1] ].pos.x ; ye = cross[ passed[I-1] ].pos.y ;
   
      for(r=0;r<7;r++){
        setcolor(LIGHTGREEN)  ; circle(gx(x0,y0),gy(x0,y0),r);
        setcolor(LIGHTMAGENTA); circle(gx(xe,ye),gy(xe,ye),r);
      }
     
    setcolor(LIGHTGREEN)  ; outtextxy(gx(x0,y0-0.4),gy(x0,y0-0.4),"START");
      outtextxy(gx(x0+0.1,y0-0.1),gy(x0+0.1,y0-0.1),cross[passed[0]].ename);
    setcolor(LIGHTMAGENTA); outtextxy(gx(xe,ye-0.4),gy(xe,ye-0.4),"GOAL");
      outtextxy(gx(xe+0.1,ye-0.1),gy(xe+0.1,ye-0.1),cross[passed[I-1]].ename);
      */
}

/*描画ルーチンまとめ*/
void draw(void){
    cleardevice();
    map_show2(90);
    path_color();
}

/*マウスから最短距離の点を返す*/
int search_point(int x,int y){
   int i;
   int f=-1;
   double x0,y0,d,d0,dm;
   dm=100000000;
  for(i=0;i<90; i++)
    { 
       x0=cross[i].pos.x; y0=cross[i].pos.y;
        d=hypot(gx(x0,y0)-x,gy(x0,y0)-y);
if(d<dm) {dm=d;f=cross[i].id;}
    }
return f;
}

/*地点入力*/
int search_cross(int num)
{
  int i,x,y,button;double x0,y0,r;
  int f= -1;int fff=-1;int F=0;int d =30;

  while(f==-1){
    if(xtc_is_left_button()!=0 || xtc_is_center_button()!=0
          || xtc_is_right_button()!=0 || F==1){
             cleardevice() ; map_show(90);xflush();F=0;
    }
if(xtc_is_left_button()!=0 && xtc_is_right_button()==0)
  { if(RR>0.049)RR-=0.04; F=1;}//printf("%d\n",xtc_is_left_button());}
if(xtc_is_center_button()!=0){
  fff = search_point(x,y);}
if(xtc_is_right_button()!=0 && xtc_is_left_button()==0){
  if(x>HW_X && y>HW_Y) {ORIGIN_X+=d;ORIGIN_Y+=d;}
  if(x>HW_X && y<HW_Y) {ORIGIN_X+=d;ORIGIN_Y-=d;}
  if(x<HW_X && y<HW_Y) {ORIGIN_X-=d;ORIGIN_Y-=d;}
  if(x<HW_X && y>HW_Y) {ORIGIN_X-=d;ORIGIN_Y+=d;}
F=1;
}
if(xtc_is_right_button()!=0 && xtc_is_left_button()!=0)
{if(RR<1.41) RR+=0.04;F=1;}     
if(xtc_is_left_button()!=0 && xtc_is_center_button()!=0
          && xtc_is_right_button()!=0) exit(0);
x=cur_pointer_x;y=cur_pointer_y;
if(fff>=0) f=fff;
  }
return f;
}


/* 隣接する2つの交差点を通る移動体を表示する */
void vehicle_unit_move(int from,int to,int crossing_number,int path[],int goal)
{
     double x,y,x0,y0,x2,y2,vx,r,vy,dx,dy,d,steps,H,ER,theta,ntheta,dtheta;
     int i,j,k,ss,minsteps,maxsteps; minsteps = 10 ; maxsteps = 24 ;
     dx = cross[ to ].pos.x - cross[ from ].pos.x; x = cross[ from ].pos.x;   
     dy = cross[ to ].pos.y - cross[ from ].pos.y; y = cross[ from ].pos.y;
     d  = hypot(dx,dy); if ( d > 3) d = 3; k = 6 ; H = 0.04 ; ER = 0.20 ;
     x0 = x ; y0 = y ; x2 = dx + x0 ; y2 = dy + y0 ;steps = 2 *(int)(k*d);
     if ( steps < minsteps) steps = minsteps ;
     if ( steps > maxsteps) steps = maxsteps ; ss = steps / 2;
     vx = dx/(double)steps;    vy = dy/(double)steps;
     theta = atan2(dx,dy)*180/M_PI ; dtheta = theta - ROT;
      if (dtheta >  180) dtheta-=360 ; if (dtheta < -180) dtheta+=360 ;
      ntheta = dtheta / ss; r=RR-ER*d;

    setwritemode( COPY_PUT );
    setcolor(LIGHTGREEN);
    for (j=0; j < R; j++  )    circle(gx(x,y),gy(x,y),j);   
    for (i=0; i < steps; i++)                     /* steps数だけ繰り返す */
      {
for(j=0; j < R; j++ )  circle(gx(x,y),gy(x,y),j);
path_color();
    setwritemode( COPY_PUT );
        setcolor(YELLOW);setlinestyle(SOLID_LINE,5);
         line(gx(x,y),gy(x,y),gx(x0,y0),gy(x0,y0));
        setcolor(GREEN);setlinestyle(SOLID_LINE,5);
         line(gx(x2,y2),gy(x2,y2),gx(x,y),gy(x,y));
      /* ・ORIGIN_X 等のパラメータを変更 マーカー座標の移動 */
x += vx;  y += vy;     
if(i<ss){RR-=r/ss;}
if(i < ss ) ROT      += ntheta;
          ORIGIN_X = HW_X - ( (x - ROT_X) * cos(ROT/180.0*M_PI) - (y - ROT_Y) * sin(ROT/180.0*M_PI) )*50/RR;
  ORIGIN_Y = HW_Y + ( (x - ROT_X) * sin(ROT/180.0*M_PI) + (y - ROT_Y) * cos(ROT/180.0*M_PI) )*50/RR;

        cleardevice();
crossing_number = map_read("map2.dat");
        map_show2(crossing_number);      
setcolor(LIGHTGREEN);
  for(j=0; j < R; j++)   circle(gx(x,y),gy(x,y),j);
path_color();
        setcolor(GREEN);setlinestyle(SOLID_LINE,5);
          line(gx(x2,y2),gy(x2,y2),gx(x,y),gy(x,y));
setcolor(YELLOW);setlinestyle(SOLID_LINE,5);
          line(gx(x,y),gy(x,y),gx(x0,y0),gy(x0,y0));
        uwaitloop();                           
      }
}

/* 与えられた交差点startからendまでマーカーを移動させる */
void vehicle_move_search(int path[],int crossing_number, int goal)
{
    int i,j,k,m,p,s,t;
    double x1,y1,x2,y2,xc,yc,xr,yr,xrc,yrc,real,rot,x,y;
    int current=path[0], next=path[1],c,start;
    m=k+1;t=0;

    for (i=0;path[i+1] != -1;i++)/* 目的地endに達しない限り繰り返す */

      {
  draw();
  next = path[i+1];
    /* マーカーの移動 */

  printf("→%s\n",cross[current].jname);
         vehicle_unit_move(path[i],path[i+1],crossing_number,path,goal);
T++;
  current = next;
      }
p=T;
        printf("目的地「%s」に到着しました！\n",cross[path[p]].jname);
      draw();
      real = RR - RR0 ; rot = ROT ; s = 20;
      x = cross[passed[T]].pos.x ; y=cross[passed[T]].pos.y ;
      x1=-x; y1=-y; xc = cross[passed[T]].pos.x ; yc=cross[passed[T]].pos.y ;
      if (ROT >  180) ROT-=360 ; if (ROT < -180) ROT+=360 ;
      for(j=0;j<s;j++){
        RR  -= real / s;
ROT -= rot /  s;
x += x1/s ; y += y1/s;

xr=cross[passed[0]].pos.x;yr=cross[passed[0]].pos.y;
xrc=(gx(xr,yr)+gx(xc,yc))/2;yrc=(gy(xc,yc)+gy(xr,yr))/2;
  ORIGIN_X = 450 - ( (x - ROT_X) * cos(ROT/180.0*M_PI) - (y - ROT_Y) * sin(ROT/180.0*M_PI) )*50/RR;
  ORIGIN_Y = 390 + ( (x - ROT_X) * sin(ROT/180.0*M_PI) + (y - ROT_Y) * cos(ROT/180.0*M_PI) )*50/RR;
          draw();
  uwaitloop();
      }
}

void mause(){
  int o=0;
   printf("\n左クリックすると開始します！\n");
   while(o!=256)  {o=xtc_is_left_button();}
}

/* メイン関数 */
int main(void)
{
    int i,j,r,ttt,crossing_number,path[25],F,b,B;/* 交差点数 */
    double x1,x2,y1,y2,x0,xe,y0,ye;
    char replay[200];
    initgraph();                              /* ウィンドウを開く */
    xtc_move_window(380,0);
    xtc_resize_window(W_X,W_Y);
    crossing_number = map_read("map2.dat");
    F=1;
while(F != -1){
    cleardevice();   T=0;I=0;goal=0;start=0;S=-1;G=-1;L=0;    map_show(90);
    for(i=0;i<crossing_number;i++){
      cross[i].distance=0;    /* 適当に初期化しておきます for print_cross */
      passed[i]=-1;
    }

  /* 目的地の取得 */
  printf("マウスを使って目的地を指定して下さい！\n");
  printf("注意！決定は１秒ほど長押しして下さい！\n");
  printf("左クリックのみ        →  拡大！\n");
  printf("右クリックのみ        →  地図を移動！\n");
  printf("中心クリックのみ      →  矢印に最も近い点を決定！\n");
  printf("左右クリック同時押し  →  縮小！\n");
  goal=search_cross(crossing_number);G=goal;
  if(goal<0)    return 1;    /* 目的地決定失敗 */
  cleardevice();map_show(90);uwaitloop();
  printf("\n目的地を「%s」に決定しました！\n処理中！\n",cross[goal].jname);
  dijkstra(crossing_number,goal);
sleep(2);
  printf("\nマウスを使って出発地を指定して下さい！\n");
  RR=RR0;ORIGIN_X=HW_X;ORIGIN_Y=390;
  cleardevice();map_show(90);uwaitloop();
  start=search_cross(crossing_number);S=start;
  if(start<0)    return 1;
  if(pickup_path(crossing_number,start,goal,path,25)<0)    return 1;
  printf("\n出発地を「%s」に決定しました！\n処理中！\n",cross[start].jname);

sleep(1);RR=RR0;ORIGIN_X=HW_X;ORIGIN_Y=390;
  cleardevice();map_show2(90);uwaitloop();

   i=0;
  while(path[i]>=0)
    {      passed[i]= cross[path[i]].id; i++ ; I++;}
  cleardevice();map_show2(90);uwaitloop();T=-1;
path_color();uwaitloop();T=0;
  /*
   for(j=0;j<I-1;j++){
    x1 = cross[ path[j] ].pos.x ; y1 = cross[ path[j] ].pos.y ;
    x2 = cross[path[j+1]].pos.x ; y2 = cross[path[j+1]].pos.y ;
    setcolor(LIGHTGRAY);setlinestyle(SOLID_LINE,5);
      line(gx(x2,y2),gy(x2,y2),gx(x1,y1),gy(x1,y1));
      setcolor(LIGHTBLUE);
      for(r=0;r<5;r++){ circle(gx(x1,y1),gy(x1,y1),r);
                        circle(gx(x2,y2),gy(x2,y2),r); }
    if(j>0){
      if(j<I-1) { setcolor(WHITE);
        outtextxy(gx(x1+0.1,y1+0.1),gy(x1+0.1,y1+0.1),cross[path[j]].ename);
      }
    }   
      for(r=0;r<7;r++){
        setcolor(LIGHTGREEN)  ; circle(gx(x0,y0),gy(x0,y0),r);
        setcolor(LIGHTMAGENTA); circle(gx(xe,ye),gy(xe,ye),r);
      }
    x0 = cross[ passed[0] ].pos.x ; y0 = cross[ passed[0] ].pos.y ;
    xe = cross[ passed[I-1] ].pos.x ; ye = cross[ passed[I-1] ].pos.y ;
    setcolor(LIGHTGREEN)  ; outtextxy(gx(x0,y0-0.4),gy(x0,y0-0.4),"START");
      outtextxy(gx(x0+0.1,y0-0.1),gy(x0+0.1,y0-0.1),cross[passed[0]].ename);
    setcolor(LIGHTMAGENTA); outtextxy(gx(xe,ye-0.4),gy(xe,ye-0.4),"GOAL");
      outtextxy(gx(xe+0.1,ye-0.1),gy(xe+0.1,ye-0.1),cross[passed[I-1]].ename);
     
      uwaitloop();
  }
*/
uwaitloop;
  mause();
    vehicle_move_search(path,crossing_number,goal);
//ttt=T;

    printf("\n左クリック→はじめから！\n右クリック→もう終りたい！");
b=0;B=0;
while(b==0 && B==0){
b=xtc_is_left_button();B=xtc_is_right_button();
     if(b != 0) printf("\nではもう一度\n処理中！\n");
     if(B != 0)          F = -1;
}
if(F==1) sleep(2);

  }
    printf("またの御利用をお待ちしております！\n");
    closegraph();                             /* ウィンドウを閉じる */
    return 0;



}