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

# # Camera position from A4 on the ground plane
# 
# [Stijn Oomes](https://www.StijnOomes.com/contact)
# 
# Thursday 2 - Monday 6 February 2023
# 
# Wednesday 3 January 2024

# ## Import libraries

# In[1]:


from __future__ import division
from __future__ import absolute_import
from math import sqrt, acos, pi
#from statistics import mean
#from statistics import stdev


# ## Collect data

# In[2]:


# diameter of circle

m = 0.260 # m


# In[3]:


# image

image_width = 3024  # pixels
image_height = 4032

image_focalLength = 3062


# In[4]:


# IMG_3544

# p1 = [1536,  347]
# p2 = [1167, 1345] 


# In[5]:


# IMG_3545

# p1 = [ 970,  458]
# p2 = [1322, 1469] 


# In[6]:


# IMG_3547

p1 = [ 570,  240]
p2 = [1078, 1180] 


# ## Define classes & functions

# In[7]:

def imageToOpticalCoordinates(image_width, image_height, image_x, image_y):
    
    optical_x0 = (image_width +1)/2.0
    optical_y0 = (image_height +1)/2.0
   
    optical_x = image_x - optical_x0
    optical_y = optical_y0 - image_y
    
    optical_coordinates = [optical_x, optical_y]
    
    return optical_coordinates


# In[8]:


class Direction3D(object):
    
    def __init__(self, X = 0.0, Y = 0.0, Z = 0.0):
        
        self.X = X
        self.Y = Y
        self.Z = Z


# In[9]:


def spreadFromDirections(d1, d2):
    
    inproduct = d1.X*d2.X + d1.Y*d2.Y + d1.Z*d2.Z
    
    quadrance1 = d1.X*d1.X + d1.Y*d1.Y + d1.Z*d1.Z
    quadrance2 = d2.X*d2.X + d2.Y*d2.Y + d2.Z*d2.Z
    
    spread = 1 - (inproduct**2/(quadrance1*quadrance2))
    
    return spread

def quadranceFromDirection(d2):
    
    
    quadrance2 = d2.X*d2.X + d2.Y*d2.Y + d2.Z*d2.Z
    
    
    return quadrance2


# ## Calculate end-points in optical coordinates

# In[10]:


p1_optic = imageToOpticalCoordinates(image_width, image_height, p1[0], p1[1])
print p1_optic[0],p1_optic[1]


# In[11]:


p2_optic = imageToOpticalCoordinates(image_width, image_height, p2[0], p2[1])
print p2_optic[0],p2_optic[1]


# ## Calculate directions

# In[12]:


d_g = Direction3D(0.0, 0.0, -1.0)


# In[13]:


d_1 = Direction3D(p1_optic[0],p1_optic[1],image_focalLength)
print [d_1.X, d_1.Y, d_1.Z]


# In[14]:


d_2 = Direction3D(p2_optic[0],p2_optic[1],image_focalLength)
print [d_2.X, d_2.Y, d_2.Z]


# ## Calculate spreads

# In[15]:


p1g = spreadFromDirections(d_g,d_1)
print format(p1g, u'.6f')


# In[16]:


p2g = spreadFromDirections(d_g,d_2)
print format(p2g, u'.6f')


# In[17]:


p12 = spreadFromDirections(d_1,d_2)
print format(p12, u'.6f')


# ## Calculate quadrance

# In[18]:


M = m*m
print format(M, u'.6f')

# From quadrance back to distances  - precalculation
q1 = quadranceFromDirection(d_1)
q2 = quadranceFromDirection(d_2)
d1 = sqrt(quadranceFromDirection(d_1))
d2 = sqrt(quadranceFromDirection(d_2))
# From spread back to cos  - precalculation
c3 = sqrt(p12)
a3 = acos(c3) * ( 180.0 / pi)
c1 = sqrt(p1g)
a1 = acos(c1) * ( 180.0 / pi)
c2 = sqrt(p2g)
a2 = acos(c2) * ( 180.0 / pi)
print("distance 1 {} from quadrance {} and direction ({},{},{})".format(d1, q1, d_1.X, d_1.Y, d_1.Z));
print("distance 2 {} from quadrance {} and direction ({},{},{})".format(d2, q2, d_2.X, d_2.Y, d_2.Z));
print("angle between those distances from spread {} and cos {} is {} degrees".format(p12, c3, a3));
print("angle between distance 1 and gravity from spread {} and cos {} is {} degrees".format(p1g, c1, a1));
print("angle between distance 2 and gravity from spread {} and cos {} is {} degrees".format(p2g, c2, a2));

ql = q1 + q2 - sqrt(4 * q1 * q2) 
dl = sqrt(ql)
print("quadrance L {} or distance L {} in pixels should be equivalent with {} meters".format(ql, dl, m));
factor = m / dl
print("distance 1 {} in pixels should be equivalent with distance {} meters, which gives a height of {} meters".format(d1, d1 * factor, d1 * factor * (1 - c1)));
print("distance 2 {} in pixels should be equivalent with distance {} meters, which gives a heigth of {} meters".format(d2, d2 * factor, d2 * factor * (1 - c2)));

def determine_classic_camera_position(d1,d2,c3):
    # Following https://web.maths.unsw.edu.au/~norman/papers/TrigComparison.pdf -> Problem 4

    bisector_sqr = (2 * (d1 * d2)**2 * (1 + c3)) / (d1**2 + 2 * (d1 * d2) + d2 ** 2)

    bisector = sqrt (bisector_sqr)

    # From https://web.maths.unsw.edu.au/~norman/papers/Survivor.pdf -> Section 5
 

#    return (x, y, z)

def determine_camera_position(p1g,p2g,p12):

    root = sqrt((1-p1g)*(1-p2g)*(1-p12))
    denominator = (1-p1g)+(1-p2g) - 2*root

    X = M * ((1-p2g)-root)**2 / denominator**2
    x = sqrt(X)


    Y = M * (1-p1g)*(1-p2g)*(p12-(1-p1g)-(1-p2g)+2*root) / denominator**2
    y = sqrt(Y)


    Z = M * (1-p1g)*(1-p2g) / denominator
    z = sqrt(Z)

    return (x, y, z)


import time

start_time = time.clock()

n_iter = 5

for i in range(n_iter):
    determine_camera_position(p1g,p2g,p12)

end_time = time.clock()

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