###################################################
###パッケージの読み込み######
###################################################

import time
import numpy as np
from scipy.stats import rankdata
#from numpy import *
import pandas as pd
import itertools
import math

start_time = time.time()

###################################################
###データ読み込み######
###################################################

# ラベル付き隣接行列
"""
m = [
    [0, 142.2064614, 0, 89.68804473, 0, 0, 0, 0, 0],
    [142.2064614, 0, 314.46613, 0, 204.8164485, 0, 0, 0, 0],
    [0, 314.46613, 0, 0, 0, 81.37164805, 0, 0, 0],
    [89.68804473, 0, 0, 0, 287.8825196, 0, 373.8821118, 0, 0],
    [0, 204.8164485, 0, 287.8825196, 0, 201.4898183, 0, 502.5906807, 0],
    [0, 0, 81.3716480459089, 0, 201.4898183, 0, 0, 0, 496.0501523],
    [0, 0, 0, 373.8821118, 0, 0, 0, 239.2130025, 0],
    [0, 0, 0, 0, 502.5906807, 0, 239.2130025, 0, 129.1523508],
    [0, 0, 0, 0, 0, 496.0501523, 0, 129.1523508, 0],
]
"""
# csvに書き込んでもよいです 量が多いときはこれ自体をコード書いて処理してつくるのがよいでしょう
m = np.genfromtxt( "C:/Users/deded/Documents/最短経路探索/shibuyamatrix.csv", delimiter="," , filling_values=(0) ) #隣接行列の読み込み

# ノード数は隣接行列の要素数です
m_node = len(m)

# OD表読み込み
OD = pd.DataFrame(np.loadtxt( "C:/Users/deded/Documents/最短経路探索/OD.csv", delimiter=","))
OD.columns = ["O", "D"]

Olist = OD['O']
Dlist = OD['D']

# 各ノードの緯度経度読み込み
position = pd.DataFrame(np.loadtxt( "C:/Users/deded/Documents/最短経路探索/Node_position.csv", delimiter=","))
position.columns = ["Lat","Lon"]

Latlist = position['Lat']
Lonlist = position['Lon']

###################################################
###定義######
###################################################

# ネットワークｍにおいて出発地oriから目的地desまでの最短経路を探していきます
def astar(m,ori,des):
    num = len(m)  # グラフのノード数
    closedset = [] #計算済みのノードリスト
    openset = [ori-1] #計算中ノードを格納しておくためのリスト，最初は始点を入れる
    camefrom = {} #それぞれのノードについてどのノードから最も効率的にたどり着けるか
    g_score = [float("inf") for i in range(num)] #スタートからある点までの距離
    g_score[ori-1] = 0 #スタートまでの距離は0
    f_score = {} #A*で用いる距離の推定値
    f_score[ori-1] = g_score[ori-1] + heuristic_cost_estimate(ori-1, des-1)
    while openset:
        current = min((f_score[node], node) for node in openset)[1]
        if current == des-1:
            return (reconstruct_path(camefrom,current),g_score[des-1])
        openset.remove(current)
        closedset.append(current)

        for neighbor in neighbor_nodes(current):
            if neighbor in closedset:
                continue
            if neighbor not in openset:
                openset.append(neighbor)
            tentative_g_score = g_score[current] + m[current][neighbor]
            if tentative_g_score >= g_score[neighbor]:
                continue
            camefrom[neighbor] = current
            g_score[neighbor] = tentative_g_score
            f_score[neighbor] = g_score[neighbor] + heuristic_cost_estimate(neighbor,des-1)
    return False


def reconstruct_path(came_from, current_node):
    path = [current_node + 1]
    while current_node in came_from:
        current_node = came_from[current_node]
        path.append(current_node + 1)
    return list(reversed(path))

def neighbor_nodes(p):
    neighbors = []
    for i in range(m_node):
        if m[p][i] != 0:
            neighbors.append(i)
    return neighbors

def heuristic_cost_estimate(p1, p2):
    return manhattan_distance(p1, p2)

def manhattan_distance(p1, p2):
    return abs(Latlist[p1]-Latlist[p2]) + abs(Lonlist[p1]-Lonlist[p2])

# 実行
for v in range(len(Olist)):

    origin = int(Olist[v])
    destination = int(Dlist[v])

    print(origin, destination)
    print(astar(m,origin,destination))

execution_time = time.time() - start_time
print(execution_time)