#!/usr/bin/env python
# coding: utf-8

# # Camera position from points on the soccer field and gravity
# 
# [Stijn Oomes](https://www.StijnOomes.com/contact)
# 
# 3 January - 28 February 2024

# ## Import libraries

# In[1]:


from __future__ import division
from __future__ import absolute_import
from math import sqrt

import numpy as np
import cv2

#get_ipython().run_line_magic('matplotlib', 'inline')
#import matplotlib.pyplot as plt
#from matplotlib.image import imread


# ## Collect data

# In[2]:


# image

image_width = 3024  # pixels
image_height = 4032

image_focalLength = 3000


# In[3]:


# IMG_3818.JPG
# iPhone 12

g = [0.0, 0.5, 0.5]

a = 0.5 # m 

point1 = [ 500, 1396]
point2 = [1375, 1400]

# point3 = [1365, 2019]
# point4 = [ 261, 2028]

# point5 = [1024,  979]
# point6 = [1747,  982]

# point7 = [ 181, 1164]


# ## Show image

# In[4]:


img = cv2.cvtColor(cv2.imread("kitchen_floor.jpg"), code=cv2.COLOR_BGR2RGB)

#plt.imshow(img)
#plt.axis(u"off") 
#plt.show()


# ## Calculate directions

# In[5]:


def imagePointToNormalizedDirection(image_width, image_height, focal_length, image_point):
    
    optical_x0 = (image_width +1)/2.0
    optical_y0 = (image_height +1)/2.0
   
    x = image_point[0] - optical_x0
    y = optical_y0 - image_point[1]
    z = -focal_length
    
    length = sqrt(x*x+y*y+z*z)
    
    direction = [x/length, y/length, z/length,]
    
    return direction


# In[6]:


d_g = g


# In[7]:


d_1 = imagePointToNormalizedDirection(image_width, image_height, image_focalLength, point1)
d_1


# In[8]:


d_2 = imagePointToNormalizedDirection(image_width, image_height, image_focalLength, point2)
d_2


# In[9]:


a


# ## Rational Trigonometry - Cartesian coordinates
# 
# - **input**
#     - directions: gravity, to point 1, and to point 2
#     - distance: between point 1 and 2
# - **output**
#     - position of camera

# In[14]:


def spreadFromDirections(d1, d2):
    
    inproduct = d1[0]*d2[0] + d1[1]*d2[1] + d1[2]*d2[2]
    
    quadrance1 = d1[0]*d1[0] + d1[1]*d1[1] + d1[2]*d1[2]
    quadrance2 = d2[0]*d2[0] + d2[1]*d2[1] + d2[2]*d2[2]
    
    spread = 1 - (inproduct**2/(quadrance1*quadrance2))
    
    return spread


def cameraPositionCartesian(d_g, d_1, d_2, a):

    p1 = spreadFromDirections(d_g,d_1)
    p2 = spreadFromDirections(d_g,d_2)
    q12 = spreadFromDirections(d_1,d_2)

    A = a*a

    root = sqrt((1-p1)*(1-p2)*(1-q12))
    denominator = (1-p1)+(1-p2) - 2*root
    
    x = sqrt(A * (1-p1)*(1-p2)*(q12-(1-p1)-(1-p2)+2*root) / denominator**2)
    y = sqrt(A) * ((1-p2)-root) / denominator
    z = sqrt(A * (1-p1)*(1-p2) / denominator)
    
    return [x,y,z]


# ## Rational Trigonometry - Spherical coordinates
# 
# - **input**
#     - directions: gravity, to point 1, and to point 2
#     - distance: between point 1 and 2
# - **output**
#     - position of camera

# In[11]:


def spreadFromDirections(d1, d2):
    
    inproduct = d1[0]*d2[0] + d1[1]*d2[1] + d1[2]*d2[2]
    
    quadrance1 = d1[0]*d1[0] + d1[1]*d1[1] + d1[2]*d1[2]
    quadrance2 = d2[0]*d2[0] + d2[1]*d2[1] + d2[2]*d2[2]
    
    spread = 1 - (inproduct**2/(quadrance1*quadrance2))
    
    return spread


def cameraPositionSpherical(d_g, d_1, d_2, a):

    p1 = spreadFromDirections(d_g,d_1)
    p2 = spreadFromDirections(d_g,d_2)
    q12 = spreadFromDirections(d_1,d_2)

    A = a*a

    root = sqrt((1-p1)*(1-p2)*(1-q12))
    denominator = (1-p1)+(1-p2) - 2*root
    
    R = A * (1-p2) / denominator
    s = 1 - ((1-p2)*A + (p1-p2)*R )**2 / (4*p1*(1-p2)**2 * A * R)
    t = p1
    
    x = sqrt(s * t * R)
    y = sqrt((1-s) * t * R)
    z = sqrt((1-t) * R)
    
    return [x,y,z]


# ## Comparison Cartesian and Spherical


print cameraPositionCartesian(d_g, d_1, d_2, a)

print cameraPositionSpherical(d_g, d_1, d_2, a)


# In[ ]:


import time

n_iter = 5000

start_time = time.clock()

for i in range(n_iter):
    cameraPositionCartesian(d_g, d_1, d_2, a)

end_time = time.clock()

elapsed_time = end_time - start_time
print('Execution time for Cartesian:', elapsed_time, 'seconds')

start_time = time.clock()

for i in range(n_iter):
    cameraPositionSpherical(d_g, d_1, d_2, a)

end_time = time.clock()

elapsed_time = end_time - start_time
print('Execution time for Spherical:', elapsed_time, 'seconds')