/*
Import a ZDF point cloud and downsample it.
*/

#include <Zivid/CloudVisualizer.h>
#include <Zivid/Zivid.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <iostream>

Zivid::PointCloud downsample(const Zivid::PointCloud &, int);
Eigen::MatrixXi downsampleAndRound(const Eigen::MatrixXi &, int);
Eigen::MatrixXf gridSum(const Eigen::MatrixXf &, int);
Eigen::MatrixXf lineSum(Eigen::MatrixXf, int);
float nanToZero(float);
void visualizePointCloud(const Zivid::PointCloud &, Zivid::Application &);

int main()
{
    try
    {
        Zivid::Application zivid;

        std::string filename = "Zivid3D.zdf";
        std::cout << "Reading " << filename << " point cloud" << std::endl;
        Zivid::Frame frame(filename);

        auto pointCloud = frame.getPointCloud();

        auto downsamplingFactor = 4;

        auto pointCloudDownsampled = downsample(pointCloud, downsamplingFactor);

        visualizePointCloud(pointCloud, zivid);
        visualizePointCloud(pointCloudDownsampled, zivid);
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << Zivid::toString(e) << std::endl;
        return EXIT_FAILURE;
    }
}

Zivid::PointCloud downsample(const Zivid::PointCloud &pointCloud, int downsamplingFactor)
{
    /*
	Function for downsampling a Zivid point cloud. The downsampling factor represents the denominator
	of a fraction that represents the size of the downsampled point cloud relative to the original
	point cloud, e.g. 2 - one-half,  3 - one-third, 4 one-quarter, etc.
	*/

    if((pointCloud.height() % downsamplingFactor) || (pointCloud.width() % downsamplingFactor))
    {
        throw std::invalid_argument("Downsampling factor (" + std::to_string(downsamplingFactor)
                                    + ") has to a factor of the width (" + std::to_string(pointCloud.width())
                                    + ") and height (" + std::to_string(pointCloud.height())
                                    + ") of the input point cloud.");
    }

    Eigen::MatrixXf x(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXf y(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXf z(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXi r(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXi g(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXi b(pointCloud.height(), pointCloud.width());
    Eigen::MatrixXf contrast(pointCloud.height(), pointCloud.width());

    for(size_t i = 0; i < pointCloud.height(); i++)
    {
        for(size_t j = 0; j < pointCloud.width(); j++)
        {
            x(i, j) = pointCloud(i, j).x;
            y(i, j) = pointCloud(i, j).y;
            z(i, j) = pointCloud(i, j).z;
            r(i, j) = pointCloud(i, j).red();
            g(i, j) = pointCloud(i, j).green();
            b(i, j) = pointCloud(i, j).blue();
            contrast(i, j) = pointCloud(i, j).contrast;
        }
    }

    Eigen::MatrixXi redDownsampled = downsampleAndRound(r, downsamplingFactor);
    Eigen::MatrixXi greenDownsampled = downsampleAndRound(g, downsamplingFactor);
    Eigen::MatrixXi blueDownsampled = downsampleAndRound(b, downsamplingFactor);

    std::function<float(float)> is_not_nan_functor = [](float f) { return !std::isnan(f); };
    std::function<float(float)> nan_to_zero_functor = nanToZero;

    Eigen::MatrixXf contrastNulled =
        (z.unaryExpr(is_not_nan_functor)).cwiseProduct(contrast.unaryExpr(nan_to_zero_functor));

    auto contrastWeight = gridSum(contrastNulled, downsamplingFactor);

    Eigen::MatrixXf xContrasted = x.cwiseProduct(contrastNulled);
    Eigen::MatrixXf yContrasted = y.cwiseProduct(contrastNulled);
    Eigen::MatrixXf zContrasted = z.cwiseProduct(contrastNulled);
    Eigen::MatrixXf contrastContrasted = contrast.cwiseProduct(contrastNulled);

    Eigen::MatrixXf xDownsampled = gridSum(xContrasted, downsamplingFactor).cwiseQuotient(contrastWeight);
    Eigen::MatrixXf yDownsampled = gridSum(yContrasted, downsamplingFactor).cwiseQuotient(contrastWeight);
    Eigen::MatrixXf zDownsampled = gridSum(zContrasted, downsamplingFactor).cwiseQuotient(contrastWeight);
    Eigen::MatrixXf contrastDownsampled = gridSum(contrastContrasted, downsamplingFactor).cwiseQuotient(contrastWeight);

    Zivid::PointCloud pointCloudDownsampled(redDownsampled.rows(), redDownsampled.cols());

    for(int i = 0; i < redDownsampled.rows(); i++)
    {
        for(int j = 0; j < redDownsampled.cols(); j++)
        {
            pointCloudDownsampled(i, j).setRgb(redDownsampled(i, j), greenDownsampled(i, j), blueDownsampled(i, j));
            pointCloudDownsampled(i, j).setContrast(contrastDownsampled(i, j));
            pointCloudDownsampled(i, j).x = xDownsampled(i, j);
            pointCloudDownsampled(i, j).y = yDownsampled(i, j);
            pointCloudDownsampled(i, j).z = zDownsampled(i, j);
        }
    }

    return pointCloudDownsampled;
}

Eigen::MatrixXi downsampleAndRound(const Eigen::MatrixXi &matrixi, int downsamplingFactor)
{
    Eigen::MatrixXf matrixf = matrixi.template cast<float>();

    std::function<float(float)> round_functor = [](float f) { return std::round(f); };

    return ((gridSum(matrixf, downsamplingFactor) / (downsamplingFactor * downsamplingFactor)).unaryExpr(round_functor))
        .template cast<int>();
}

Eigen::MatrixXf gridSum(const Eigen::MatrixXf &r, int downsamplingFactor)
{
    return lineSum(lineSum(r, downsamplingFactor).transpose(), downsamplingFactor).transpose();
}

Eigen::MatrixXf lineSum(Eigen::MatrixXf matrix, int downsamplingFactor)
{
    Eigen::Map<Eigen::MatrixXf> flattenedMatrixMap(matrix.data(),
                                                   downsamplingFactor,
                                                   matrix.rows() * matrix.cols() / downsamplingFactor);
    std::function<float(float)> nan_to_zero_functor = nanToZero;

    Eigen::MatrixXf flattenedMatrixNansRemoved = flattenedMatrixMap.unaryExpr(nan_to_zero_functor);

    Eigen::MatrixXf flattenedMatrixColwiseSum = flattenedMatrixNansRemoved.colwise().sum();

    Eigen::Map<Eigen::MatrixXf> reshapedMatrixMap(flattenedMatrixColwiseSum.data(),
                                                  matrix.rows() / downsamplingFactor,
                                                  matrix.cols());

    return reshapedMatrixMap;
}

float nanToZero(float x)
{
    if(std::isnan(x))
    {
        return 0;
    }
    else
    {
        return x;
    }
}

void visualizePointCloud(const Zivid::PointCloud &pointCloud, Zivid::Application &zivid)
{
    std::cout << "Setting up visualization" << std::endl;
    Zivid::CloudVisualizer vis;
    zivid.setDefaultComputeDevice(vis.computeDevice());

    std::cout << "Displaying the point cloud" << std::endl;
    vis.showMaximized();
    vis.show(pointCloud);
    vis.resetToFit();

    std::cout << "Running the visualizer. Blocking until the window closes" << std::endl;
    vis.run();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_D_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_D_LOG_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/invlog_10.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/constant/log_2olog_10.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <tuple>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;

    template < class A0 >
    struct logarithm< A0, tag::not_simd_type, double>
    {
      static BOOST_FORCEINLINE void kernel_log(A0 a0,
                                    A0& dk,
                                    A0& hfsq,
                                    A0& s,
                                    A0& r,
                                    A0& f) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0, signed>;
        A0 x;
        i_t k;
        std::tie(x, k) = fast_(frexp)(a0);
        const i_t x_lt_sqrthf = (Sqrt_2o_2<A0>() > x) ? Mone<i_t>() : Zero<i_t>();
        k += x_lt_sqrthf;
        f = dec(x+bitwise_and(x, genmask(x_lt_sqrthf)));
        dk = tofloat(k);
        s = f/(Two<A0>()+f);
        A0 z = sqr(s);
        A0 w = sqr(z);
        A0 t1= w*horn<A0,
                      0x3fd999999997fa04ll,
                      0x3fcc71c51d8e78afll,
                      0x3fc39a09d078c69fll
                      > (w);
        A0 t2= z*horn<A0,
                      0x3fe5555555555593ll,
                      0x3fd2492494229359ll,
                      0x3fc7466496cb03dell,
                      0x3fc2f112df3e5244ll
                      > (w);
        r = t2+t1;
        hfsq = Half<A0>()*sqr(f);
      }

      static BOOST_FORCEINLINE A0 log( A0 a0) BOOST_NOEXCEPT
      {
        // ln(2)hi  =  6.93147180369123816490e-01  or  0x3fe62e42fee00000
        // ln(2)lo  =  1.90821492927058770002e-10  or  0x3dea39ef35793c76
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return  dk*Constant<A0, 0x3fe62e42fee00000ll>()-
          ((hfsq-(s*(hfsq+R)+dk*Constant<A0, 0x3dea39ef35793c76ll>()))-f);
      }

      static BOOST_FORCEINLINE A0 log2(A0 a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
      }

      static BOOST_FORCEINLINE A0 log10(A0 a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (a0 == Inf<A0>()) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (is_ltz(a0)) return Nan<A0>();
      #else
        if (is_nan(a0)||is_ltz(a0)) return Nan<A0>();
      #endif
        if (is_eqz(a0)) return Minf<A0>();
        A0 dk, hfsq, s, R, f;
        kernel_log(a0, dk, hfsq, s, R, f);
        return -(hfsq-(s*(hfsq+R))-f)*Invlog_10<A0>()+dk*Log_2olog_10<A0>();
      }
    };

  }
} }
#endif