/*
  gcc -g vector_select.cpp -o vector_select
  gcc -g vector_select.cpp -o vector_select -Wl,--subsystem,console -lGrWin -mwindows
*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h> // usleep()
#include <math.h> // usleep()

#include <float.h> // double型の最大値 #define DBL_MAX を使いたいに為にインクルード
#include <GrWin.h>

/* 
   このプログラムは、複数の沢山ある経路(ベクトル)が存在している時に、
   ある出発点と終着点(ベクトル)を与えた時に、沢山ある経路を繋いで、
   出発から終着までを繼ぐ複数の経路を選びだすプログラムである

   ベクトルは、緯度、軽度、時間 の出発と到着を示す6要素で示される
   3次元ベクトルである。

   現時点のこのプログラムでは、緯度、軽度、時間は100x100x100の
   格子状のいずれかの2点を結んで、ベクトルとしている。

   プログラムは以下のようになっている。

   [Step.1] まず1000〜5000くらの経路ベクトルを乱数で作る

   [Step.2] 出発と到着のベクトル(旅程ベクトル)を一つ作る(ここでは乱数で)

   [Step.3] 旅程ベクトルに近い経路ベクトルを一つ選ぶ

   [Step.4] 旅程ベクトルの両端のベクトル(ここでは2つできる)を新しい旅程
   ベクトルとして、経路ベクトルを選ぶ→ これを、選べる経路ベクトルがなく
   なるまで、繰り返す。

   [Step.5] このようにして選び出された経路ベクトルを、出発から近い順番に
   表示する
  
   高速化と省メモリもあるが、それ以上に、最終的に経路ベクトルがいくつ
   選択されるのか分からないので、すべて動的メモリを使っている。

   その為、処理が恐しく面倒くさい

   それと、ベクトルが視認できないと、訳が分からんので、とても簡単な3D
   ルーチンも作ってある。

   「こんなもの、もう二度と作りたくないので、今のうちに退避しておく」
*/


// 経路ベクトル、旅程ベクトルの構造体
typedef struct VECTOR{
  double lon_s;
  double lat_s;
  double time_s;
  double lon_e;
  double lat_e;
  double time_e;
  double checkflag;  // チェックフラグ
  struct VECTOR *parent; /* 親の構造体を示すポインタ(自分の上) */
  struct VECTOR *prev;   /* 前の構造体を示すポインタ */
  struct VECTOR *next;   /* 次の構造体を示すポインタ */
} VECTOR;


// 3次元座標の構造体
typedef struct D3{
  double x;
  double y;
  double z;
} D3;

// 3次元表示用のクラス
class Coordinate
{
  
public:
  double Xf, Yf, Zf;   // 私の目のある場所(固定)
  double Xa, Ya, Za;   // 私が見ている一点(固定)
  double Xw, Yw, Zw;   // ある点の座標(変数)

  Coordinate(D3 f, D3 a){
    Xf = f.x; Yf = f.y; Zf = f.z;   // 私の目のある場所(固定)
    Xa = a.x; Ya = a.y; Za = a.z;   // 私が見ている一点(固定) 
  }
  
  void d3_to_d2_henkan(double x, double y, double z, 
                       double *Xe_dash, double *Ye_dash);  // その点が私の目の前のどのあたりに映るか
  
};

void Coordinate::d3_to_d2_henkan(double x, double y, double z, 
                                 double *Xe_dash, double *Ye_dash)  // その点が私の目の前のどのあたりに映るか
{
  /*
    3次元のある一点、例えば (Xf, Yf, Zf) = (-50,-60,170)から
    3次元の別のある一点 例えば(Xa, Ya, Za) = (100,110, 0)を向いた時、
    
    私の目に映る座標(例えば、(Xw, Yw, Zw)=(1,2,3)が
    どのような2次元座標上に見えるか
    
    という計算をしている(つもり)。 
    
    バグと思ったら、ここで確認する
    http://www.geocities.co.jp/SiliconValley-Bay/4543/Rubic/Mathematics/Mathematics-5.html
  */


  Xw = x; Yw = y; Zw = z;   // ある点の座標(変数)
  
  double a = sqrt((Xf-Xa)*(Xf-Xa)+(Yf-Ya)*(Yf-Ya));
  double b = sqrt(a*a+(Zf-Za)*(Zf-Za));
  
  double sin_theta = (Yf-Ya)/a;
  double cos_theta = (Xf-Xa)/a;
  
  double sin_phi = (Zf-Za)/b;
  double cos_phi = a/b;
  
  double Xe = -sin_theta*(Xw-Xf) + cos_theta*(Yw-Yf) + 0.0*(Zw-Zf);
  double Ye = -sin_phi*cos_theta*(Xw-Xf)-sin_phi*sin_theta*(Yw-Yf)+cos_phi*(Zw-Zf);
  double Ze = -cos_phi*cos_theta*(Xw-Xf)-cos_phi*sin_theta*(Yw-Yf)-sin_phi*(Zw-Zf);
  
  // 最後に平面上の座標に変換
  
  *Xe_dash = Xe / Ze;
  *Ye_dash = Ye / Ze;
}

// 旅程ベクトルを参照して、1の経路ベクトルを選ぶアルゴリズム
// 選び方は、現段階は、単なるハミング距離
int select_vector(VECTOR* in_vector, VECTOR* top_vector, VECTOR* out_vector)
{
  int ret=0;

  VECTOR* p_vector =  top_vector;
  
  double min = DBL_MAX; // doubleの最大値を入力
  while (p_vector != NULL){

    double dummy = 0.0;   // doubleの最小値を入力
    
    dummy += pow((in_vector->lon_s - p_vector->lon_s),2.0);
    dummy += pow((in_vector->lat_s - p_vector->lat_s),2.0);
    // dummy += pow((in_vector->time_s - p_vector->time_s),2.0); // 時間は閾値としてのみ使う
    dummy += pow((in_vector->lon_e - p_vector->lon_e),2.0);
    dummy += pow((in_vector->lat_e - p_vector->lat_e),2.0);
    // dummy += pow((in_vector->time_e - p_vector->time_e),2.0); // 時間は閾値としてのみ使う
    
    if (dummy > 1000) dummy = DBL_MAX; // 十分にベクトルが近くなければ、ダメ

    if (in_vector->time_s - p_vector->time_s > 0.0) dummy = DBL_MAX; // スタート時間より早く出発する移動手段は却下する
    if (p_vector->time_e - in_vector->time_e > 0.0) dummy = DBL_MAX; // 到着時間より遅く到着する移動手段は却下する
    
    // printf("dummy[%d]=%f\t\n",i, dummy);
    
    if (dummy < min) {
      min = dummy;
      memcpy((char *)out_vector,(char *)p_vector, sizeof(VECTOR));
      out_vector->checkflag = 0; // (念のため)チェックフラグを書き変えておく
      ret = 1;
    }
    
    p_vector = p_vector->next;
  }

  return(ret);
}

// このプログラムは、旅程ベクトルを選び、その余りのベクトルから、別の旅程ベクトルを選び・・・と、
// 自分自身を呼び出す再帰呼出しルーチンになっている。
// つまり、メインルーチンで獲得したポインタから、ベクトルが芋蔓的に拾うことができる

VECTOR* function(VECTOR* p_parent_person_vector, VECTOR* p_person_vector, VECTOR* p_top_train_vector) 
{

  printf("p_person_vector->lon_s = %f\t\n",p_person_vector->lon_s);
  printf("p_person_vector->lat_s = %f\t\n",p_person_vector->lat_s);
  printf("p_person_vector->time_s = %f\t\n",p_person_vector->time_s);
  printf("p_person_vector->lon_e = %f\t\n",p_person_vector->lon_e);
  printf("p_person_vector->lat_e = %f\t\n",p_person_vector->lat_e);
  printf("p_person_vector->time_e = %f\t\n",p_person_vector->time_e);
  printf("\n");

  VECTOR* p_route_vector = (VECTOR *)malloc(sizeof(VECTOR));
  if(p_route_vector == NULL) {
    printf("メモリが確保できません\n");
    exit(EXIT_FAILURE);
  }

  if (select_vector(p_person_vector, p_top_train_vector, p_route_vector) == 0){
    free(p_route_vector); // 確保したメモリを解放して
    printf("NULL happened\n\n");
    return NULL; // NULLを返す
  }
  
  // 親ベクターの入力
  p_route_vector->parent = p_parent_person_vector;

  //// これが(欲しい)ルートの計算結果
  printf("p_route_vector->lon_s = %f\t\n",p_route_vector->lon_s);
  printf("p_route_vector->lat_s = %f\t\n",p_route_vector->lat_s);
  printf("p_route_vector->time_s = %f\t\n",p_route_vector->time_s);
  printf("p_route_vector->lon_e = %f\t\n",p_route_vector->lon_e);
  printf("p_route_vector->lat_e = %f\t\n",p_route_vector->lat_e);
  printf("p_route_vector->time_e = %f\t\n",p_route_vector->time_e);
  printf("\n");
 
  // ここで差分計算してしまおう
  VECTOR lower_vector, upper_vector; // 両側のベクトル
  
  lower_vector.lon_s = p_person_vector->lon_s;
  lower_vector.lat_s = p_person_vector->lat_s;
  lower_vector.time_s = p_person_vector->time_s;
  lower_vector.lon_e = p_route_vector->lon_s;
  lower_vector.lat_e = p_route_vector->lat_s;
  lower_vector.time_e = p_route_vector->time_s;

  printf("lower_vector.lon_s = %f\t\n",lower_vector.lon_s);
  printf("lower_vector.lat_s = %f\t\n",lower_vector.lat_s);
  printf("lower_vector.time_s = %f\t\n",lower_vector.time_s);
  printf("lower_vector.lon_e = %f\t\n",lower_vector.lon_e);
  printf("lower_vector.lat_e = %f\t\n",lower_vector.lat_e);
  printf("lower_vector.time_e = %f\t\n",lower_vector.time_e);
  printf("\n");
  
  upper_vector.lon_s = p_route_vector->lon_e;
  upper_vector.lat_s = p_route_vector->lat_e;
  upper_vector.time_s = p_route_vector->time_e;
  upper_vector.lon_e = p_person_vector->lon_e;
  upper_vector.lat_e = p_person_vector->lat_e;
  upper_vector.time_e = p_person_vector->time_e;
  
  printf("upper_vector.lon_s = %f\t\n",upper_vector.lon_s);
  printf("upper_vector.lat_s = %f\t\n",upper_vector.lat_s);
  printf("upper_vector.time_s = %f\t\n",upper_vector.time_s);
  printf("upper_vector.lon_e = %f\t\n",upper_vector.lon_e);
  printf("upper_vector.lat_e = %f\t\n",upper_vector.lat_e);
  printf("upper_vector.time_e = %f\t\n",upper_vector.time_e);
  printf("\n");
  
  p_route_vector->prev = function(p_route_vector, &lower_vector, p_top_train_vector);
  p_route_vector->next = function(p_route_vector, &upper_vector, p_top_train_vector);
  
  return p_route_vector;
}


int main(void)
{
  D3 f, a;

  f.x = -25; f.y = -25; f.z = -25;
  a.x =  0;  a.y =  0;  a.z =  0;
  
  Coordinate co(f,a);
  
  int width = 800, height = 800;    /* ウィンドウサイズ440×400 */
  
  GWopen(0);    /* ウィンドウのオープン */
  GWsize(-5, &width, &height);    /* ウィンドウサイズ設定 */
  GWsize(-3, NULL, NULL);         /* フレーム(枠)サイズ設定 */
  GWvport(0.0, 0.0, (float)width / (float)height, 1.0);     /* ビューポート設定 */
  GWindow(-1, -1, 1, 1);
  
  GWsetpen(13,1,5,-1);
  
  double x1, y1, x2, y2;
  
  co.d3_to_d2_henkan(0,0,0, &x1,&y1);     
  co.d3_to_d2_henkan(100,0,0,&x2,&y2);     
  GWline(x1,y1,x2,y2);

  co.d3_to_d2_henkan(0,100,0,&x2,&y2);     
  GWline(x1,y1,x2,y2);

  co.d3_to_d2_henkan(0,0,100,&x2,&y2);     
  GWline(x1,y1,x2,y2);


  ///////// 電車ベクトルの取り扱い(ここから)

  int i;
  VECTOR  *p_train_vector, *p_prev_train_vector, *p_next_train_vector;
  VECTOR  *p_top_train_vector, *p_bottom_train_vector;

  for(i=0; i<1999; i++){  
    p_train_vector = (VECTOR *)malloc(sizeof(VECTOR));
    if(p_train_vector == NULL) {
      printf("メモリが確保できません %d\n",i);
      exit(EXIT_FAILURE);
    }

    if (i==0){   // 最初のベクトル(0番目) (特別扱い)
      p_top_train_vector = p_train_vector;
      p_top_train_vector->prev = NULL;  // 先頭ベクタの前のベクタは(当然)NULL
      p_prev_train_vector = p_train_vector;
    }

    /// データの搭載
    p_train_vector->lon_s = double(rand()%100);
    p_train_vector->lat_s = double(rand()%100);
    p_train_vector->time_s = double(rand()%100);
    p_train_vector->lon_e = double(rand()%100);
    p_train_vector->lat_e = double(rand()%100);
    p_train_vector->time_e = p_train_vector->time_s + double(rand()%100 + 1); 
    p_train_vector->checkflag  = 0;
    
    // (最後に)ポインタをリンクする
    p_prev_train_vector->next = p_train_vector;
    p_train_vector->prev = p_prev_train_vector;
    p_train_vector->next = NULL;
    p_prev_train_vector = p_train_vector;
  }
  
  p_bottom_train_vector = p_train_vector;  //最後の一人
  
  ///// 鉄道ベクトルの表示(テスト後消去)
  
  p_train_vector =  p_top_train_vector;
  
  GWsetpen(16,1,5,-1);// 青色
  
  do{

    /*
      printf("lon_s:%f\t\n", p_train_vector->lon_s);
      printf("lat_s:%f\t\n", p_train_vector->lat_s);
      printf("time_s:%f\t\n",p_train_vector->time_s);
      printf("lon_e:%f\t\n", p_train_vector->lon_e);
      printf("lat_e:%f\t\n", p_train_vector->lat_e);
      printf("time_e:%f\t\n",p_train_vector->time_e);
      printf("\n");
    */
    
    // ある点の座標(変数)
    co.d3_to_d2_henkan(p_train_vector->lon_s, p_train_vector->lat_s, p_train_vector->time_s, &x1,&y1);     
    co.d3_to_d2_henkan(p_train_vector->lon_e, p_train_vector->lat_e, p_train_vector->time_e, &x2,&y2);     
    GWline(x1,y1,x2,y2);  
    
    p_train_vector = p_train_vector->next; 
    
  } while( p_train_vector != NULL);
  
  ///// 旅客ベクタの入力(ここでは1つ) /////
  
  VECTOR  *p_person_vector, *p_prev_person_vector, *p_next_person_vector;
  VECTOR  *p_top_person_vector, *p_bottom_person_vector;
  
  // 最初のベクトル(0番目)
  p_person_vector= (VECTOR *)malloc(sizeof(VECTOR));
  if(p_person_vector == NULL) {
    printf("メモリが確保できません\n");
    exit(EXIT_FAILURE);
  }
  
  p_person_vector->lon_s = double(rand()%100);
  p_person_vector->lat_s = double(rand()%100);
  p_person_vector->time_s = double(rand()%100);
  p_person_vector->lon_e = double(rand()%100);
  p_person_vector->lat_e = double(rand()%100);
  p_person_vector->time_e = p_person_vector->time_s + double(rand()%100 + 1); 

  p_top_person_vector = p_person_vector; // personベクタのプリミティブ
  
  // ある点の座標(変数)
  co.d3_to_d2_henkan(p_person_vector->lon_s, p_person_vector->lat_s, p_person_vector->time_s, &x1,&y1);     
  co.d3_to_d2_henkan(p_person_vector->lon_e, p_person_vector->lat_e, p_person_vector->time_e, &x2,&y2);     
  GWsetpen(17,1,10,-1);// ピンク色
  GWline(x1,y1,x2,y2);  

  ///// 旅客ベクタの入力(ここまで) /////
  

  ///// 旅程ベクタの算出
  VECTOR* top_route_vector;
  top_route_vector = function(NULL, p_person_vector, p_top_train_vector);
  ///// 旅程ベクタの算出 終了

  VECTOR* p_route_vector;

  printf("もう一つのやり方\n");

  top_route_vector->parent = NULL; 
  p_route_vector = top_route_vector;  

  do{
    do{
      // 下向き
      while(p_route_vector->prev != NULL){
        p_route_vector = p_route_vector->prev;
      }
      
      // 枝のprevの最下位に到着したら、まずは印刷
      printf("lon_s:%f\t\n", p_route_vector->lon_s);
      printf("lat_s:%f\t\n", p_route_vector->lat_s);
      printf("time_s:%f\t\n",p_route_vector->time_s);
      printf("lon_e:%f\t\n", p_route_vector->lon_e);
      printf("lat_e:%f\t\n", p_route_vector->lat_e);
      printf("time_e:%f\t\n",p_route_vector->time_e);
      printf("\n");

      co.d3_to_d2_henkan(p_route_vector->lon_s, p_route_vector->lat_s, p_route_vector->time_s,&x1,&y1);
      co.d3_to_d2_henkan(p_route_vector->lon_e, p_route_vector->lat_e, p_route_vector->time_e,&x2,&y2);     
      GWsetpen(18,1,10,-1);// 色
      GWline(x1,y1,x2,y2);  
      
      p_route_vector->checkflag = 1; // 書き出したらフラグを立てる
      
      // 最下位の枝にnextがあれば続ける
      if (p_route_vector->next != NULL){
        p_route_vector = p_route_vector->next;
      }
      else{
        break; // ここより下はないので、抜ける
      }
    }while(1);
    
    // ここから登り方向
    
    do{
      do{
        if (p_route_vector->parent == NULL){
          exit(0);
        }
        
        // 上向き  
        p_route_vector = p_route_vector->parent;
        
      }while (p_route_vector->checkflag == 1);
      
      /////v_print(p_route_vector); // 登りながら表示    
      printf("lon_s:%f\t\n", p_route_vector->lon_s);
      printf("lat_s:%f\t\n", p_route_vector->lat_s);
      printf("time_s:%f\t\n",p_route_vector->time_s);
      printf("lon_e:%f\t\n", p_route_vector->lon_e);
      printf("lat_e:%f\t\n", p_route_vector->lat_e);
      printf("time_e:%f\t\n",p_route_vector->time_e);
      printf("\n");
      
      co.d3_to_d2_henkan(p_route_vector->lon_s, p_route_vector->lat_s, p_route_vector->time_s,&x1,&y1);
      co.d3_to_d2_henkan(p_route_vector->lon_e, p_route_vector->lat_e, p_route_vector->time_e,&x2,&y2);     
      GWsetpen(18,1,10,-1);// 色
      GWline(x1,y1,x2,y2);  
      
      p_route_vector->checkflag = 1;// 書き出したらフラグを立てる
      
      // p_route_vector->next が NULLか、p_route_vector->checkflag が"1"なら、さらに
      
    }while( p_route_vector->next == NULL ); 
    
    p_route_vector = p_route_vector->next;
    
  }while(1);
  
}