// This file is part of the AliceVision project.
// Copyright (c) 2017 AliceVision 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/.

#include <aliceVision/sfmData/SfMData.hpp>
#include <aliceVision/sfmDataIO/sfmDataIO.hpp>
#include <aliceVision/image/all.hpp>
#include <aliceVision/image/conversion.hpp>
#include <aliceVision/system/main.hpp>
#include <aliceVision/utils/filesIO.hpp>
#include <aliceVision/cmdline/cmdline.hpp>
#include <aliceVision/camera/IntrinsicScaleOffsetDisto.hpp>

#include <boost/program_options.hpp>

#include <filesystem>
#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <iterator>
#include <iomanip>
#include <map>
#include <fstream>

// 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::camera;
using namespace aliceVision::geometry;
using namespace aliceVision::image;
using namespace aliceVision::sfmData;

namespace po = boost::program_options;
namespace fs = std::filesystem;

bool exportToMatlab(const SfMData& sfm_data, const std::string& outDirectory)
{
    const double unknownScale = 0.0;
    // WARNING: Observation::id_feat is used to put the ID of the 3D landmark.
    std::map<IndexT, std::vector<Observation>> observationsPerView;

    {
        const std::string landmarksFilename = (fs::path(outDirectory) / "scene.landmarks").string();
        std::ofstream landmarksFile(landmarksFilename);
        landmarksFile << "# landmarkId X Y Z\n";
        for (const auto& s : sfm_data.getLandmarks())
        {
            const IndexT landmarkId = s.first;
            const Landmark& landmark = s.second;
            landmarksFile << landmarkId << " " << landmark.X[0] << " " << landmark.X[1] << " " << landmark.X[2] << "\n";
            for (const auto& obs : landmark.getObservations())
            {
                const IndexT obsView = obs.first;  // The ID of the view that provides this 2D observation.
                observationsPerView[obsView].push_back(Observation(obs.second.getCoordinates(), landmarkId, unknownScale));
            }
        }
        landmarksFile.close();
    }

    // Export observations per view
    for (const auto& obsPerView : observationsPerView)
    {
        const std::vector<Observation>& viewObservations = obsPerView.second;
        const IndexT viewId = obsPerView.first;

        const std::string viewFeatFilename = (fs::path(outDirectory) / (std::to_string(viewId) + ".reconstructedFeatures")).string();
        std::ofstream viewFeatFile(viewFeatFilename);
        viewFeatFile << "# landmarkId x y\n";
        for (const Observation& obs : viewObservations)
        {
            viewFeatFile << obs.getFeatureId() << " " << obs.getX() << " " << obs.getY() << "\n";
        }
        viewFeatFile.close();
    }

    // Expose camera poses
    {
        const std::string cameraPosesFilename = (fs::path(outDirectory) / "cameras.poses").string();
        std::ofstream cameraPosesFile(cameraPosesFilename);
        cameraPosesFile << "# viewId R11 R12 R13 R21 R22 R23 R31 R32 R33 C1 C2 C3\n";
        for (const auto& v : sfm_data.getViews())
        {
            const View& view = *v.second.get();
            if (!sfm_data.isPoseAndIntrinsicDefined(view))
                continue;

            const Pose3 pose = sfm_data.getPose(view).getTransform();
            cameraPosesFile << view.getViewId() << " " << pose.rotation()(0, 0) << " " << pose.rotation()(0, 1) << " " << pose.rotation()(0, 2) << " "
                            << pose.rotation()(1, 0) << " " << pose.rotation()(1, 1) << " " << pose.rotation()(1, 2) << " " << pose.rotation()(2, 0)
                            << " " << pose.rotation()(2, 1) << " " << pose.rotation()(2, 2) << " " << pose.center()(0) << " " << pose.center()(1)
                            << " " << pose.center()(2) << "\n";
        }
        cameraPosesFile.close();
    }

    // Expose camera intrinsics
    // Note: we export it per view, It is really redundant but easy to parse.
    {
        const std::string cameraIntrinsicsFilename = (fs::path(outDirectory) / "cameras.intrinsics").string();
        std::ofstream cameraIntrinsicsFile(cameraIntrinsicsFilename);
        cameraIntrinsicsFile << "# viewId pinhole f u0 v0\n"
                                "# viewId radial1 f u0 v0 k1\n"
                                "# viewId radial3 f u0 v0 k1 k2 k3\n"
                                "# viewId brown f u0 v0 k1 k2 k3 t1 t2\n"
                                "# viewId fisheye4 f u0 v0 k1 k2 k3 k4\n"
                                "# viewId fisheye1 f u0 v0 k1\n";

        for (const auto& v : sfm_data.getViews())
        {
            const View& view = *v.second.get();
            if (!sfm_data.isPoseAndIntrinsicDefined(view))
            {
                continue;
            }

            const auto & intrinsics = sfm_data.getIntrinsicSharedPtr(view.getIntrinsicId());
            cameraIntrinsicsFile << view.getViewId() << " " << camera::EINTRINSIC_enumToString(intrinsics->getType());
            for (double p : intrinsics->getParameters())
            {
                cameraIntrinsicsFile << " " << p;
            }

            auto casted = camera::IntrinsicScaleOffsetDisto::cast(intrinsics);
            if (casted)
            {
                auto disto = casted->getDistortion();
                if (disto)
                {
                    for (double p : disto->getParameters())
                    {
                        cameraIntrinsicsFile << " " << p;
                    }
                }
            }

            cameraIntrinsicsFile << "\n";
        }
        cameraIntrinsicsFile.close();
    }

    return true;
}

int aliceVision_main(int argc, char* argv[])
{
    // command-line parameters
    std::string sfmDataFilename;
    std::string outputFolder;

    // clang-format off
    po::options_description requiredParams("Required parameters");
    requiredParams.add_options()
        ("input,i", po::value<std::string>(&sfmDataFilename)->required(),
         "SfMData file.")
        ("output,o", po::value<std::string>(&outputFolder)->required(),
         "Output folder.");
    // clang-format on

    CmdLine cmdline("AliceVision exportMatlab");
    cmdline.add(requiredParams);
    if (!cmdline.execute(argc, argv))
    {
        return EXIT_FAILURE;
    }

    // export
    {
        // Create output dir
        if (!utils::exists(outputFolder))
            fs::create_directory(outputFolder);

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

        if (!exportToMatlab(sfmData, outputFolder))
            return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}
