// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.

#ifndef ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_
#define ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_

#include "aliceVision/numeric/numeric.hpp"
#include "aliceVision/linearProgramming/ISolver.hpp"
#include <fstream>
#include <utility>
#include <vector>
//--
//- Implementation of algorithm from Paper titled :
//- [1] "Multiple-View Geometry under the L_\infty Norm."
//- Author : Fredrik Kahl, Richard Hartley.
//- Date : 9 sept 2008.

//- [2] "Multiple View Geometry and the L_\infty -norm"
//- Author : Fredrik Kahl
//- ICCV 2005.
//--

namespace aliceVision {
namespace lInfinityCV {

using namespace linearProgramming;

//-- Estimate the translation and the structure
//    from image points coordinates and camera rotations.
//    - Estimation of Ci from Ri and xij
// [1] -> 6.1 Cameras with Known Rotation
//
//    - This implementation Use L1 norm instead of the L2 norm of
//      the paper, it allows to use standard standard LP
//      (simplex) instead of using SOCP (second order cone programming).
//      Implementation by Pierre Moulon
//
//
//  This implementation handle noisy measurement by adding a slack
//   variables for each x,y,z residual.
//  Based on idea expressed in (See Algorithm 2.0 of [3]):
// [3] Outlier Removal Using Duality.
//      Carl Olsson, Anders Eriksson and Richard Hartley, Richard.
//      CVPR 2010.

/// Encode translation and structure linear program with slack variables
///  in order to handle noisy measurements.
inline void EncodeTiXi_withNoise(const Mat& M,  // Scene representation
                                 const std::vector<Mat3>& Ri,
                                 double sigma,  // Start upper bound
                                 sRMat& A,
                                 Vec& C,
                                 std::vector<LPConstraints::eLP_SIGN>& vec_sign,
                                 std::vector<double>& vec_costs,
                                 std::vector<std::pair<double, double>>& vec_bounds)
{
    // Build Constraint matrix.

    const size_t Ncam = (size_t)M.row(3).maxCoeff() + 1;
    const size_t N3D = (size_t)M.row(2).maxCoeff() + 1;
    const size_t Nobs = M.cols();

    assert(Ncam == Ri.size());

    A.resize(5 * Nobs + 3, 3 * (Ncam + N3D + Nobs));

    C.resize(5 * Nobs + 3, 1);
    C.fill(0.0);
    vec_sign.resize(5 * Nobs + 3);

    const size_t transStart = 0;
    const size_t pointStart = transStart + 3 * Ncam;
    const size_t offsetStart = pointStart + 3 * N3D;
#define TVAR(i, el) (0 + 3 * (i) + (el))
#define XVAR(j, el) (pointStart + 3 * (j) + (el))
#define OFSVAR(k, el) (offsetStart + 3 * (k) + (el))

    // Set objective coefficients.
    vec_costs = std::vector<double>(3 * (Ncam + N3D + Nobs), 0.0);
    for (size_t k = 0; k < Nobs; ++k)
    {
        vec_costs[OFSVAR(k, 0)] = 1.0;
        vec_costs[OFSVAR(k, 1)] = 1.0;
        vec_costs[OFSVAR(k, 2)] = 1.0;
    }

    // By default set free variable:
    vec_bounds = std::vector<std::pair<double, double>>(3 * (Ncam + N3D + Nobs));
    fill(vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30));
    // Change the offset to be positive
    for (size_t k = 0; k < 3 * Nobs; ++k)
        vec_bounds[offsetStart + k].first = 0;

    size_t rowPos = 0;
    // Add the cheirality conditions (R_i*X_j + T_i)_3 >= 1
    for (size_t k = 0; k < Nobs; ++k)
    {
        const size_t indexPt3D = M(2, k);
        const size_t indexCam = M(3, k);
        const Mat3& R = Ri[indexCam];

        A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(2, 0);
        A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(2, 1);
        A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(2, 2);
        A.coeffRef(rowPos, TVAR(indexCam, 2)) = 1.0;
        A.coeffRef(rowPos, OFSVAR(k, 2)) = 1.0;
        C(rowPos) = 1.0;
        vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL;
        ++rowPos;
    }  // end for (k)

    // Add conic constraint:
    for (size_t k = 0; k < Nobs; ++k)
    {
        const Vec2 pt = M.block<2, 1>(0, k);
        const double u = pt(0);
        const double v = pt(1);
        const size_t indexPt3D = M(2, k);
        const size_t indexCam = M(3, k);
        const Mat3& R = Ri[indexCam];

        // x-residual =>
        // (R_i*X_j + T_i)_1 / (R_i*X_j + T_i)_3 - u >= -sigma
        // (R_i*X_j + T_i)_1 - u * (R_i*X_j + T_i)_3  + sigma (R_i*X_j + T_i)_3  >= 0.0
        // R_i_3 * (sigma-u) + R_i_1 + t_i_1 + t_i_3 * (sigma-u) >= 0

        A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0, 0) + (sigma - u) * R(2, 0);
        A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0, 1) + (sigma - u) * R(2, 1);
        A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0, 2) + (sigma - u) * R(2, 2);
        A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
        A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma - u;
        A.coeffRef(rowPos, OFSVAR(k, 0)) = 1.0;
        C(rowPos) = 0.0;
        vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL;
        ++rowPos;

        A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0, 0) - (sigma + u) * R(2, 0);
        A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0, 1) - (sigma + u) * R(2, 1);
        A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0, 2) - (sigma + u) * R(2, 2);
        A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0;
        A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + u);
        A.coeffRef(rowPos, OFSVAR(k, 0)) = -1.0;
        C(rowPos) = 0.0;
        vec_sign[rowPos] = LPConstraints::LP_LESS_OR_EQUAL;
        ++rowPos;

        // y-residual =>
        A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1, 0) + (sigma - v) * R(2, 0);
        A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1, 1) + (sigma - v) * R(2, 1);
        A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1, 2) + (sigma - v) * R(2, 2);
        A.coeffRef(rowPos, OFSVAR(k, 1)) = 1.0;
        A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
        A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma - v;

        C(rowPos) = 0.0;
        vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL;
        ++rowPos;

        A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1, 0) - (sigma + v) * R(2, 0);
        A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1, 1) - (sigma + v) * R(2, 1);
        A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1, 2) - (sigma + v) * R(2, 2);
        A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0;
        A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + v);
        A.coeffRef(rowPos, OFSVAR(k, 1)) = -1.0;
        C(rowPos) = 0.0;
        vec_sign[rowPos] = LPConstraints::LP_LESS_OR_EQUAL;
        ++rowPos;
    }
    // Fix the translation ambiguity. (set first cam at (0,0,0)
    // LP_EQUAL
    A.coeffRef(rowPos, TVAR(0, 0)) = 1.0;
    C(rowPos) = 0.0;
    vec_sign[rowPos] = LPConstraints::LP_EQUAL;
    ++rowPos;
    A.coeffRef(rowPos, TVAR(0, 1)) = 1.0;
    C(rowPos) = 0.0;
    vec_sign[rowPos] = LPConstraints::LP_EQUAL;
    ++rowPos;
    A.coeffRef(rowPos, TVAR(0, 2)) = 1.0;
    C(rowPos) = 0.0;
    vec_sign[rowPos] = LPConstraints::LP_EQUAL;
    ++rowPos;

#undef TVAR
#undef XVAR
#undef OFSVAR
}

struct TiXi_withNoise_L1_ConstraintBuilder
{
    TiXi_withNoise_L1_ConstraintBuilder(const std::vector<Mat3>& vec_Ri, const Mat& M)
    {
        _M = M;
        _vec_Ri = vec_Ri;
    }

    /// Setup constraints for the translation and structure problem,
    ///  in the LPConstraints object.
    bool Build(double gamma, LPConstraintsSparse& constraint)
    {
        EncodeTiXi_withNoise(_M,
                             _vec_Ri,
                             gamma,
                             constraint._constraintMat,
                             constraint._Cst_objective,
                             constraint._vec_sign,
                             constraint._vec_cost,
                             constraint._vec_bounds);

        constraint._bminimize = true;

        //-- Setup additional information about the Linear Program constraint
        // We look for nb translations and nb 3D points.
        const size_t N3D = (size_t)_M.row(2).maxCoeff() + 1;
        const size_t Nobs = _M.cols();
        const size_t NNoiseVar = Nobs;  // noise over Zdepth, (x,y) residual
        const size_t Ncam = (size_t)_M.row(3).maxCoeff() + 1;

        constraint._nbParams = (Ncam + N3D + NNoiseVar) * 3;

        return true;
    }

    std::vector<Mat3> _vec_Ri;  // Rotation matrix
    Mat _M;                     // M contains (X,Y,index3dPoint, indexCam)^T
};

}  // namespace lInfinityCV
}  // namespace aliceVision

#endif  // ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_
