#import rospy
import tkinter as tk
import numpy as np
import ruamel.yaml
from PIL import Image, ImageTk
from pathlib import Path


#===== Applicationクラスの定義　tk.Frameクラスを継承 =====#
class Application(tk.Frame):

	#--- コンストラクタ ---
	# プログラムを実行したときに最初にすべき処理を全て記述する
	def __init__(self, master):
		super().__init__(master)   # スーパークラスのコンストラクタを実行
		self.master.title("Waypoints Manager")

		## 画面上部のメニューを作成
		self.menu_bar = tk.Menu(self)   # メニューバーを配置
		self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF)   # バーに追加するメニューを作成
		self.menu_bar.add_cascade(label="File", menu=self.file_menu)   # Fileメニューとしてバーに追加
		self.file_menu.add_command(label="Save",   # FileメニューにSaveコマンドを追加
									command=self.menu_save_clicked,   #コールバック関数を設定
									accelerator="Ctrl+S"# 右側に表示するキーボードショートカット
									)   
		self.file_menu.add_command(label="Save As",   # 同様にSave Asコマンドを追加
									command=self.menu_saveas_clicked,
									accelerator="Ctrl+Shift+S"
									)
		self.bind_all("<Control-s>", self.menu_save_clicked)   #キーボードショートカットを設定
		self.bind_all("<Control-Shift-S>", self.menu_saveas_clicked)
		self.master.config(menu=self.menu_bar)   # 大元に作成したメニューバーを設定

		## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止
		self.__map_img_pil, self.__map_yaml = self.get_map_info()
		self.__waypoints = self.get_waypoints()

		## canvasを配置
		self.canvas = tk.Canvas(self.master, background="#a0a0a0")   # 画像を描画するcanvas
		self.canvas.pack(expand=True, fill=tk.BOTH)   # canvasを配置
		self.update()   # 情報の更新をする(canvasのサイズなどの情報が更新される)

		## 画像をcanvasのサイズにフィッティングして描画
		self.canv_w = self.canvas.winfo_width()   # canvasの幅を取得
		self.canv_h = self.canvas.winfo_height()   # canvasの高さを取得
		self.mat_affine = np.eye(3)
		scale = 1
		offset_x = 0
		offset_y = 0
		if (self.canv_w / self.canv_h) > (self.__map_img_pil.width / self.__map_img_pil.height):
			# canvasの方が横長　画像をcanvasの縦に合わせてリサイズ
			scale = self.canv_h / self.__map_img_pil.height
			offset_x = (self.canv_w - self.__map_img_pil.width*scale) / 2
		else:
			# canvasの方が縦長　画像をcanvasの横に合わせてリサイズ
			scale = self.canv_w / self.__map_img_pil.width
			offset_y = (self.canv_h - self.__map_img_pil.height*scale) / 2
		
		self.mat_affine = self.scale_mat(scale, self.mat_affine)
		self.mat_affine = self.translate_mat(offset_x, offset_y, self.mat_affine)
		self.draw_img_tk = self.draw_image(self.mat_affine)

		## 右クリックしたときに表示するポップアップメニューを作成
		self.popup_menu = tk.Menu(self, tearoff=tk.OFF)
		self.popup_menu.add_command(label="add waypoint", command=self.add_waypoint)
		self.right_click_coord = None   # 右クリックしたときの座標を保持する変数

		## マウスイベントを設定
		self.master.bind("<MouseWheel>", self.mouse_wheel)
		self.master.bind("<B1-Motion>", self.left_click_move)
		self.master.bind("<Button-1>", self.left_click)
		self.master.bind("<Button-3>", self.right_click)

		## ウィンドウに関するコールバック
		# self.master.bind("<Configure>", self.window_resize_callback)

		self.plot_origin()
		return


	#--- 地図上の原点に円を描画する ---
	def plot_origin(self):
		origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの
		return 



	#--- mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む ---
	#--- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように ---
	def get_map_info(self):
		map_path = Path('..','..','waypoint_nav','maps','map')   # .pgmと.yamlの手前までのパス
		map_img_pil = Image.open(Path(str(map_path)+'.pgm'))   # .pgmをplillowで読み込む
		with open(str(map_path)+'.yaml') as file:   # .yamlを読み込む
			map_yaml = ruamel.yaml.YAML().load(file)
		
		return map_img_pil, map_yaml    # この2つの変数を戻り値とする
	


	#--- これもget_param()でパスを受け取り、読み込めるようにする ---
	def get_waypoints(self):
		file_path = Path('..','..','waypoint_nav','param','waypoints.yaml')
		with open(file_path) as file:
			waypoints = ruamel.yaml.YAML().load(file)
		return waypoints



	#--- マウスを左クリックしながらドラッグしたときのコールバック関数 ---
	def left_click_move(self, event):
		print("x=" + str(event.x) + "  y=" + str(event.y))
		return
	


	#--- 右クリックしたときのコールバック関数 ---
	def right_click(self, event):
		self.popup_menu.post(event.x_root, event.y_root)   # メニューをポップアップ
		self.right_click_coord = [event.x, event.y]   # クリックされた座標を変数に格納
		return



	#--- マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 ---
	#--- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも ---
	def mouse_wheel(self, event):
		if event.delta > 0:
			# 上に回転（タッチパッドなら下にドラッグ）=> 拡大
			print(event.x, event.y, event.delta)
		else:
			# 下に回転（タッチパッドなら上にドラッグ）=> 縮小
			print(event.x, event.y, event.delta)
		# 回転の向きに対するevent.deltaの正負はプラットフォームにより異なる可能性あり
		return
	


	#--- 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 ---
	def add_waypoint(self):
		print("Clicked \"add waypoint\"")
		return



	#--- Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 ---
	def menu_save_clicked(self, event=None):
		print("Clicked \"Save\"")
		return
	


	#---  Fileメニューの"Save As"がクリックされたときに実行されるコールバック関数 ---
	def menu_saveas_clicked(self, event=None):
		print("Clicked \"Save As\"")
		return
	


	#--- 左クリックされたときに実行されるコールバック関数 ---
	def left_click(self, event):
		self.popup_menu.unpost()

	

	#--- mat_affineにx,yの平行移動を加えた同次変換行列を返す ---
	def translate_mat(self, x, y, mat_affine):
		mat = np.eye(3)
		mat[0, 2] = float(x)
		mat[1, 2] = float(y)
		return np.dot(mat, mat_affine)
	
	#--- mat_affineにscale倍のリサイズを加えた同次変換行列を返す ---
	def scale_mat(self, scale, mat_affine):
		mat = np.eye(3)
		mat[0, 0] = scale
		mat[1, 1] = scale
		return np.dot(mat, mat_affine)

	
	def draw_image(self, mat_affine):
		mat_inv = np.linalg.inv(mat_affine)
		img = self.__map_img_pil.transform(
				(self.canv_w, self.canv_w),
				Image.Transform.AFFINE, tuple(mat_inv.flatten()),
				Image.Resampling.NEAREST,
				fillcolor = 160
				)
		tk_img = ImageTk.PhotoImage(image=img)
		self.canvas.create_image(0, 0, anchor='nw', image=tk_img)   # 画像の描画
		return tk_img


	#--- 
	def window_resize_callback(self, event):
		cw = self.canvas.winfo_width()
		ch = self.canvas.winfo_height()
		if (self.canv_w != cw) or (self.canv_h != ch):
			self.canv_w = cw
			self.canv_h = ch
			self.canvas.delete("all")
			self.canvas.create_image(self.canv_w/2, self.canv_h/2, image=self.draw_img_tk)
		return




#===== メイン処理　プログラムはここから実行される =====#
if __name__ == "__main__":
	#rospy.init_node("manager_GUI")
	root = tk.Tk()   # 大元になるウィンドウ
	w, h = root.winfo_screenwidth(), root.winfo_screenheight()
	root.geometry("%dx%d+0+0" % (w, h))
	app = Application(master=root)   # tk.Frameを継承したApplicationクラスのインスタンス
	app.mainloop()