/*
  gcc -g wf5.cpp -o wf5
 
  ワーシャル-フロイド法を使った複数人数を拾う経路計画(その5)
 
  参考：ゼオス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);
	}
  }
 
 
  // では、ここで   N(13):出発値 A(0),D(3),H(7),K(10)の最短距離を求めるアルゴリズムを考えてみる
  // 総当たり戦でいくなら、さほど難しくはない
  // バス、タクシー程度であれば、スケールはあまり考えなくても良い
 
  int p[] = {13, 12, 0, 8, 3};
  int res_p[] = {-1, -1, -1, -1, -1};
 
  double dis_old = 9999.9;
  double dis_new;
 
  int i,j,k,m;
  double dis_i, dis_k, dis_j, dis_m;
 
  for (i = 1; i < 5; i++){
	dis_i = d[p[0]][p[i]];
 
	for (k = 1; k < 5; k++){
	  if (k == i){
		continue;
	  }
	  dis_k = d[p[i]][p[k]];
	  
	  for (j = 1; j < 5; j++){
		if ((j == k) || (j == i)){
		  continue;
		}
		dis_j = d[p[k]][p[j]];
 
		for (m = 1; m < 5; m++){
		  if ((m == k) || (m == i) || (m == j)){
			continue;
		  }
 
		  dis_m = d[p[j]][p[m]];
 
		  dis_new = dis_i + dis_k + dis_j + dis_m;
 
		  if (dis_new < dis_old){
			dis_old = dis_new;
			res_p[0] = 0;
			res_p[1] = i;
			res_p[2] = k;
			res_p[3] = j;
			res_p[4] = m;
 
			printf("dis_old = %f, i:%d k:%d j:%d m:%d \n", dis_old,i,k,j,m);
			printf("dis_old = %f, %d→%d→%d→%d→%d\n", dis_old,p[res_p[0]],p[res_p[1]],p[res_p[2]],p[res_p[3]],p[res_p[4]]);
 
		  }
		}
	  }
	}
  }
 
  printf("dis_old = %f, %d→%d→%d→%d→%d\n", 
		 dis_old,p[res_p[0]],p[res_p[1]],p[res_p[2]],p[res_p[3]],p[res_p[4]]);
 
  // dis_old = 13.563715, 13→0→3→9→12
 
 
  // 経路パス表示
 
  for (int n = 0; n < 4; n++){
 
	int i = p[res_p[n]];
	int k = p[res_p[n+1]];
	
	int cur;
	printf("d[%d][%d]: ",i,k);
	
	for (cur = i; cur != k; cur = next[cur][k]){
	  printf("%d -> ", cur);
	}
	printf("%d\n", k);
  }
 
}