#!/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

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


# ## Collect data

# In[2]:


# image Stijn's iPhone

image_width = 3024  # pixels
image_height = 4032

image_focalLength = 3000

image_width = 320  # pixels
image_height = 240

image_focalLength = 65.7


# In[3]:


# Phineas_position2.png (Febr 29, 2024)
# Nao v6   

g = [0.0, 0.0, 0.5] # Assume looking straight

a = 1.5 # m Center circle diameter - Distance J from https://spl.robocup.org/wp-content/uploads/SPL-Rules-2024.pdf

point1 = [ 50.5, 191.5]
point2 = [  285, 184.5]

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

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

# point7 = [ 181, 1164]


# ## Show image

# In[4]:


#img = imread(u"IMG_3818.jpg")

#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


# ## 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[10]:


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]


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

# In[20]:


from math import cos
from math import acos
from math import sin
from math import asin
from math import pi


# In[39]:


def angleFromDirections(d1, d2):
    
    inproduct = d1[0]*d2[0] + d1[1]*d2[1] + d1[2]*d2[2]
    
    length1 = sqrt(d1[0]*d1[0] + d1[1]*d1[1] + d1[2]*d1[2])
    length2 = sqrt(d2[0]*d2[0] + d2[1]*d2[1] + d2[2]*d2[2])
    
    angle = acos(inproduct/(length1*length2))
    
    return angle

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

    a1 = angleFromDirections(d_g,d_1)
    a2 = angleFromDirections(d_g,d_2)
    b12 = angleFromDirections(d_1,d_2)

    root = cos(a1)*cos(a2)*cos(b12)
    denominator = cos(a1)**2 + cos(a2)**2 - 2*root
    
    r = a * cos(a2) / sqrt(denominator)
    phi = acos( (cos(a2)**2 * a*a + (sin(a1)**2 - sin(a2)**2) * r*r) / (2*sin(a1) * cos(a2)**2 * a * r) )
    theta = a1
    
    x = -r * sin(theta) * sin(phi)
    y = r * sin(theta) * cos(phi)
    z = r * cos(theta)
    
    return [x,y,z]


# ## Comparison Cartesian and Spherical

# In[36]:


print(cameraPositionCartesian(d_g, d_1, d_2, a))


# In[37]:


print(cameraPositionSpherical(d_g, d_1, d_2, a))


# In[40]:


print(cameraPositionClassical(d_g, d_1, d_2, a))


# In[ ]:



import time

n_iter = 50

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')
start_time = time.clock()

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

end_time = time.clock()

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