/*
  gcc -g wf4.cpp -o wf4
 
  ワーシャル-フロイド法(その4)
 
  参考：ゼオスTTのブログ
  http://zeosutt.hatenablog.com/entry/2015/05/05/045943
 
*/
 
#include <stdio.h>
#include <math.h>
#include <stdlib.h> 
 
typedef struct location{
  // ちなみに X,Y 軸座標は、→に＋  ↑に＋
  int num;
  char name[10];
  double longitude; // 経度 東経 139.691 X軸  
  double latitude;  // 緯度 北緯 35.698  Y軸 
} LOCATION;
 
LOCATION location[] = {
  {0,"A",35.896889, 139.966776},
  {1,"B",35.911303, 139.950609},
  {2,"C",35.902413, 139.950861},
  {3,"D",35.905665, 139.945430},
  {4,"E",35.891797, 139.959714},
  {5,"F",35.888019, 139.949214},
  {6,"G",35.887682, 139.945090},
  {7,"H",35.900693, 139.938919},
  {8,"I",35.898430, 139.932503},
  {9,"J",35.899640, 139.919513},
  {10,"K",35.893257, 139.934952},
  {11,"L",35.892819, 139.943374},
  {12,"M",35.887158, 139.922399},
  {13,"N",35.893778, 139.952453}
};
 
#define rad2deg(a) ((a)/M_PI * 180.0) /* rad を deg に換算するマクロ関数 */
#define deg2rad(a) ((a)/180.0 * M_PI) /* deg を rad に換算するマクロ関数 */
 
double distance_km(LOCATION* A, LOCATION* B, double *rad_up)
{
  double earth_r = 6378.137;
  
  double loRe = deg2rad(B->longitude - A->longitude); // 東西
  double laRe = deg2rad(B->latitude - A->latitude); // 南北
  
  double EWD = cos(deg2rad(A->latitude))*earth_r*loRe; // 東西距離
  double NSD = earth_r*laRe; //南北距離
  
  double distance = sqrt(pow(NSD,2)+pow(EWD,2));  
  *rad_up = atan2(NSD, EWD);
  
  return distance;
}
 
LOCATION* new_location(LOCATION* D, double diff_p_x, double diff_p_y) 
{
  double earth_r = 6378.137;
  
  double loRe = diff_p_x / earth_r / cos(deg2rad(D->latitude)); // 東西
  double laRe = diff_p_y / earth_r;  // 南北
  
  double diff_lo = rad2deg(loRe); // 東西
  double diff_la = rad2deg(laRe); // 南北
  
  static LOCATION C;
  
  C.longitude = D->longitude + diff_lo; // 東西
  C.latitude = D->latitude + diff_la; // 南北
  
  return &C;
}
 
double diff_longitude(double diff_p_x, double latitude) 
{
  double earth_r = 6378.137;
  // ↓ これが正解だけど、
  double loRe = diff_p_x / earth_r / cos(deg2rad(latitude)); // 東西
  // 面倒なので、これで統一しよう(あまり差が出ないしね)
  //double loRe = diff_p_x / earth_r / cos(deg2rad(35.700759)); // 東西
  double diff_lo = rad2deg(loRe); // 東西
  
  return diff_lo; // 東西
}
 
double diff_latitude(double diff_p_y) 
{
  double earth_r = 6378.137;
  double laRe = diff_p_y / earth_r;  // 南北
  double diff_la = rad2deg(laRe); // 南北
  
  return diff_la; // 南北
}
 
 
 
double d[14][14];  // d[i][k]：ノードiからノードkへの距離 
int next[14][14];  // ノードi から ノードj への最短経路における i の1つ後のノード
 
int main(void)
{
  // 変数の初期化
  for(int i = 0; i < 14; i++){
	for(int j = 0; j < 14; j++){
	  d[i][j] = 99999999.9; // 距離の初期化(でっかい値を入力しておく(INT_MAXは足し算の時に桁上がりが起こるので使わない)
	  next[i][j] = j; // ノードi から ノードj への最短経路における i の1つ後のノード
	}
  }
 
 
  // 確認用の表示
  printf("\n[STEP1]\n");
 
  for(int i = 0; i < 14; i++){
	for(int k = 0; k < 14; k++){
	  printf("d[%d][%d]):%f\t",i,k,d[i][k]);
	  printf("next[%d][%d]):%d\n",i,k,next[i][k]);
	}
  }
 
  //// ここからは実際の距離を手書き
 
  for(int i = 0; i < 14; i++){
	d[i][i] = 0; //// 同じノードへの距離は0になるので、上書き
  }
 
  double rad_up;
  d[0][1]  = distance_km(&location[0], &location[1], &rad_up);
  d[0][4]  = distance_km(&location[0], &location[4], &rad_up);
 
  d[1][0]  = distance_km(&location[1], &location[0], &rad_up);
  d[1][3]  = distance_km(&location[1], &location[3], &rad_up);
 
  d[2][3]  = distance_km(&location[2], &location[3], &rad_up);
  d[2][4]  = distance_km(&location[2], &location[4], &rad_up);
  d[2][13]  = distance_km(&location[2], &location[13], &rad_up);
 
  d[3][1]  = distance_km(&location[3], &location[1], &rad_up);
  d[3][2]  = distance_km(&location[3], &location[2], &rad_up);
  d[3][7]  = distance_km(&location[3], &location[7], &rad_up);
 
  d[4][0]  = distance_km(&location[4], &location[0], &rad_up);
  d[4][2]  = distance_km(&location[4], &location[2], &rad_up);
  d[4][13]  = distance_km(&location[4], &location[13], &rad_up);
 
  d[5][4]  = distance_km(&location[5], &location[4], &rad_up);
  d[5][6]  = distance_km(&location[5], &location[6], &rad_up);
  d[5][13]  = distance_km(&location[5], &location[13], &rad_up);
 
  d[6][5]  = distance_km(&location[6], &location[5], &rad_up);
  d[6][11]  = distance_km(&location[6], &location[11], &rad_up);
  d[6][12]  = distance_km(&location[6], &location[12], &rad_up);
 
  d[7][3]  = distance_km(&location[7], &location[3], &rad_up);
  d[7][8]  = distance_km(&location[7], &location[8], &rad_up);
  d[7][11]  = distance_km(&location[7], &location[11], &rad_up);
 
  d[8][7]  = distance_km(&location[8], &location[7], &rad_up);
  d[8][9]  = distance_km(&location[8], &location[9], &rad_up);
  d[8][10]  = distance_km(&location[8], &location[10], &rad_up);
 
  d[9][8]  = distance_km(&location[9], &location[8], &rad_up);
 
  d[10][8]  = distance_km(&location[10], &location[8], &rad_up);
  d[10][11]  = distance_km(&location[10], &location[11], &rad_up);
 
  d[11][6]  = distance_km(&location[11], &location[6], &rad_up);
  d[11][7]  = distance_km(&location[11], &location[7], &rad_up);
  d[11][10]  = distance_km(&location[11], &location[10], &rad_up);
 
  d[12][6]  = distance_km(&location[12], &location[6], &rad_up);
 
  d[13][2]  = distance_km(&location[13], &location[2], &rad_up);
  d[13][4]  = distance_km(&location[13], &location[4], &rad_up);
  d[13][5]  = distance_km(&location[13], &location[5], &rad_up);
 
  
  // 確認用の表示
  printf("\n[STEP2]\n");
 
  for(int i = 0; i < 14; i++){
	for(int k = 0; k < 14; k++){
	  printf("d[%d][%d]):%f\t",i,k,d[i][k]);
	  printf("next[%d][%d]):%d\n",i,k,next[i][k]);
	}
  }
 
  // 経路長計算
  for (int k =0; k < 14; k++){
	for (int i =0; i < 14; i++){
	  for(int j = 0; j < 14; j++){
		if(d[i][j] > d[i][k] + d[k][j]){
 
		  d[i][j] = d[i][k] + d[k][j];
		  next[i][j] = next[i][k]; //更新処理
		}
	  }
	}
  }
 
  // 計算結果
  printf("\n[STEP3]\n");
 
  for(int i = 0; i < 14; i++){
	for(int k = 0; k < 14; k++){
	  printf("d[%d][%d]):%f\t",i,k,d[i][k]);
	  printf("next[%d][%d]):%d\n",i,k,next[i][k]);
	}
  }
 
  // 経路パス表示
 
  printf("\n[Path]\n");
  for(int i = 0; i < 14; i++){
	for(int k = 0; k < 14; k++){
	  
	  int cur;
	  printf("d[%d][%d]: ",i,k);
	  
	  for (cur = i; cur != k; cur = next[cur][k]){
        printf("%d -> ", cur);
	  }
	  printf("%d\n", k);
	}
  }
}