using HomotopyContinuation
using LinearAlgebra

@var X1,X2,X3,
Y1,Y2,Y3,
Z1,Z2,Z3,
L1,L2,
K1,K2,
R1,R2,R3,
S1,S2,S3,
T1,T2,
U1,U2,U3;

function cayleyRot(c1,c2,c3)
    #fr = 1 + c1^2 + c2^2 + c3^2
    #technically we have to scale the matrix by 1/fr but examining the equations we see that this scaling factor cancels out in the end
    # one warning: You should be aware that forgetting the scaling might lead to solutins where it is 0 and you divide by 0.
    [
        1+c1^2-c2^2-c3^2   2*(c1*c2-c3)       2*(c1*c3+c2)
        2*(c1*c2+c3)       1-c1^2+c2^2-c3^2   2*(c2*c3-c1)
        2*(c1*c3-c2)       2*(c2*c3+c1)       1-c1^2-c2^2+c3^2
    ]
end


#Setting up the points and lines in the normalised form described on the exercise sheet. 
x = [[X1],[X2],[X3],[1]]
y = [[Y1],[Y2],[Y3],[1]]
z = [[Z1],[Z2],[Z3],[1]]

l = [[L1],[L2],[1],[0]]
k = [[K1],[K2],[1],[0]]

#Setting up the cameras

Cam1 = 
[
    1 0 0 0;
    0 1 0 0;
    0 0 1 0
]

Cam2 = hcat(
    cayleyRot(R1,R2,R3),
    [T1; T2; 1]
)

Cam3 = hcat(
    cayleyRot(S1,S2,S3),
    [U1;U2;U3]
)

#Define the map as in 1. and 2.
function takePicOfPoint(Cam, Pt)
    pic = Cam * Pt
    [
        pic[1][1] / pic[3][1],
        pic[2][1] / pic[3][1]
    ]
end

function takePicOfLine(Cam, FinitePt, InftyPt)

    pt1 = Cam * FinitePt
    pt2 = Cam * InftyPt

    z1 = pt1[3][1]
    z2 = pt2[3][1]

    ptAtInfinity = z1*pt2 - z2*pt1

    ptAtInfinity[1][1] / ptAtInfinity[2][1]
end

pic1 = vcat(
    takePicOfPoint(Cam1,x),
    takePicOfPoint(Cam1,y),
    takePicOfPoint(Cam1,z),
    [takePicOfLine(Cam1,x,l)],
    [takePicOfLine(Cam1,y,k)]
)

pic2 = vcat(
    takePicOfPoint(Cam2,x),
    takePicOfPoint(Cam2,y),
    takePicOfPoint(Cam2,z),
    [takePicOfLine(Cam2,x,l)],
    [takePicOfLine(Cam2,y,k)]
)

pic3 = vcat(
    takePicOfPoint(Cam3,x),
    takePicOfPoint(Cam3,y),
    takePicOfPoint(Cam3,z),
    [takePicOfLine(Cam3,x,l)],
    [takePicOfLine(Cam3,y,k)]
)

picMap = vcat(pic1, pic2, pic3)

vars = [X1,X2,X3,Y1,Y2,Y3,Z1,Z2,Z3,L1,L2,K1,K2,R1,R2,R3,S1,S2,S3,T1,T2,U1,U2,U3]
J = differentiate(picMap, vars)
p = rand(24)
det(evaluate(J, vars => p))

@var u[1:24]
F = System(picMap-u; parameters=u)
M = monodromy_solve(F)
solutions(M)

numden = get_num_den.(picMap-p)
nums = first.(numden)
dens = last.(numden)
G = System(nums; variables=vars)
H = solve(G)
sols = solutions(H)
good_sols = filter(sols) do s
    den_vals = evaluate(dens, vars => s)
    all(abs.(den_vals) .> 1e-13)
end
length(good_sols)