// This file is part of the AliceVision project.
// 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/.

#include <aliceVision/sfm/sfm.hpp>
#include <aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp>
#include <aliceVision/sfmData/SfMData.hpp>
#include <aliceVision/sfmDataIO/sfmDataIO.hpp>
#include <aliceVision/cmdline/cmdline.hpp>
#include <aliceVision/system/main.hpp>
#include <aliceVision/config.hpp>

#include <uncertaintyTE/uncertainty.h>
#include <uncertaintyTE/IO.h>

#include <boost/program_options.hpp>

#include <string>
#include <sstream>
#include <vector>

// These constants define the current software version.
// They must be updated when the command line is changed.
#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1
#define ALICEVISION_SOFTWARE_VERSION_MINOR 0

using namespace aliceVision;
using namespace aliceVision::sfm;
using namespace aliceVision::sfmData;
using namespace aliceVision::sfmDataIO;
namespace po = boost::program_options;


int aliceVision_main(int argc, char **argv)
{
  // command-line parameters
  std::string sfmDataFilename;
  std::string outSfMDataFilename;
  std::string outputStats;
  std::string algorithm = cov::EAlgorithm_enumToString(cov::eAlgorithmSvdTaylorExpansion);
  bool debug = false;

  params.add_options()
    ("input,i", po::value<std::string>(&sfmDataFilename)->required(),
      "SfMData file to align.")
  ("output,o", po::value<std::string>(&outSfMDataFilename)->required(),
    "Output SfMData scene.")
  ("outputCov,c", po::value<std::string>(&outputStats),
    "Output covariances file.")
  ("algorithm,a", po::value<std::string>(&algorithm)->default_value(algorithm),
    "Algorithm.")
  ("debug,d", po::value<bool>(&debug)->default_value(debug),
    "Enable creation of debug files in the current folder.")
    ("verboseLevel,v", po::value<std::string>(&verboseLevel)->default_value(verboseLevel),
      "verbosity level (fatal,  error, warning, info, debug, trace).");

  CmdLine cmdline("AliceVision computeUncertainty");
  cmdline.add(params);
  if (!cmdline.execute(argc, argv))
  {
      return EXIT_FAILURE;
  }
  
  if (sfmDataFilename.empty() ||
      outSfMDataFilename.empty())
  {
    std::cerr << "Invalid input or output filename." << std::endl;
    return EXIT_FAILURE;
  }

  // Load input scene
  SfMData sfmData;
  if (!Load(sfmData, sfmDataFilename, ESfMData(ALL)))
  {
    std::cerr << std::endl
      << "The input SfMData file \"" << sfmDataFilename << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }

  ceres::CRSMatrix jacobian;
  {
    BundleAdjustmentCeres bundleAdjustmentObj;
    BundleAdjustment::ERefineOptions refineOptions = BundleAdjustment::REFINE_ROTATION | BundleAdjustment::REFINE_TRANSLATION | BundleAdjustment::REFINE_STRUCTURE;
    bundleAdjustmentObj.createJacobian(sfmData, refineOptions, jacobian);
  }

  {
    cov::Options options;
    // Configure covariance engine (find the indexes of the most distatnt points etc.)
    // setPts2Fix(opt, mutable_points.size() / 3, mutable_points.data());
    options._numCams = sfmData.getValidViews().size();
    options._camParams = 6;
    options._numPoints = sfmData.getLandmarks().size();
    options._numObs = jacobian.num_rows / 2;
    options._algorithm = cov::EAlgorithm_stringToEnum(algorithm);
    options._epsilon = 1e-10;
    options._lambda = -1;
    options._svdRemoveN = -1;
    options._maxIterTE = -1;
    options._debug = debug;

    cov::Statistic statistic;
    std::vector<double> points3D;
    points3D.reserve(sfmData.getLandmarks().size() * 3);
    for(auto& landmarkIt: sfmData.getLandmarks())
    {
      double* p = landmarkIt.second.X.data();
      points3D.push_back(p[0]);
      points3D.push_back(p[1]);
      points3D.push_back(p[2]);
    }

    cov::Uncertainty uncertainty;

    getCovariances(options, statistic, jacobian, &points3D[0], uncertainty);

    if(!outputStats.empty())
      saveResults(outputStats, options, statistic, uncertainty);

    {
      const std::vector<double> posesUncertainty = uncertainty.getCamerasUncEigenValues();

      std::size_t indexPose = 0;
      for (Poses::const_iterator itPose = sfmData.getPoses().begin(); itPose != sfmData.getPoses().end(); ++itPose, ++indexPose)
      {
        const IndexT idPose = itPose->first;
        Vec6& u = sfmData._posesUncertainty[idPose]; // create uncertainty entry
        const double* uIn = &posesUncertainty[indexPose*6];
        u << uIn[0],  uIn[1],  uIn[2],  uIn[3],  uIn[4],  uIn[5];
      }
    }
    {
      const std::vector<double> landmarksUncertainty = uncertainty.getPointsUncEigenValues();

      std::size_t indexLandmark = 0;
      for (Landmarks::const_iterator itLandmark = sfmData.getLandmarks().begin(); itLandmark != sfmData.getLandmarks().end(); ++itLandmark, ++indexLandmark)
      {
        const IndexT idLandmark = itLandmark->first;
        Vec3& u = sfmData._landmarksUncertainty[idLandmark]; // create uncertainty entry
        const double* uIn = &landmarksUncertainty[indexLandmark*3];
        u << uIn[0],  uIn[1],  uIn[2];
      }
    }
  }

  std::cout << "Save into \"" << outSfMDataFilename << "\"" << std::endl;
  
  // Export the SfMData scene in the expected format
  if (!Save(sfmData, outSfMDataFilename, ESfMData(ALL)))
  {
    std::cerr << std::endl
      << "An error occurred while trying to save \"" << outSfMDataFilename << "\"." << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}
