#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)   # 大元に作成したメニューバーを設定

		## 右クリックしたときに表示するポップアップメニューを作成
		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   # 右クリックしたときの座標を保持する変数

		## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止
		self.__map_img_pil, self.__map_yaml = self.get_map_info()
		self.__waypoints = self.get_waypoints()
		self.current_waypoints = self.__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.scale_mat(scale)   # 同次変換行列を更新
		self.translate_mat(offset_x, offset_y)   # 同次変換行列を更新
		self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil)	# 描画する画像を保持する変数を初期化
		self.draw_image()   # 画像を描画

		## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画
		origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m)
		self.map_resolution = self.__map_yaml['resolution']
		self.img_origin = [-origin[0]/self.map_resolution, self.__map_img_pil.height+origin[1]/self.map_resolution, 1]
		self.plot_origin()

		## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画
		self.plot_waypoints()

		## マウスイベントに対するコールバックを設定
		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("<ButtonRelease-1>", self.left_click_release)
		self.master.bind("<Button-3>", self.right_click)
		## ウィンドウに関するコールバック
		self.master.bind("<Configure>", self.window_resize_callback)

		self.old_click_point = None
		return



	"""
	+++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++
	--- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように ---
	"""
	def get_map_info(self):
		map_path = Path('..','..','waypoint_nav','maps','map_gakunai')   # .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つの変数を戻り値とする
	


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



	"""
	+++++ 地図上の原点に円を描画する +++++
	"""
	def plot_origin(self):
		origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換
		r = 10   # 円の半径（ピクセル）
		x1 = origin_affine[0] - r
		y1 = origin_affine[1] - r
		x2 = origin_affine[0] + r + 1
		y2 = origin_affine[1] + r + 1
		if self.canvas.find_withtag("origin"):
			self.canvas.moveto("origin", x1, y1)
		else:
			self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue')
		return



	"""
	+++++ 地図上にウェイポイントを示す円を描画する +++++
	"""
	def plot_waypoints(self):
		return




	"""
	+++++ 左クリックされたときに実行されるコールバック関数 +++++
	"""
	def left_click(self, event):
		self.popup_menu.unpost()   # 右クリックで出るポップアップメニューを非表示
		# クリックした座標の近くにあるオブジェクトを取得
		clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20)
		if clicked_obj:   # オブジェクトがクリックされた場合
			tag = self.canvas.gettags(clicked_obj[0])[0]
			print("Clicked " + tag)
		return



	"""
	+++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++
	"""
	def left_click_move(self, event):
		if not self.old_click_point is None:
			delta_x = event.x-self.old_click_point[0]
			delta_y = event.y-self.old_click_point[1]
			if (np.linalg.norm([delta_x, delta_y]) < 300):
				self.translate_mat(event.x-self.old_click_point[0], event.y-self.old_click_point[1])
				self.draw_image()
				self.canvas.move("origin", delta_x, delta_y)   # self.plot_origin()
				self.plot_waypoints()
		self.old_click_point = [event.x, event.y]
		return
	


	"""
	+++++ マウスの左クリックを離したときのコールバック関数 +++++
	"""
	def left_click_release(self, event):
		self.old_click_point = None
		return



	"""
	+++++ 右クリックしたときのコールバック関数 +++++
	"""
	def right_click(self, event):
		# クリックした座標の近くにあるオブジェクトを取得
		clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20)
		if clicked_obj:   # 何かオブジェクトがクリックされていた場合
			return
		# クリックされた座標 => 元画像の座標 の変換
		mat_inv = np.linalg.inv(self.mat_affine)
		img_xy = np.dot(mat_inv, [event.x, event.y, 1])
		# 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている）
		if (img_xy[0] < 0) or (img_xy[1] < 0) or \
			(img_xy[0] > self.__map_img_pil.width) or (img_xy[1] > self.__map_img_pil.height):
			return
		self.popup_menu.post(event.x_root, event.y_root)   # メニューをポップアップ
		self.right_click_coord = img_xy[0:2]   # クリックされた元画像上の座標を変数に格納
		return



	"""
	+++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++
	"""
	def add_waypoint(self):
		print("Clicked \"add waypoint\"", self.right_click_coord)
		pix_xy = np.array(self.right_click_coord, np.uint32)
		print("value=",self.__map_img_pil.getpixel((pix_xy[0], pix_xy[1])))
		return
	


	"""
	+++++ マウスホイールを回転したとき（タッチパッドをドラッグしたとき）のコールバック関数 +++++
	--- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも ---
	"""
	def mouse_wheel(self, event):
		self.translate_mat(-event.x, -event.y)
		if event.delta > 0:
			# 上に回転（タッチパッドなら下にドラッグ）=> 拡大
			self.scale_mat(1.1)
		else:
			# 下に回転（タッチパッドなら上にドラッグ）=> 縮小
			self.scale_mat(0.9)
		self.translate_mat(event.x, event.y)
		self.draw_image()
		self.plot_origin()
		self.plot_waypoints()
		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
	



	"""
	+++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++
	"""
	def translate_mat(self, x, y):
		mat = np.eye(3)
		mat[0, 2] = float(x)
		mat[1, 2] = float(y)
		self.mat_affine = np.dot(mat, self.mat_affine)
		return
	


	"""
	+++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++
	"""
	def scale_mat(self, scale):
		mat = np.eye(3)
		mat[0, 0] = scale
		mat[1, 1] = scale
		self.mat_affine = np.dot(mat, self.mat_affine)
		return

	

	"""
	+++++ 元画像をaffne変換して描画、その画像を返す +++++
	"""
	def draw_image(self):
		mat_inv = np.linalg.inv(self.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
				)
		self.draw_img_tk = ImageTk.PhotoImage(image=img)   # 描画する画像を変数に保持
		if not self.canvas.find_withtag("map_image"):   # 初めて画像を描画するとき
			self.canvas.create_image(0, 0, anchor='nw', image=self.draw_img_tk, tags="map_image")   # 画像の描画
		else:
			self.canvas.itemconfig("map_image", image=self.draw_img_tk)   # 既に描画された画像を差し替える
		return



	"""
	+++++ ウィンドウサイズが変更されたとき、情報を更新する +++++
	"""
	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
		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()