import numpy as np
import cv2
import math


class CoordinateTransform:
    def __init__(self,h,w,Radius,foc,pixsize,z):
	self.w = w
	self.h = h
	#a = np.array([[w],[h],[1]])
	#self.high = z;
	self.R = Radius	#mm
	#focallengthinpixel=float(foc)/pixsize
	#self.inverseCalib_matrix = np.array([[float(1)/focallengthinpixel,0,0],[0,float(1)/focallengthinpixel,0],[0,0,1]])
	#print(a)
	#b = np.dot(self.inverseCalib_matrix,a)
	#print(b)
	#b = b*z
	#inverse calibration matrix without cx and cy
	self.xresolution = 156
	self.yresolution = 96
	#self.xresolution = b[0][0]
	#self.yresolution = b[1][0]
	#print(b)
	#print(self.xresolution)
	#print(self.yresolution)

    def transform(self, angle, x, y):
	angledeg = angle
	anglerad = float(angle*math.pi)/180
	beta = (math.pi/2) - anglerad

	Rx = self.R*math.cos(anglerad)
	Ry = self.R*math.sin(anglerad)

	OldPositioninPixel = np.array([[x],[y],[1]])
	#print(self.xresolution, self.w,x)
	m = float(self.xresolution*x)/self.w
	n = float(self.yresolution*y)/self.h
	OldPosition = np.array([[m],[n],[1.0]])
	#print(OldPositioninPixel)
	#OldPosition = np.dot(self.inverseCalib_matrix,OldPositioninPixel)
	#print(OldPosition)
	#OldPosition = OldPosition * self.high
	#print(OldPosition)
	
	
	Rotate = np.array([[math.cos(beta),-math.sin(beta),0],[math.sin(beta),math.cos(beta),0],[0,0,1]])	#rotate coordinate
	Translation1 = np.array([[1,0,Rx],[0,1,-Ry],[0,0,1]])	#transform to camera center

	if angledeg < 90 and angledeg > 0:
		gamma = (math.pi/2) - anglerad
		k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
		l = (float(self.yresolution)/2)/math.cos(gamma)
		TranslationQ1 = np.array([[1,0,-(math.cos(gamma)*k)],[0,1,-(math.sin(gamma)*k+l)],[0,0,1]])
		M1 = np.dot(TranslationQ1,Translation1)
		#print(TranslationQuadrant1)
		#print(Translation1)
		#print(M1)
		M2 = np.dot(M1,Rotate)
		#print(M2)
		M3 = np.dot(M2,OldPosition)
		print(M3)
	if angledeg > 90 and angledeg < 180:
		gamma = (math.pi) - anglerad
		k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
		l = (float(self.yresolution)/2)/math.cos(gamma)
		TranslationQ2 = np.array([[1,0,-(math.cos(gamma)*k+l)],[0,1,-(math.sin(gamma)*k)],[0,0,1]])
		M1 = np.dot(TranslationQ2,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg > 180 and angledeg < 270:
		gamma = ((3*math.pi)/2) - anglerad
		k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
		l = (float(self.yresolution)/2)/math.cos(gamma)
		TranslationQ3 = np.array([[1,0,math.cos(gamma)*k],[0,1,math.sin(gamma)*k+l],[0,0,1]])
		M1 = np.dot(TranslationQ3,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg > 270 and angledeg < 360:
		gamma = (2*math.pi) - anglerad
		k = (float(self.xresolution)/2) - (float(self.yresolution)/2)*math.tan(gamma)
		l = (float(self.yresolution)/2)/math.cos(gamma)
		TranslationQ4 = np.array([[1,0,math.sin(gamma)*k+l],[0,1,-(math.cos(gamma)*k)],[0,0,1]])
		M1 = np.dot(TranslationQ4,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg == 0:
		TranslationA0 = np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
		M1 = np.dot(TranslationA0,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg == 90:
		TranslationA90= np.array([[1,0,-(float(self.xresolution)/2)],[0,1,-(float(self.yresolution)/2)],[0,0,1]])
		M1 = np.dot(TranslationA90,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg == 180:
		TranslationA180= np.array([[1,0,-(float(self.yresolution)/2)],[0,1,(float(self.xresolution)/2)],[0,0,1]])
		M1 = np.dot(TranslationA180,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	if angledeg == 270:
		TranslationA270= np.array([[1,0,(float(self.xresolution)/2)],[0,1,(float(self.yresolution)/2)],[0,0,1]])
		M1 = np.dot(TranslationA270,Translation1)
		M2 = np.dot(M1,Rotate)
		M3 = np.dot(M2,OldPosition)
	# return position with base orientation
        return M3[0][0], M3[1][0]