# coding: UTF-8

import numpy as np
import sys
import math
import cv2
from time import sleep

mode = 1
SELECT_VP = 1
SELECT_REFLINE = 2
SELECT_CLCLINE = 3

dragOn = False
nVp = 0 #Nunber of vanishing point
nVline = 0 #Nunber of vanishing line

wsize=5
cnt=0
filename = './man.jpg'	#デフォルトで読み込むファイル名 defaut filename

xary = np.array([])
yary = np.array([])
refx = np.array([])
refy = np.array([])
rref = 0
calcLinex = np.array([])
calcLiney = np.array([])
vlinePoint =[[],[]]

msg1 = ['Click 1st point:', 'Click 2nd point:', 'Click 3rd point:', 'Click 4th point:']
msg2 = ['Input 1st coord(x,y):','Input 2nd coord(x,y):','Input 3rd coord(x,y):','Input 4th coord(x,y):']

#マウス操作のためのコールバック関数
#Callback function for mouse
def onMouse( event, u, v, flag, params ):
	#wname, img = params
	global dragOn
	global cnt
	global xary,yary,uary,vary
	global refx, refy
	global rref
	global calcLinex, calcLiney
#	if event == cv2.EVENT_MOUSEMOVE:
	"""
		if dragOn == True:
			u = 2.0*np.pi*(x-sizex/2)#/float(sizex)
			v = 2.0*np.pi*(y-sizey/2)#/float(sizey)
			im = np.zeros((sizex,sizey,1), dtype = np.float32)
			MakeSinImage(im,u,v)
			cv2.imshow( 'sine', im )
			for j in range(y-wsize,y+wsize):
				for i in range(x-wsize,x+wsize):
					fil[j][i] = 1.0
					fil[sizey-j][sizex-i]=1.0
			#fil[y][x] = 1.0
			#fil[sizey-y][sizex-x] = 1.0
			cv2.imshow('filter',fil)
			Finv = Fimg * fil
			finv = np.fft.ifft2(np.fft.fftshift(Finv))
			finv = finv/255.0
			cv2.imshow('inv', np.real(finv))
	"""
#	if event == cv2.EVENT_LBUTTONUP:
#		dragOn = False
	if event == cv2.EVENT_LBUTTONDOWN:
		dragOn = True
		# クリックしたときの動き
		# if mode == hoge 的な感じで分けたい
		cv2.circle(img, (u, v), 3, (0, 255, 0), -1)
		cv2.imshow('image', img)
		if mode == SELECT_VP:
			xary = np.append(xary, u)
			yary = np.append(yary, v)
		elif mode == SELECT_REFLINE:
			refx = np.append(refx, u)
			refy = np.append(refy, v)
		elif mode == SELECT_CLCLINE:
			calcLinex = np.append(calcLinex, u)
			calcLiney = np.append(calcLiney, v)
		else:
			pass

param = sys.argv
mode = SELECT_VP

if len(param)==2:
	filename = param[1]

img = cv2.imread(filename)
sizex=img.shape[0]
sizey=img.shape[1]

# nVp = input('Input nunber of vanishing point : ')
nVline = input('Input nunber of vanishig line : ')


cv2.imshow('image', img)
sleep(0.01)

cv2.setMouseCallback('image', onMouse)
print('消失線ごとに2箇所クリックしてください')
# print(msg1[cnt%4])

# 消失線を引くための点を選択
while(1):
    if cv2.waitKey(20) & 0xFF == 27:
        break
	if bFlag==True:
		pass

# 消失線を引く、消失点を求める、VHを引く
mode = SELECT_REFLINE
print(xary)
print(yary)

# y = ax + b
a = np.array([])
b = np.array([])
for i in range(0, nVline * 2, 2):
    a = np.append(a, (yary[i + 1] - yary[i]) / (xary[i + 1] - xary[i]))
    print(a)
    b = np.append(b, (-a[i / 2] * xary[i] + yary[i]))

print('y = {}x + {}'.format(a[0], b[0]))
print('y = {}x + {}'.format(a[1], b[1]))

v = np.array([(b[1] - b[0]) / (a[0] - a[1]),(a[0] * b[1] - a[1] * b[0]) / (a[0] - a[1])])
print(v)

# カメラ高さHを求める
cv2.line(img, (0, int(v[1])), (sizey, int(v[1])), (0, 0, 255), 3)
cv2.imshow('image', img)
print('基準の高さを選択してください')

# 既知の長さを選択
while(1):
    if cv2.waitKey(20) & 0xFF == 27:
        break
	if bFlag==True:
		pass

rref = input('tall[mm] : ')


# 求める長さを選択
mode = SELECT_CLCLINE
print('求めたい高さを選択してください')

while(1):
    if cv2.waitKey(20) & 0xFF == 27:
        break
	if bFlag==True:
		pass

tf = ((calcLinex[0] - calcLinex[1])**2 + (calcLiney[0] - calcLiney[1])**2)**0.5
print(tf)
hf = abs(calcLiney[1] - v[1])
print(hf)
hb = abs(refy[1] - v[1])
print(hb)
cb = ((refx[0] - refx[1])**2 + (refy[0] - refy[1])**2)**0.5
print(cb)
tv = ((calcLinex[0] - v[0])**2 + (calcLiney[0] - v[1])**2)**0.5
cv = ((refx[0] - v[0])**2 + (refy[0] - v[1])**2)**0.5


result = (tf / hf) * (hb / cb) * (cv / tv) * rref

print(result)

cv2.waitKey(0)
cv2.destroyAllWindows()