#include <cilantro/icp_common_instances.hpp>
#include <cilantro/point_cloud.hpp>
#include <cilantro/common_renderables.hpp>
#include <cilantro/timer.hpp>
#include <iostream>
#include <fstream>
#include "steven.h"

void generate_input_data(cilantro::PointCloud3f &dst,
                         cilantro::PointCloud3f &src,
                         cilantro::RigidTransform3f &tf_ref)
{
    //dst.gridDownsample(0.005f);

    // Create a distorted and transformed version of the dst point cloud
    src = dst;
//     for (size_t i = 0; i < src.size(); i++) {
//         src.points.col(i) += 0.01f*Eigen::Vector3f::Random();
//         src.normals.col(i) += 0.02f*Eigen::Vector3f::Random();
//         src.normals.col(i).normalize();
//         src.colors.col(i) += 0.05f*Eigen::Vector3f::Random();
//     }

    Eigen::Matrix3f tmp;
    tmp = Eigen::AngleAxisf(-0.1f ,Eigen::Vector3f::UnitZ()) *
          Eigen::AngleAxisf(0.1f, Eigen::Vector3f::UnitY()) *
          Eigen::AngleAxisf(-0.1f, Eigen::Vector3f::UnitX());
    tf_ref.linear() = tmp;

    tf_ref.translation() = Eigen::Vector3f(-0.20f, -0.05f, 0.10f);

    src.transform(tf_ref);
}

int main(int argc, char ** argv) {
    if (argc < 2) {
        std::cout << "Please provide path to PLY file." << std::endl;
        return 0;
    }

    cilantro::RigidTransform3f cumulativeMatrix = cilantro::RigidTransform3f::Identity();
    cilantro::PointCloud3f cumulativeCloud;

    for (int i = 0 ; i < 100 ; i++)
    {
        char dstName[30];
        char srcName[30];

        sprintf(dstName, "model/model_back/1_%05d.ply", i);
        sprintf(srcName, "model/model_back/1_%05d.ply", i + 1);

        cilantro::PointCloud3f dst(dstName), src(srcName);

         dst.transform(cumulativeMatrix);
         src.transform(cumulativeMatrix);

        if (!dst.hasNormals()) {
            std::cout << "Input cloud is empty or does not have normals!" << std::endl;
            return 0;
        }

        cilantro::SimpleCombinedMetricRigidICP3f icp(dst.points, dst.normals, src.points);

        // Parameter setting
        icp.setMaxNumberOfOptimizationStepIterations(1).setPointToPointMetricWeight(0.0f).setPointToPlaneMetricWeight(1.0f);
        icp.correspondenceSearchEngine().setMaxDistance(30);
        icp.setConvergenceTolerance(1e-4f).setMaxNumberOfIterations(30);

        cilantro::RigidTransform3f tf_est = icp.estimate().getTransform();

        std::cout << "Iterations performed: " << icp.getNumberOfPerformedIterations() << std::endl;
        std::cout << "Has converged: " << icp.hasConverged() << std::endl;
        std::cout << "ESTIMATED transformation R:" << std::endl << tf_est.matrix() << std::endl;

        if (i == 0)
        {
            cumulativeCloud.append(dst);
        }

        src.transform(tf_est);

        cumulativeCloud.append(src);

        cumulativeMatrix = tf_est * cumulativeMatrix;
    }


    using namespace steven;
    steven::PlyFile outfile;
    outfile.add_properties_to_element("vertex", { "x", "y", "z" },
        Type::FLOAT32, cumulativeCloud.points.size() / 3, reinterpret_cast<uint8_t*>(cumulativeCloud.points.data()), Type::INVALID, 0);

    std::filebuf fb_binary;
    fb_binary.open("model/new1.ply", std::ios::out | std::ios::binary);
    std::ostream outstream_binary(&fb_binary);

    outfile.write(outstream_binary, true);

    return 0;
}