@doc raw"""
    EssentialManifold <: AbstractPowerManifold{ℝ}

The essential manifold is the space of the essential matrices which is represented as
a quotient space of the [`Rotations`](@ref) manifold product ``\mathrm{SO}(3)^2``.

Let ``R_x(θ), R_y(θ), R_x(θ) \in ℝ^{x×3}`` denote the rotation around the ``z``,
``y``, and ``x`` axis in ``ℝ^3``, respectively, and further the groups

```math
H_z = \bigl\{(R_z(θ),R_z(θ))\ \big|\ θ ∈ [-π,π) \bigr\}
```

and

```math
H_π = \bigl\{ (I,I), (R_x(π), R_x(π)), (I,R_z(π)), (R_x(π), R_y(π))  \bigr\}
```

acting elementwise on the left from ``\mathrm{SO}(3)^2`` (component wise).

Then the unsigned Essential manifold ``\mathcal{M}_{\text{E}}`` can be identified
with the quotient space

````math
\mathcal{M}_{\text{E}} := (\text{SO}(3)×\text{SO}(3))/(H_z × H_π),
````

and for the signed Essential manifold ``\mathcal{M}_{\text{Ǝ}}``, the quotient reads

````math
\mathcal{M}_{\text{Ǝ}} := (\text{SO}(3)×\text{SO}(3))/(H_z).
````

An essential matrix is defined as

````math
E = (R'_1)^T [T'_2 - T'_1]_{×} R'_2,
````

where the poses of two cameras ``(R_i', T_i'), i=1,2``, are contained in the space of
rigid body transformations ``SE(3)`` and the operator ``[⋅]_{×}\colon ℝ^3 → \operatorname{SkewSym}(3)``
denotes the matrix representation of the cross product operator. For more details see [TronDaniilidis:2017](@cite).

# Constructor
    EssentialManifold(is_signed=true)

Generate the manifold of essential matrices, either the signed (`is_signed=true`) or
unsigned (`is_signed=false`) variant.

"""
struct EssentialManifold <: AbstractPowerManifold{ℝ, Rotations{TypeParameter{Tuple{3}}}, NestedPowerRepresentation}
    is_signed::Bool
    manifold::Rotations{TypeParameter{Tuple{3}}}
end

function EssentialManifold(is_signed::Bool = true)
    return EssentialManifold(is_signed, Rotations(3; parameter = :type))
end

@doc raw"""
    check_point(M::EssentialManifold, p; kwargs...)

Check whether the matrix is a valid point on the [`EssentialManifold`](@ref) `M`,
i.e. a 2-element array containing SO(3) matrices.
"""
function check_point(M::EssentialManifold, p; kwargs...)
    return check_point(
        PowerManifold(M.manifold, NestedPowerRepresentation(), 2),
        p;
        kwargs...,
    )
end

"""
    check_vector(M::EssentialManifold, p, X; kwargs... )

Check whether `X` is a tangent vector to manifold point `p` on the [`EssentialManifold`](@ref) `M`,
i.e. `X` has to be a 2-element array of `3`-by-`3` skew-symmetric matrices.
"""
function check_vector(M::EssentialManifold, p, X; kwargs...)
    return check_vector(
        PowerManifold(M.manifold, NestedPowerRepresentation(), 2),
        p,
        X;
        kwargs...,
    )
end

@doc raw"""
    distance(M::EssentialManifold, p, q)

Compute the Riemannian distance between the two points `p` and `q` on the [`EssentialManifold`](@ref). This is done by
computing the distance of the equivalence classes ``[p]`` and ``[q]`` of the points
``p=(R_{p_1},R_{p_2}), q=(R_{q_1},R_{q_2}) ∈ SO(3)^2``, respectively. Two points in ``SO(3)^2`` are equivalent iff their
corresponding essential matrices, given by
````math
E = R_1^T [e_z]_{×}R_2,
````
are equal (up to a sign flip). Using the logarithmic map, the distance is given by
````math
\text{dist}([p],[q]) = \| \text{log}_{[p]} [q] \| = \| \log_p (S_z(t_{\text{opt}})q) \|,
````
where ``S_z ∈ H_z = \{(R_z(θ),R_z(θ))\colon θ \in [-π,π) \}`` in which ``R_z(θ)`` is the rotation around the z axis with angle
``θ`` and ``t_{\text{opt}}`` is the minimizer of the cost function
````math
f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,
````
where ``d(⋅,⋅)`` is the distance function in ``SO(3)`` [TronDaniilidis:2017](@cite).
"""
distance(M::EssentialManifold, p, q) = norm(M, p, log(M, p, q))

@doc raw"""
    exp(M::EssentialManifold, p, X)

Compute the exponential map on the [`EssentialManifold`](@ref) from `p` into direction
`X`, i.e.
````math
\text{exp}_p(X) =\text{exp}_g( \tilde X),  \quad g \in \text(SO)(3)^2,
````
where ``\tilde X`` is the horizontal lift of ``X``[TronDaniilidis:2017](@cite).
"""
exp(::EssentialManifold, ::Any...)

get_iterator(::EssentialManifold) = Base.OneTo(2)

function _isapprox(
        M::EssentialManifold,
        p,
        q::T;
        atol::Real = eps(real(float(number_eltype(number_eltype(T))))),
        kwargs...,
    ) where {T}
    return isapprox(distance(M, p, q), 0.0; atol = atol, kwargs...)
end

"""
    is_flat(::EssentialManifold)

Return false. [`EssentialManifold`](@ref) is not a flat manifold.
"""
is_flat(M::EssentialManifold) = false

@doc raw"""
    log(M::EssentialManifold, p, q)

Compute the logarithmic map on the [`EssentialManifold`](@ref) `M`, i.e. the tangent vector,
whose geodesic starting from `p` reaches `q` after time 1. Here, ``p=(R_{p_1},R_{p_2})`` and
``q=(R_{q_1},R_{q_2})`` are elements of ``SO(3)^2``. We use that any essential matrix can, up to
scale, be decomposed to
````math
E = R_1^T [e_z]_{×}R_2,
````
where ``(R_1,R_2)∈SO(3)^2``. Two points in ``SO(3)^2`` are equivalent iff their corresponding
essential matrices are equal (up to a sign flip).
To compute the logarithm, we first move `q` to another representative of its equivalence class.
For this, we find ``t= t_{\text{opt}}`` for which the function
````math
f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,
````
where ``d(⋅,⋅)`` is the distance function in ``SO(3)``, is minimized. Further, the group ``H_z`` acting
on the left on ``SO(3)^2`` is defined as
````math
H_z = \{(R_z(θ),R_z(θ))\colon θ \in [-π,π) \},
````
where ``R_z(θ)`` is the rotation around the z axis with angle ``θ``. Points in ``H_z`` are denoted by
``S_z``. Then, the logarithm is defined
as
````math
\log_p (S_z(t_{\text{opt}})q) = [\text{Log}(R_{p_i}^T R_z(t_{\text{opt}})R_{b_i})]_{i=1,2},
````
where ``\text{Log}`` is the [`logarithm`](@ref log(::Rotations, ::Any...)) on ``SO(3)``. For more
details see [TronDaniilidis:2017](@cite).
"""
log(M::EssentialManifold, ::Any, ::Any)

function log!(M::EssentialManifold, X, p, q)
    # compute the closest representative of q
    t = 0
    f_min = Inf
    q2 = copy(M, q)

    if !M.is_signed
        q2min = copy(M, q)
        for k in 1:4
            #flip sign in q to get another member of its equivalence class
            if k == 2
                q2[1][2:3, :] = -q[1][2:3, :]
                q2[2][[1 3], :] = -q[2][[1 3], :]
            elseif k == 3
                q2[2][1:2, :] = -q[2][1:2, :]
            elseif k == 4
                q2[1][2:3, :] = -q[1][2:3, :]
                q2[2][2:3, :] = -q[2][2:3, :]
            end
            t_temp, f_temp = dist_min_angle_pair(p, q2)
            if f_temp < f_min
                q2min = copy(M, q2)
                f_min = f_temp
                t = t_temp
            end
        end
        q2 = q2min
    else
        t, _ = dist_min_angle_pair(p, q2)
    end
    Rz = [cos(t) -sin(t) 0; sin(t) cos(t) 0; 0 0 1]
    representative_q = zeros(size(q2))
    representative_q = [Rz * q2[1], Rz * q2[2]]

    # use the logarithmic map in SO(3)^2 and the corresponding geodesic between p and representative_q to compute the solution
    log!.(Ref(M.manifold), X, p, representative_q)
    return X
end

@doc raw"""
    dist_min_angle_pair(p, q)

This function computes the global minimizer of the function
````math
f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,
````
for the given values. This is done by finding the discontinuity points ``t_{d_i}, i=1,2`` of its derivative
and using Newton's method to minimize the function over the intervals ``[t_{d_1},t_{d_2}]`` and ``[t_{d_2},t_{d_1}+2π]``
separately. Then, the minimizer for which ``f`` is minimal is chosen and given back together with the minimal value.
For more details see Algorithm 1 in [TronDaniilidis:2017](@cite).
"""
function dist_min_angle_pair(p, q)
    #compute rotations
    q211 = q[1] * p[1]'
    q212 = q[2] * p[2]'

    #compute values from Proposition 9
    t_break1, c1, m1, Φ1 = dist_min_angle_pair_discontinuity_distance(q211)
    t_break2, c2, m2, Φ2 = dist_min_angle_pair_discontinuity_distance(q212)

    #check if cost is constant
    tol_m_zero = 5 * eps(eltype(q211)) # i.e. promoted from p and q
    tol_break = sqrt(eps(eltype(q211)))
    if (abs(m1) < tol_m_zero) && (abs(m2) < tol_m_zero)
        t_min = 0
        f_min = 2 * pi^2
    else
        if abs(mod(t_break1 - t_break2 + pi, 2 * pi) - pi) < tol_break
            t_min = t_break1 + pi
            f_min = 0
        else
            t_search1 = t_break1
            t_search2 = t_break2
            if t_search1 > t_search2
                t_search1 = t_search1 - 2 * pi
            end

            #compute derivatives of each term at discontinuity points
            df1_break1 = dist_min_angle_pair_compute_df_break(t_break1, q211)
            df2_break2 = dist_min_angle_pair_compute_df_break(t_break2, q212)

            #compute derivative of each term at other's discontinuity
            θ1_break2 = acos(clamp((m1 * sin(t_break2 + Φ1) + c1 - 1) / 2, -1.0, 1.0))
            df1_break2 = -θ1_break2 * (m1 * cos(t_break2 + Φ1)) / (2 * sin(θ1_break2))
            θ2_break1 = acos(clamp((m2 * sin(t_break1 + Φ2) + c2 - 1) / 2, -1.0, 1.0))
            df2_break1 = -θ2_break1 * (m2 * cos(t_break1 + Φ2)) / (2 * sin(θ2_break1))

            #compute left and right derivatives of sum of the two terms
            df_break1n = df1_break1 + df2_break1
            df_break1p = -df1_break1 + df2_break1
            df_break2n = df2_break2 + df1_break2
            df_break2p = -df2_break2 + df1_break2

            f_min = Inf
            if sign(df_break1p) != sign(df_break2n)
                #parabolic prediction of min
                t_min0 =
                    t_search1 -
                    df_break1p * (t_search2 - t_search1) / (df_break2n - df_break1p)
                #use Newton's search to find t_min
                t_min, f_min = dist_min_angle_pair_df_newton(
                    m1,
                    Φ1,
                    c1,
                    m2,
                    Φ2,
                    c2,
                    t_min0,
                    t_search1,
                    t_search2,
                )
            end
            t_search1 = t_search1 + 2 * pi
            if sign(df_break2p) != sign(df_break1n)
                #parabolic prediction of min
                t_min0 =
                    t_search2 -
                    df_break2p * (t_search1 - t_search2) / (df_break1n - df_break2p)
                #use Newton's search to find t_min
                t_min2, f_min2 = dist_min_angle_pair_df_newton(
                    m1,
                    Φ1,
                    c1,
                    m2,
                    Φ2,
                    c2,
                    t_min0,
                    t_search2,
                    t_search1,
                )
                #choose t with lower function values
                if f_min2 < f_min
                    t_min = t_min2
                    f_min = f_min2
                end
            end
        end
    end
    return t_min, f_min
end

@doc raw"""
    dist_min_angle_pair_discontinuity_distance(q)

This function computes the point ``t_{\text{di}}`` for which the first derivative of
````math
f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,
````
does not exist. This is the case for ``\sin(θ_i(t_{\text{di}})) = 0``. For more details see Proposition 9
and its proof, as well as Lemma 1 in [TronDaniilidis:2017](@cite).
"""
function dist_min_angle_pair_discontinuity_distance(q)
    c1 = q[1, 1] + q[2, 2]
    c2 = q[1, 2] - q[2, 1]
    c3 = q[3, 3]

    m = norm([c1 c2])
    Φ = atan(c1, c2)

    t_break = -0.5 * pi - Φ
    return t_break, c3, m, Φ
end

@doc raw"""
    dist_min_angle_pair_compute_df_break(t_break, q)

This function computes the derivatives of each term ``f_i, i=1,2,`` at discontinuity point `t_break`. For more details see [TronDaniilidis:2017](@cite).
"""
function dist_min_angle_pair_compute_df_break(t_break, q)
    c = cos(t_break)
    s = sin(t_break)

    q1_r_break_q1 = [c -s 0; s c 0; 0 0 1] * q
    F = svd(q1_r_break_q1 + I(3))
    df_break = pi * abs(F.U[3, 1])
    return df_break
end

@doc raw"""
    dist_min_angle_pair_df_newton(m1, Φ1, c1, m2, Φ2, c2, t_min, t_low, t_high)

This function computes the minimizer of the function
````math
f(t) = f_1 + f_2, \quad f_i = \frac{1}{2} θ^2_i(t), \quad θ_i(t)=d(R_{p_i},R_z(t)R_{b_i}) \text{ for } i=1,2,
````
in the interval ``[```t_low`, `t_high```]`` using Newton's method. For more details see [TronDaniilidis:2017](@cite).
"""
function dist_min_angle_pair_df_newton(m1, Φ1, c1, m2, Φ2, c2, t_min, t_low, t_high)
    tol_dist = sqrt(eps(eltype(t_min)))
    θ1 = zero(t_min)
    θ2 = zero(t_min)
    for i in 1:100
        #compute auxiliary values
        mc1 = m1 * cos(t_min + Φ1)
        mc2 = m2 * cos(t_min + Φ2)

        #compute θi, i=1,2
        θ1 = acos(clamp(((m1 * sin(t_min + Φ1) + c1 - 1) / 2), -1, 1))
        θ2 = acos(clamp(((m2 * sin(t_min + Φ2) + c2 - 1) / 2), -1, 1))

        #compute the first derivatives di, i=1,2
        s1 = 2 * sin(θ1)
        s2 = 2 * sin(θ2)
        d1 = (-θ1 * mc1) / s1
        d2 = (-θ2 * mc2) / s2
        d = d1 + d2

        #compute the second derivatives ddi, i=1,2
        eztuSq1 = (mc1 / s1)^2
        eztuSq2 = (mc2 / s2)^2
        dd1 = eztuSq1 + θ1 / 2 * cot(θ1 / 2) * (1 - eztuSq1)
        dd2 = eztuSq2 + θ2 / 2 * cot(θ2 / 2) * (1 - eztuSq2)
        dd = dd1 + dd2

        #compute the new t_min
        t_old = t_min
        t_min = clamp(t_old - d / dd, t_low + tol_dist, t_high - tol_dist)
        if abs(t_min - t_old) < tol_dist
            break
        end
    end
    f_min = θ1^2 + θ2^2
    return t_min, f_min
end

@doc raw"""
    manifold_dimension(M::EssentialManifold{is_signed, ℝ})

Return the manifold dimension of the [`EssentialManifold`](@ref), which is `5`[TronDaniilidis:2017](@cite).
"""
function manifold_dimension(::EssentialManifold)
    return 5
end

function power_dimensions(::EssentialManifold)
    return (2,)
end

@doc raw"""
    project(M::EssentialManifold, p, X)

Project the matrix `X` onto the tangent space
````math
T_{p} \text{SO}(3)^2 = T_{\text{vp}}\text{SO}(3)^2 ⊕ T_{\text{hp}}\text{SO}(3)^2,
````
by first computing its projection onto the vertical space ``T_{\text{vp}}\text{SO}(3)^2`` using [`vert_proj`](@ref).
Then the orthogonal projection of `X` onto the horizontal space ``T_{\text{hp}}\text{SO}(3)^2`` is defined as
````math
\Pi_h(X) = X - \frac{\text{vert\_proj}_p(X)}{2} \begin{bmatrix} R_1^T e_z \\ R_2^T e_z \end{bmatrix},
````
with ``R_i = R_0 R'_i, i=1,2,`` where ``R'_i`` is part of the pose of camera ``i`` ``g_i = (R'_i,T'_i) ∈ \text{SE}(3)``
and ``R_0 ∈ \text{SO}(3)`` such that ``R_0(T'_2-T'_1) = e_z``.
"""
project(::EssentialManifold, ::Any, ::Any)

function project!(M::EssentialManifold, Y, p, X)
    X_proj = [pe' * Xe for (pe, Xe) in zip(p, X)]
    X_proj .= [0.5 * (Xe - Xe') for Xe in X_proj]
    # projection on the vertical component
    s = vert_proj(M, p, X_proj)
    # orthogonal projection
    copyto!(
        Y,
        [
            Xe - (s / 2) * get_vector(M.manifold, pe, pe[3, :], DefaultOrthogonalBasis())
                for (Xe, pe) in zip(X_proj, p)
        ],
    )
    return Y
end

function Base.show(io::IO, M::EssentialManifold)
    return print(io, "EssentialManifold($(M.is_signed))")
end

function parallel_transport_direction(M::EssentialManifold, p, X, d)
    return parallel_transport_to(M, p, X, exp(M, p, d))
end
function parallel_transport_direction!(M::EssentialManifold, Y, p, X, d)
    parallel_transport_to!(M, Y, p, X, exp(M, p, d))
    return Y
end

@doc raw"""
    parallel_transport_to(M::EssentialManifold, p, X, q)

Compute the vector transport of the tangent vector `X` at `p` to `q` on the
[`EssentialManifold`](@ref) `M` using left translation of the ambient group.
"""
function parallel_transport_to(::EssentialManifold, p, X, q)
    # group operation in the ambient group
    pq = [qe' * pe for (pe, qe) in zip(p, q)]
    # left translation
    return [pqe * Xe * pqe' for (pqe, Xe) in zip(pq, X)]
end
function parallel_transport_to!(::EssentialManifold, Y, p, X, q)
    # group operation in the ambient group
    pq = [qe' * pe for (pe, qe) in zip(p, q)]
    # left translation
    copyto!(Y, [pqe * Xe * pqe' for (pqe, Xe) in zip(pq, X)])
    return Y
end

@doc raw"""
    vert_proj(M::EssentialManifold, p, X)

Project `X` onto the vertical space ``T_{\text{vp}}\text{SO}(3)^2`` with
````math
\text{vert\_proj}_p(X) = e_z^T(R_1 X_1 + R_2 X_2),
````
where ``e_z`` is the third unit vector, ``X_i ∈ T_{p}\text{SO}(3)`` for ``i=1,2,`` and it holds ``R_i = R_0 R'_i, i=1,2,`` where ``R'_i`` is part of the
pose of camera ``i`` ``g_i = (R_i,T'_i) ∈ \text{SE}(3)`` and ``R_0 ∈ \text{SO}(3)`` such that ``R_0(T'_2-T'_1) = e_z`` [TronDaniilidis:2017](@cite).
"""
function vert_proj(M::EssentialManifold, p, X)
    return sum(vert_proj.(Ref(M.manifold), p, X))
end
function vert_proj(M::Rotations{TypeParameter{Tuple{3}}}, p, X)
    return (p[3, :]' * get_coordinates(M, p, X, DefaultOrthogonalBasis()))
end
