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)

#write code for a) here

#write answer for b) here

#write code for c) here

#write code for d) here
#hint 1: given a system f of rational functions, you can extract its numerators and denominators as follows:
    #numden = get_num_den.(f)
    #nums = first.(numden)
    #dens = last.(numden)
#hint 2: from a vector S of solutions, you can filter out those with 0 denominators as follows:
    #good_sols = filter(S) do s
    #    den_vals = evaluate(dens, vars => s)
    #    all(abs.(den_vals) .> 1e-10)
    #end
#here, vars is a vector of all variables. it is important to specify that exact same variable vector when 
#defining the system to be solved using the command System() as otherwise Julia might use another variable
#ordering as the one specified in vars