// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <deque>
#include <vector>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/concepts/linestring_concept.hpp>

#include <boost/geometry/util/write_dsv.hpp>

#include <test_common/test_point.hpp>
#include <test_geometries/all_custom_linestring.hpp>
#include <test_geometries/wrapped_boost_array.hpp>

template <typename Geometry>
void test_linestring()
{
    BOOST_CONCEPT_ASSERT( (bg::concept::Linestring<Geometry>) );
    BOOST_CONCEPT_ASSERT( (bg::concept::ConstLinestring<Geometry>) );

    Geometry geometry;
    typedef typename bg::point_type<Geometry>::type P;

    bg::clear(geometry);
    BOOST_CHECK_EQUAL(boost::size(geometry), 0);

    bg::append(geometry, bg::make<P>(1, 2));
    BOOST_CHECK_EQUAL(boost::size(geometry), 1);

    bg::append(geometry, bg::make<P>(3, 4));
    BOOST_CHECK_EQUAL(boost::size(geometry), 2);

    bg::traits::resize<Geometry>::apply(geometry, 1);
    BOOST_CHECK_EQUAL(boost::size(geometry), 1);

    //std::cout << bg::dsv(geometry) << std::endl;
    P p = *boost::begin(geometry);
    //std::cout << bg::dsv(p) << std::endl;
    BOOST_CHECK_EQUAL(bg::get<0>(p), 1);
    BOOST_CHECK_EQUAL(bg::get<1>(p), 2);

    bg::clear(geometry);
    BOOST_CHECK_EQUAL(boost::size(geometry), 0);
}

template <typename Point>
void test_all()
{
    test_linestring<bg::model::linestring<Point> >();
    test_linestring<test::wrapped_boost_array<Point, 10> >();
    test_linestring<all_custom_linestring<Point> >();
}

int test_main(int, char* [])
{
    test_all<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}


#include <boost/assign/std/vector.hpp>
#include <cradle/geometry/line_strip.hpp>

#include <cradle/test.hpp>

double const tolerance = 0.00001;

using namespace boost::assign;
using namespace cradle;

TEST_CASE("actual_poly_test")
{
    std::vector<vertex2> vertices;
    vertices += make_vector<double>(-1, 0), make_vector<double>(1, 0),
        make_vector<double>(0, 1);
    polygon2 original;
    initialize(&original.vertices, vertices);

    std::vector<line_strip> strips;
    std::vector<line_segment<2, double>> segments;

    for (polygon2_edge_view ev(original); !ev.done(); ev.advance())
        segments.push_back(line_segment<2, double>(ev.p0(), ev.p1()));

    strips = connect_line_segments(segments, tolerance);

    REQUIRE(is_polygon(strips, tolerance));
    REQUIRE(as_polygon(strips, tolerance));

    optional<polygon2> poly;
    as_polygon(&poly, strips, tolerance);
    REQUIRE(poly);
    REQUIRE(almost_equal(original, poly.get()));
}

TEST_CASE("not_connected_test")
{
    std::vector<vertex2> vertices;
    vertices += make_vector<double>(-1, 0), make_vector<double>(1, 0),
        make_vector<double>(0, 1);
    polygon2 original;
    initialize(&original.vertices, vertices);

    std::vector<line_strip> strips;
    std::vector<line_segment<2, double>> segments;

    polygon2_edge_view ev(original);
    ev.advance();
    for (; !ev.done(); ev.advance())
        segments.push_back(line_segment<2, double>(ev.p0(), ev.p1()));

    strips = connect_line_segments(segments, tolerance);

    REQUIRE(!is_polygon(strips, tolerance));
    REQUIRE(!as_polygon(strips, tolerance));

    optional<polygon2> poly;
    as_polygon(&poly, strips, tolerance);
    REQUIRE(!poly);
}

TEST_CASE("extraneous_segments_test")
{
    std::vector<vertex2> vertices;
    vertices += make_vector<double>(-1, 0), make_vector<double>(1, 0),
        make_vector<double>(0, 1);
    polygon2 original;
    initialize(&original.vertices, vertices);

    std::vector<line_strip> strips;
    std::vector<line_segment<2, double>> segments;

    for (polygon2_edge_view ev(original); !ev.done(); ev.advance())
        segments.push_back(line_segment<2, double>(ev.p0(), ev.p1()));

    segments.push_back(line_segment<2, double>(
        make_vector<double>(2, 0), make_vector<double>(2, 1)));

    strips = connect_line_segments(segments, tolerance);

    REQUIRE(!is_polygon(strips, tolerance));
    REQUIRE(!as_polygon(strips, tolerance));

    optional<polygon2> poly;
    as_polygon(&poly, strips, tolerance);
    REQUIRE(!poly);
}

TEST_CASE("multiple_polys_test")
{
    polygon2 original;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(-1, 0), make_vector<double>(1, 0),
            make_vector<double>(0, 1);
        initialize(&original.vertices, vertices);
    }

    std::vector<line_strip> strips;
    std::vector<line_segment<2, double>> segments;

    for (polygon2_edge_view ev(original); !ev.done(); ev.advance())
        segments.push_back(line_segment<2, double>(ev.p0(), ev.p1()));

    strips = connect_line_segments(segments, tolerance);

    polygon2 other;
    {
        std::vector<vertex2> vertices;
        vertices += make_vector<double>(2, 0), make_vector<double>(3, 0),
            make_vector<double>(0, 1);
        initialize(&other.vertices, vertices);
    }

    for (polygon2_edge_view ev(other); !ev.done(); ev.advance())
        segments.push_back(line_segment<2, double>(ev.p0(), ev.p1()));

    strips = connect_line_segments(segments, tolerance);

    REQUIRE(!is_polygon(strips, tolerance));
    REQUIRE(!as_polygon(strips, tolerance));

    optional<polygon2> poly;
    as_polygon(&poly, strips, tolerance);
    REQUIRE(!poly);
}


// -*- mode: C++; tab-width: 4; c-basic-offset: 4 -*-
//
/// \author Seppo Enarvi
/// \date 2003 - 2005
//
// Abstract data types and algorithms of analytic geometry

#include <boost/test/included/unit_test_framework.hpp>
#include <boost/smart_ptr.hpp>

#include "cartesian_vector_test.h"

using boost::unit_test::test_suite;
using namespace boost;

test_suite *
init_unit_test_suite(int argc, char * argv[])
{
    test_suite * tests(BOOST_TEST_SUITE("geomtests"));

    shared_ptr<cartesian_vector_test> cvt(new cartesian_vector_test());
    tests->add(BOOST_CLASS_TEST_CASE(&cartesian_vector_test::run, cvt));

    return tests;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the fixed_point basic narrowing constructors.

#define BOOST_TEST_MODULE test_negatable_basic_narrowing_constructors
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_basic_narrowing_constructors)
{
  bool result = true;

  {
    // This fixed-point negatable type has only 2 IntegralRange digits.
    // For all practical purposes, all constructions and assignments
    // must be explicit.
    //
    // For example, the following should fail.
    //  fixed_point_type x = std::uint8_t(1U);
    // But the following should succeed.
    //  fixed_point_type x(std::uint8_t(1U));

    typedef boost::fixed_point::negatable<2, -5> fixed_point_type;

    fixed_point_type x(std::uint8_t(1U));

    x = fixed_point_type(1U);

    x = static_cast<fixed_point_type>(1U);

    result &= (x == 1U);
  }

  {
    typedef boost::fixed_point::negatable<8, -7> fixed_point_type;

    // This is OK because 8 range digits in fixed_point_type
    // allow for non-narrowing conversion to boost::uint8_t.
    fixed_point_type x = boost::uint8_t(1U);

    // Here we require explicit construction from 16-bit
    // unsiged integer boost::uint16_t.
    // In other words,
    // fixed_point_type z = boost::uint16_t(1U);
    // will not work because uint16_t has more
    // digits than the IntegralRange digits of y.

    // But using explicit construction, this code
    // sequence is OK.
    fixed_point_type y(boost::uint16_t(1U));

    result &= (x == 1U);
    result &= (y == 1U);
  }

  {
    typedef boost::fixed_point::negatable<16, -15> fixed_point_type;

    // This is OK because 16 range digits in fixed_point_type
    // allow for non-narrowing conversion to boost::uint16_t.
    fixed_point_type x = boost::uint16_t(1U);

    // This is OK because conversion from 8-bit unsigned
    // integer to fixed_point_type is non-narrowing anyway.
    fixed_point_type y = boost::uint8_t(1U);

    // Here we require explicit construction from 32-bit
    // unsiged integer boost::uint32_t.
    // In other words,
    // fixed_point_type z = boost::uint32_t(1U);
    // will not work because uint32_t has more
    // digits than the IntegralRange digits of z.

    // But using explicit construction, this code
    // sequence is OK.
    fixed_point_type z(boost::uint32_t(1U));

    result &= (x == 1U);
    result &= (y == 1U);
    result &= (z == 1U);
  }

  {
    typedef boost::fixed_point::negatable<64, -63> fixed_point_type;

    fixed_point_type x;

    // These are all OK because x has 64 IntegralRange digits.
    x = boost::uint8_t(1U);
    x = boost::uint16_t(1U);
    x = boost::uint32_t(1U);
    x = boost::uint64_t(1U);

    result &= (x == 1U);
  }

  // If the code compiles, then the test passes.
  BOOST_CHECK(result);
}


#pragma once

#include <armadillo>

class BoundaryConditions;

/*!
 * \brief The MatrixEquation class sets up the matrix equation from the system
 * of equations found from the governing equations.
 *
 * The governing equations are discretized using a implicit backward difference
 * method, as detailed in
 *  - Gas composition tracking in transient pipeline flow, Chaczykowski et. al., Journal of Natural Gas Science and Engineering 55 (2018)
 *
 * This class supports all combinations of inlet and outlet boundary conditions,
 * so we can for example use flow at the inlet, pressure at the outlet, and
 * temperature at the inlet, or any other combination. Also supports
 * over-determined systems.
 *
 * This class is designed for use together with a Discretizer instance,
 * which sets up the individual elements of the matrix. MatrixEquation just
 * fills in the matrix using results from Discretizer, and solves the matrix
 * equation.
 *
 * The matrix equation is solved using the sparse matrix solver arma::spsolve,
 * which uses superlu internally. This solver only works for critically
 * determined systems, not for over-determined systems. In that case the regular
 * solver is used, and the sparse matrix has to be converted to a regular
 * matrix.
 *
 * Most of the elements in the matrix are zero, so we use sparse matrices. The
 * non-zero elements are all located near the diagonal.
 *
 * \see Discretizer
 */
class MatrixEquation
{
public:
    //! Declared to avoid the inline compiler-generated default destructor.
    ~MatrixEquation();

    /*!
     * \brief Fill in the coefficient matrix A (MatrixEquation::m_coefficients)
     * and constants vector b (MatrixEquation::m_constants) in the matrix
     * equation Ax = b.
     *
     * Here `term_i` and `term_ipp` refer to the coefficients of the variables
     * in the discretized governing equations (flow, pressure and temperature --
     * \f$y_i\f$ in general). See for example eq. (14)-(16) in
     * Gas composition tracking in transient pipeline flow.
     * These coefficients are calculated by the Discretizer class.
     *
     * The boundary terms are the constant terms, the elements of the vector
     * \f$b\f$ in the matrix equation \f$Ax = b\f$.
     *
     * MatrixEquation supports all combinations of inlet and outlet boundary
     * conditions, but giving more than 3 boundary conditions lead to an
     * over-determined system, and giving less than 3 leads to a
     * under-determined system.
     *
     * \see Discretizer::discretize()
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     * \param boundaryConditions The boundary conditions
     * \param term_i Matrix coefficients at point i (see above)
     * \param term_ipp Matrix coefficients at point i+1 (see above)
     * \param boundaryTerms The boundary terms (see above)
     */
    void fillCoefficientMatrixAndConstantsVector(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables,
            const BoundaryConditions& boundaryConditions,
            const arma::cube& term_i,
            const arma::cube& term_ipp,
            const arma::mat& boundaryTerms); // fills self matrix and vector

    /*!
     * \brief Solve the matrix equation Ax = b.
     *
     * This solves the matrix equation, after the coefficient matrix
     * MatrixEquation::m_coefficients and constant vector
     * MatrixEquation::m_coefficients has been filled in by
     * fillCoefficientMatrixAndConstantsVector().
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     * \param boundaryConditions The boundary conditions
     * \return The solution of the matrix equation (x), reshaped to contain
     * flow, pressure and temperature in separate columns.
     */
    arma::mat solve(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables,
            const BoundaryConditions& boundaryConditions) const;

    //! Get coefficient matrix A. For testing purposes.
    const arma::sp_mat& coefficients() const { return m_coefficients; }
    //! Get constants vector b. For testing purposes.
    const arma::vec& constants() const { return m_constants; }

private:
    arma::sp_mat m_coefficients; //!< Coefficient matrix A
    arma::vec m_constants; //!< Constants vector b

    /*!
     * \brief Reshape output from solving the matrix equation into a matrix
     * containing flow, pressure and temperature as columns.
     *
     * Reshapes the vector x, the solution of the matrix equation Ax = b,
     * into a matrix with three columns. Also inserts the boundary conditions
     * where at the correct locations.
     *
     * Supports all combinations of inlet/outlet boundary settings.
     *
     * \param x Solution of matrix equation
     * \param boundaryConditions Boundary conditions
     * \param nGridPoints Number of grid points
     * \param nVariables Number of variables
     * \return Matrix containing flow, pressure and temperature columns.
     */
    arma::mat reshapeSolverOutput(
            const arma::vec& x,
            const BoundaryConditions& boundaryConditions,
            const arma::uword nGridPoints,
            const arma::uword nVariables) const;

    /*!
     * \brief Internal method that solves the matrix equation.
     *
     * \see solve()
     * \return Vector x, solution of the matrix equation Ax = b.
     */
    arma::vec solveMatrixEquation() const;
};


// Copyright 2018-2019 Coen Tempelaars (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * arbiterGameData.hpp
 *
 *  Created on: Dec 27, 2018
 *      Author: Coen Tempelaars
 */

#ifndef ARBITERGAMEDATA_HPP_
#define ARBITERGAMEDATA_HPP_

#include "gameData.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>

class ArbiterGameData : public GameData {
public:
    ArbiterGameData();

    void refresh(const GameData&);

    bool ballIsInNegativeGoal() const;
    bool ballIsInPositiveGoal() const;
    bool ballIsAcrossNegativeGoalline() const;
    bool ballIsAcrossPositiveGoalline() const;
    bool ballIsAcrossSideLine() const;
    bool isScrum() const;

    /* the following attributes are deliberately public */
    TeamID teamLastHoldingBall;    // there is always a team that last touched the ball
    RobotID robotLastHoldingBall;  // there is always a robot that last touched the ball

private:
    typedef boost::accumulators::accumulator_set<float, boost::accumulators::stats<boost::accumulators::tag::rolling_mean > > MovingAverageAccumulator;
    MovingAverageAccumulator ballSpeedAccumulator;
    MovingAverageAccumulator ballToRobotDistanceAccumulatorTeamA;
    MovingAverageAccumulator ballToRobotDistanceAccumulatorTeamB;
};

#endif /* ARBITERGAMEDATA_HPP_ */


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "flip_dataset.h"
#include <dlib/data_io.h>
#include <dlib/dir_nav.h>
#include <string>
#include "common.h"
#include <dlib/image_transforms.h>
#include <dlib/optimization.h>
#include <dlib/image_processing.h>

using namespace dlib;
using namespace std;

// ----------------------------------------------------------------------------------------

std::vector<long> align_points(
    const std::vector<dpoint>& from,
    const std::vector<dpoint>& to,
    double min_angle = -90*pi/180.0,
    double max_angle = 90*pi/180.0,
    long num_angles = 181
)
/*!
    ensures
        - Figures out how to align the points in from with the points in to.  Returns an
          assignment array A that indicates that from[i] matches with to[A[i]].

          We use the Hungarian algorithm with a search over reasonable angles.  This method
          works because we just need to account for a translation and a mild rotation and
          nothing else.  If there is any other more complex mapping then you probably don't
          have landmarks that make sense to flip.
!*/
{
    DLIB_CASSERT(from.size() == to.size());

    std::vector<long> best_assignment;
    double best_assignment_cost = std::numeric_limits<double>::infinity();

    matrix<double> dists(from.size(), to.size());
    matrix<long long> idists;

    for (auto angle : linspace(min_angle, max_angle, num_angles))
    {
        auto rot = rotation_matrix(angle);
        for (long r = 0; r < dists.nr(); ++r)
        {
            for (long c = 0; c < dists.nc(); ++c)
            {
                dists(r,c) = length_squared(rot*from[r]-to[c]);
            }
        }

        idists = matrix_cast<long long>(-round(std::numeric_limits<long long>::max()*(dists/max(dists))));

        auto assignment = max_cost_assignment(idists);
        auto cost = assignment_cost(dists, assignment);
        if (cost < best_assignment_cost)
        {
            best_assignment_cost = cost;
            best_assignment = std::move(assignment);
        }
    }

    // Now compute the alignment error in terms of average distance moved by each part.  We
    // do this so we can give the user a warning if it's impossible to make a good
    // alignment.
    running_stats<double> rs;
    std::vector<dpoint> tmp(to.size());
    for (size_t i = 0; i < to.size(); ++i)
        tmp[best_assignment[i]] = to[i];
    auto tform = find_similarity_transform(from, tmp);
    for (size_t i = 0; i < from.size(); ++i)
        rs.add(length(tform(from[i])-tmp[i]));
    if (rs.mean() > 0.05)
    {
        cout << "WARNING, your dataset has object part annotations and you asked imglab to " << endl;
        cout << "flip the data.  Imglab tried to adjust the part labels so that the average" << endl;
        cout << "part layout in the flipped dataset is the same as the source dataset.  " << endl;
        cout << "However, the part annotation scheme doesn't seem to be left-right symmetric." << endl;
        cout << "You should manually review the output to make sure the part annotations are " << endl;
        cout << "labeled as you expect." << endl;
    }

    return best_assignment;
}

// ----------------------------------------------------------------------------------------

std::map<string,dpoint> normalized_parts (
    const image_dataset_metadata::box& b
)
{
    auto tform = dlib::impl::normalizing_tform(b.rect);
    std::map<string,dpoint> temp;
    for (auto& p : b.parts)
        temp[p.first] = tform(p.second);
    return temp;
}

// ----------------------------------------------------------------------------------------

std::map<string,dpoint> average_parts (
    const image_dataset_metadata::dataset& data
)
/*!
    ensures
        - returns the average part layout over all objects in data.  This is done by
          centering the parts inside their rects and then averaging all the objects.
!*/
{
    std::map<string,dpoint> psum;
    std::map<string,double> pcnt;
    for (auto& image : data.images)
    {
        for (auto& box : image.boxes)
        {
            for (auto& p : normalized_parts(box))
            {
                psum[p.first] += p.second;
                pcnt[p.first] += 1;
            }
        }
    }

    // make into an average
    for (auto& p : psum)
        p.second /= pcnt[p.first];

    return psum;
}

// ----------------------------------------------------------------------------------------

void make_part_labeling_match_target_dataset (
    const image_dataset_metadata::dataset& target,
    image_dataset_metadata::dataset& data
)
/*!
    This function tries to adjust the part labels in data so that the average part layout
    in data is the same as target, according to the string labels.  Therefore, it doesn't
    adjust part positions, instead it changes the string labels on the parts to achieve
    this.  This really only makes sense when you flipped a dataset that contains left-right
    symmetric objects and you want to remap the part labels of the flipped data so that
    they match the unflipped data's annotation scheme.
!*/
{
    auto target_parts = average_parts(target);
    auto data_parts = average_parts(data);

    // Convert to a form align_points() understands.  We also need to keep track of the
    // labels for later.
    std::vector<dpoint> from, to;
    std::vector<string> from_labels, to_labels;
    for (auto& p : target_parts)
    {
        from_labels.emplace_back(p.first);
        from.emplace_back(p.second);
    }
    for (auto& p : data_parts)
    {
        to_labels.emplace_back(p.first);
        to.emplace_back(p.second);
    }

    auto assignment = align_points(from, to);
    // so now we know that from_labels[i] should replace to_labels[assignment[i]]
    std::map<string,string> label_mapping;
    for (size_t i = 0; i < assignment.size(); ++i)
        label_mapping[to_labels[assignment[i]]] = from_labels[i];

    // now apply the label mapping to the dataset
    for (auto& image : data.images)
    {
        for (auto& box : image.boxes)
        {
            std::map<string,point> temp;
            for (auto& p : box.parts)
                temp[label_mapping[p.first]] = p.second;
            box.parts = std::move(temp);
        }
    }
}

// ----------------------------------------------------------------------------------------

void flip_dataset(const command_line_parser& parser)
{
    image_dataset_metadata::dataset metadata, orig_metadata;
    string datasource;
    if (parser.option("flip"))
        datasource = parser.option("flip").argument();
    else
        datasource = parser.option("flip-basic").argument();
    load_image_dataset_metadata(metadata,datasource);
    orig_metadata = metadata;

    // Set the current directory to be the one that contains the
    // metadata file. We do this because the file might contain
    // file paths which are relative to this folder.
    set_current_dir(get_parent_directory(file(datasource)));

    const string metadata_filename = get_parent_directory(file(datasource)).full_name() +
        directory::get_separator() + "flipped_" + file(datasource).name();

    array2d<rgb_pixel> img, temp;
    for (unsigned long i = 0; i < metadata.images.size(); ++i)
    {
        file f(metadata.images[i].filename);
        string filename = get_parent_directory(f).full_name() + directory::get_separator() + "flipped_" + to_png_name(f.name());

        load_image(img, metadata.images[i].filename);
        flip_image_left_right(img, temp);
        if (parser.option("jpg"))
        {
            filename = to_jpg_name(filename);
            save_jpeg(temp, filename,JPEG_QUALITY);
        }
        else if (parser.option("webp"))
        {
            filename = to_webp_name(filename);
            const float webp_quality = std::stof(parser.option("webp").argument());
            save_webp(temp, filename, webp_quality);
        }
        else
        {
            save_png(temp, filename);
        }

        for (unsigned long j = 0; j < metadata.images[i].boxes.size(); ++j)
        {
            metadata.images[i].boxes[j].rect = impl::flip_rect_left_right(metadata.images[i].boxes[j].rect, get_rect(img));

            // flip all the object parts
            for (auto& part : metadata.images[i].boxes[j].parts)
            {
                part.second = impl::flip_rect_left_right(rectangle(part.second,part.second), get_rect(img)).tl_corner();
            }
        }

        metadata.images[i].filename = filename;
    }

    if (!parser.option("flip-basic"))
        make_part_labeling_match_target_dataset(orig_metadata, metadata);

    save_image_dataset_metadata(metadata, metadata_filename);
}

// ----------------------------------------------------------------------------------------


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

// can't do using namespace boost because then
// we get conflict with boost::default_dfs_visitor.
using namespace boost;

namespace std {
  template <typename T >
  std::istream& operator >> (std::istream & in, std::pair < T, T > &p)
  {
    in >> p.first >> p.second;
    return
      in;
  }
}

typedef adjacency_list<
  listS,                        // Store out-edges of each vertex in a std::list
  vecS,                         // Store vertex set in a std::vector
  directedS                     // The file dependency graph is directed
  > file_dep_graph;

typedef graph_traits<file_dep_graph>::vertex_descriptor vertex_t;
typedef graph_traits<file_dep_graph>::edge_descriptor edge_t;

template < typename Visitor > void
dfs_v1(const file_dep_graph & g, vertex_t u, default_color_type * color,
       Visitor vis)
{
  color[u] = gray_color;
  vis.discover_vertex(u, g);
  graph_traits < file_dep_graph >::out_edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei) {
    if (color[target(*ei, g)] == white_color) {
      vis.tree_edge(*ei, g);
      dfs_v1(g, target(*ei, g), color, vis);
    } else if (color[target(*ei, g)] == gray_color)
      vis.back_edge(*ei, g);
    else
      vis.forward_or_cross_edge(*ei, g);
  }
  color[u] = black_color;
  vis.finish_vertex(u, g);
}

template < typename Visitor > void
generic_dfs_v1(const file_dep_graph & g, Visitor vis)
{
  std::vector < default_color_type > color(num_vertices(g), white_color);
  graph_traits < file_dep_graph >::vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    if (color[*vi] == white_color)
      dfs_v1(g, *vi, &color[0], vis);
  }
}

struct dfs_visitor_default
{
  template <typename V, typename G > void
  discover_vertex(V, const G &)
  {
  }

  template <typename E, typename G > void
  tree_edge(E, const G &)
  {
  }

  template < typename E, typename G > void
  back_edge(E, const G &)
  {
  }

  template < typename E, typename G > void
  forward_or_cross_edge(E, const G &)
  {
  }

  template < typename V, typename G > void
  finish_vertex(V, const G &)
  {
  }
};

struct cycle_detector : public dfs_visitor_default
{
  cycle_detector(bool & cycle):
  has_cycle(cycle)
  {
  }
  void
  back_edge(edge_t, const file_dep_graph &)
  {
    has_cycle = true;
  }
  bool & has_cycle;
};

bool
has_cycle(const file_dep_graph & g)
{
  bool has_cycle = false;
  cycle_detector vis(has_cycle);
  generic_dfs_v1(g, vis);
  return has_cycle;
}

int
main()
{
  std::ifstream file_in("makefile-dependencies.dat");
  typedef graph_traits <file_dep_graph >::vertices_size_type size_type;
  size_type n_vertices;
  file_in >> n_vertices;        // read in number of vertices
  std::istream_iterator < std::pair < size_type,
    size_type > >input_begin(file_in), input_end;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ has trouble with the edge iterator constructor
  file_dep_graph g(n_vertices);
  while (input_begin != input_end) {
    size_type i, j;
    tie(i, j) = *input_begin++;
    add_edge(i, j, g);
  }
#else
  file_dep_graph g(input_begin, input_end, n_vertices);
#endif

  std::vector < std::string > name(num_vertices(g));
  std::ifstream name_in("makefile-target-names.dat");
  graph_traits < file_dep_graph >::vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    name_in >> name[*vi];

  assert(has_cycle(g) == false);
  return 0;
}


/* @file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Shapes/CoordinateSystemTransformation.h"
#include "Molassembler/Shapes/Shapes.h"
#include "Molassembler/Shapes/Data.h"
#include "Molassembler/Shapes/InertialMoments.h"
#include "Molassembler/Shapes/ContinuousMeasures.h"

#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Adaptors/Iota.h"

#include <iostream>
#include "Molassembler/Temple/Stringify.h"

using namespace Scine::Molassembler;
using namespace Shapes;

// From InertialMoments.cpp
extern const std::string& topName(Top top);
extern void randomlyRotate(Eigen::Ref<Continuous::PositionCollection> vs);

inline Vertex operator "" _v (unsigned long long v) { return Vertex(v); }

template<typename EnumType>
constexpr inline auto underlying(const EnumType e) {
  return static_cast<std::underlying_type_t<EnumType>>(e);
}

Continuous::PositionCollection addOrigin(const Continuous::PositionCollection& vs) {
  const unsigned N = vs.cols();
  Continuous::PositionCollection positions(3, N + 1);
  for(unsigned i = 0; i < N; ++i) {
    positions.col(i) = vs.col(i);
  }

  // Add origin point explicitly to consideration
  positions.col(N) = Eigen::Vector3d::Zero(3);
  return positions;
}

void distort(Eigen::Ref<Continuous::PositionCollection> positions, const double distortionNorm = 0.01) {
  const unsigned N = positions.cols();
  for(unsigned i = 0; i < N; ++i) {
    positions.col(i) += distortionNorm * Eigen::Vector3d::Random().normalized();
  }
}

const std::string& pointGroupString(PointGroup group) {
  static const std::vector<std::string> strings {
    "C1", "Ci", "Cs",
    "C2", "C3", "C4", "C5", "C6", "C7", "C8",
    "C2h", "C3h", "C4h", "C5h", "C6h", "C7h", "C8h",
    "C2v", "C3v", "C4v", "C5v", "C6v", "C7v", "C8v",
    "S4", "S6", "S8",
    "D2", "D3", "D4", "D5", "D6", "D7", "D8",
    "D2h", "D3h", "D4h", "D5h", "D6h", "D7h", "D8h",
    "D2d", "D3d", "D4d", "D5d", "D6d", "D7d", "D8d",
    "T", "Td", "Th",
    "O", "Oh",
    "I", "Ih",
    "Cinfv", "Dinfh"
  };

  return strings.at(
    static_cast<std::underlying_type<PointGroup>::type>(group)
  );
}

BOOST_AUTO_TEST_CASE(PointGroupMeasures, *boost::unit_test::label("Shapes")) {
  unsigned shapesTestedCount = 0;
  unsigned shapesPassCount = 0;

  for(const Shape shape : allShapes) {
#ifndef NDEBUG
    // Skip sizes greater 4 in debug builds
    if(size(shape) > 4) {
      continue;
    }
#endif

    // Even release builds bow out here
    if(size(shape) > 8) {
      continue;
    }

    auto positions = addOrigin(coordinates(shape));
    // distort(positions);
    auto normalized = Continuous::normalize(positions);

    // Add a random coordinate transformation
    normalized = Eigen::Quaterniond::UnitRandom().toRotationMatrix() * normalized;

    // Standardize the top
    const Top top = standardizeTop(normalized);
    if(top == Top::Asymmetric) {
      reorientAsymmetricTop(normalized);
    }

    const double pgCSM = Continuous::pointGroup(
      normalized,
      pointGroup(shape)
    );

    ++shapesTestedCount;

    if(pgCSM >= 0.01) {
      BOOST_TEST_MESSAGE(
        "Expected CSM(" << pointGroupString(pointGroup(shape))
        << ") < 0.01 for " << Shapes::name(shape)
        << "shape, got " << pgCSM << " (top is " << topName(top) << ")"
      );
    } else {
      ++shapesPassCount;
    }
  }

  BOOST_CHECK_MESSAGE(
    shapesTestedCount - shapesPassCount <= 1,
    "A single shape continuous symmetry measure is allowed to fail in the tests, but not multiple."
  );
}

std::ostream& operator << (std::ostream& os, const PointGroup group) {
  const auto elements = Elements::symmetryElements(group);
  const auto groupings = Elements::npGroupings(elements);
  os << pointGroupString(group) << ": {";
  for(const auto& element : elements) {
    os << element -> name() << ", ";
  }
  os << "}\n";
  for(const auto& iterPair : groupings) {
    for(const auto& grouping : iterPair.second) {
      os << "  np = " << iterPair.first << " along " << grouping.probePoint.transpose() << " -> " << Temple::stringify(grouping.groups) << "\n";
      os << "  ";
      os << Temple::stringifyContainer(grouping.groups,
        [&](const auto& grp) -> std::string {
          return Temple::stringifyContainer(grp,
            [&elements](const unsigned elementIdx) -> std::string {
              return elements.at(elementIdx)->name();
            }
          );
        }
      ) << "\n";
    }
  }

  return os;
}

BOOST_AUTO_TEST_CASE(PointGroupElementGroupings, *boost::unit_test::label("Shapes")) {
  const PointGroup limit = PointGroup::Ih;
  for(unsigned g = 0; g <= underlying(limit); ++g) {
    const auto pointGroup = static_cast<PointGroup>(g);
    const auto elements = Elements::symmetryElements(pointGroup);
    const auto groupings = Elements::npGroupings(elements);

    BOOST_CHECK_MESSAGE(
      groupings.count(1) > 0,
      "There is no single-point element group for point group " << pointGroupString(pointGroup)
    );

    bool anyGroupSizeMismatches = false;
    for(const auto& sizeGroupingsPair : groupings) {
      BOOST_CHECK_MESSAGE(
        elements.size() % sizeGroupingsPair.first == 0,
        "Grouping does not evenly divide the group "
        << pointGroupString(pointGroup) << ", G = " << elements.size()
        << ", group size = " << sizeGroupingsPair.first
      );

      bool pass = Temple::all_of(
        sizeGroupingsPair.second,
        [&](const Elements::ElementGrouping& grouping) -> bool {
          const auto size = grouping.groups.front().size();
          return Temple::all_of(
            grouping.groups,
            [&](const auto& groupSubVector) -> bool {
              return groupSubVector.size() == size;
            }
          );
        }
      );

      if(!pass) {
        anyGroupSizeMismatches = true;
      }

      BOOST_CHECK_MESSAGE(
        pass,
        "Not all subgroups of " << pointGroupString(pointGroup) << " have the same size!"
      );
    }
    if(anyGroupSizeMismatches) {
      std::cout << static_cast<PointGroup>(g);
    }
  }
}

// BOOST_AUTO_TEST_CASE(PaperCSMExamples, *boost::unit_test::label("Shapes")) {
//   /* From Continuous Symmetry Measures. 2. Symmetry Groups and the Tetrahedron,
//    * Zabrodsky, Peleg, Avnir. J. Am. Chem. Soc. 1993, 115, 8278-8289
//    *
//    * Example on p. 8283 from their ref 12
//    */
//   PositionCollection tetrahedralPositions(3, 4);
//   tetrahedralPositions.col(0) = Eigen::Vector3d {0.0, 0.0, 1.645};
//   tetrahedralPositions.col(1) = Eigen::Vector3d {0.0, 1.518860, -0.347028};
//   tetrahedralPositions.col(2) = Eigen::Vector3d {-1.286385, -0.700083, -0.391603};
//   tetrahedralPositions.col(3) = Eigen::Vector3d {1.179085, -0.755461, -0.372341};
//
//   const double expectedTdCSM = 0.17;
//   const double calculatedTd = csm::pointGroup(
//     Detail::normalize(tetrahedralPositions),
//     PointGroup::Td
//   ).value_or(1000);
//
//   std::cout << "Calculated Td CSM: " << calculatedTd << " (expect " << expectedTdCSM << ")\n";
//   BOOST_CHECK_CLOSE(expectedTdCSM, calculatedTd, 1);
//
//   /* Example from page 8284 */
//   PositionCollection c3vPositions(3, 4);
//   c3vPositions.col(0) = Eigen::Vector3d {0.0, 0.0, 1.645};
//   c3vPositions.col(1) = Eigen::Vector3d {0.0, 0.87461971, -0.48460962};
//   c3vPositions.col(2) = Eigen::Vector3d {0.75128605, -0.39338892, -0.52991926};
//
//   /* NOTE: Sign inversion on x is speculative, paper reprints P3 = P4 positions
//    * although this does not make sense to me.
//    */
//   c3vPositions.col(3) = Eigen::Vector3d {-0.75128605, -0.39338892, -0.52991926};
//
//   const double expectedC3vCSM = 1.16;
//   const double calculatedC3v = csm::pointGroup(
//     Detail::normalize(c3vPositions),
//     PointGroup::C3v
//   ).value_or(1000);
//
//   std::cout << "Calculated C3v CSM: " << calculatedC3v << " (expect " << expectedC3vCSM << ")\n";
//   BOOST_CHECK_CLOSE(expectedC3vCSM, calculatedC3v, 1);
// }

BOOST_AUTO_TEST_CASE(SquareC4D4PointGroups, *boost::unit_test::label("Shapes")) {
  const double pointGroupCSM = Continuous::pointGroup(
    Continuous::normalize(coordinates(Shape::Square)),
    PointGroup::C4
  );
  BOOST_CHECK_MESSAGE(
    std::fabs(pointGroupCSM) < 1e-10,
    "C4 point group CSM on square planar coordinates is not zero, but " << pointGroupCSM
  );

  const double D4CSM = Continuous::pointGroup(
    Continuous::normalize(coordinates(Shape::Square)),
    PointGroup::D4
  );

  BOOST_CHECK_MESSAGE(
    0 < D4CSM && D4CSM < 1e-10,
    "D4 CSM on square planar coordinates is not zero, but " << D4CSM
  );
}

BOOST_AUTO_TEST_CASE(FixedCnAxis, *boost::unit_test::label("Shapes")) {
  const std::vector<std::pair<Shape, unsigned>> highestOrderAxis {
    {Shape::Bent, 2},
    {Shape::EquilateralTriangle, 3},
    {Shape::VacantTetrahedron, 3},
    {Shape::T, 2},
    {Shape::Tetrahedron, 3},
    {Shape::Square, 4},
    {Shape::Seesaw, 2},
    {Shape::TrigonalPyramid, 3},
    {Shape::SquarePyramid, 4},
    {Shape::TrigonalBipyramid, 3},
    {Shape::Pentagon, 5},
    {Shape::Octahedron, 4},
    {Shape::TrigonalPrism, 3},
    {Shape::PentagonalPyramid, 5},
    {Shape::PentagonalBipyramid, 5},
    {Shape::SquareAntiprism, 4}
  };

  const Eigen::Matrix3d axes = Eigen::Matrix3d::Identity();

  constexpr double acceptanceThreshold = 0.3;

  for(const auto& nameOrderPair : highestOrderAxis) {
    auto positions = addOrigin(coordinates(nameOrderPair.first));
    distort(positions);

    auto normalizedPositions = Continuous::normalize(positions);
    standardizeTop(normalizedPositions);

    boost::optional<unsigned> highestFoundOrder;
    for(unsigned i = 0; i < 3; ++i) {
      Eigen::Vector3d axis = axes.col(i);
      for(unsigned n = 2; n < 6; ++n) {
        const double cn = Continuous::Fixed::element(normalizedPositions, Elements::Rotation::Cn(axis, n));

        if(cn < acceptanceThreshold) {
          highestFoundOrder = n;
        }
      }
    }

    BOOST_CHECK_MESSAGE(
      highestFoundOrder,
      "No Cn axis found along any principal moment axis for "
      << Shapes::name(nameOrderPair.first)
    );
    if(highestFoundOrder) {
      BOOST_CHECK_MESSAGE(
        highestFoundOrder.value() == nameOrderPair.second,
        "Expected to find Cn of order " << nameOrderPair.second << ", but found " << highestFoundOrder.value() << " instead."
      );
    }
  }
}

BOOST_AUTO_TEST_CASE(AlleneS4, *boost::unit_test::label("Shapes")) {
  Continuous::PositionCollection allenePositions(3, 7);
  allenePositions << 0.0, 0.0, 0.0, 0.0, 0.0, 0.928334, -0.928334,
                     0.0, 0.0, 0.0, 0.928334, -0.928334, 0.0, 0.0,
                     0.0, 1.3102, -1.3102, 1.866201, 1.866201, -1.866201, -1.866201;

  auto normalizedPositions = Continuous::normalize(allenePositions);
  const Top top = standardizeTop(normalizedPositions);
  BOOST_CHECK_MESSAGE(
    top == Top::Prolate,
    "Top isn't prolate, but " << topName(top)
  );

  const double S4CSM = Continuous::Fixed::element(normalizedPositions, Elements::Rotation::Sn(Eigen::Vector3d::UnitZ(), 4));
  BOOST_CHECK_MESSAGE(
    S4CSM < 0.1,
    "CSM(S4) = " << S4CSM << " of allene is over recognition threshold (0.1)"
  );

  const double D2dCSM = Continuous::pointGroup(normalizedPositions, PointGroup::D2d);
  BOOST_CHECK_MESSAGE(
    D2dCSM < 0.1,
    "CSM(D2d) = " << D2dCSM << " of allene is over recognition threshold (0.1)"
  );

  const auto optimizedS4Result = Continuous::element(
    normalizedPositions,
    Elements::Rotation::Sn(
      Eigen::Vector3d::UnitZ() + 0.1 * Eigen::Vector3d::Random().normalized(),
      4
    )
  );

  BOOST_CHECK_MESSAGE(
    optimizedS4Result.second.axis.isApprox(Eigen::Vector3d::UnitZ(), 1e-2),
    "Axis of optimized S4 is not +z, but " << optimizedS4Result.second.axis.transpose()
  );

  BOOST_CHECK_LT(optimizedS4Result.first, 0.1);
}

BOOST_AUTO_TEST_CASE(ReflectionPlaneOptimization, *boost::unit_test::label("Shapes")) {
  // Generate 8 points in the xy plane
  Continuous::PositionCollection planarPositions(3, 8);
  for(unsigned i = 0; i < 8; ++i) {
    Eigen::Vector3d v = 3 * Eigen::Vector3d::Random();
    v.z() = 0;
    planarPositions.col(i) = v;
  }

  auto normalized = Continuous::normalize(planarPositions);

  const double zPlaneCSM = Continuous::Fixed::element(
    normalized,
    Elements::Reflection {Eigen::Vector3d::UnitZ()}
  );

  BOOST_CHECK_LT(zPlaneCSM, 0.1);

  const auto optimizedSigma = Continuous::element(
    normalized,
    Elements::Reflection {
      Eigen::Vector3d::UnitZ() + 0.1 * Eigen::Vector3d::Random().normalized()
    }
  );

  BOOST_CHECK_MESSAGE(
    optimizedSigma.second.normal.isApprox(Eigen::Vector3d::UnitZ(), 1e-2),
    "Optimized sigma plane's normal is not +z, but " << optimizedSigma.second.normal.transpose()
  );

  BOOST_CHECK_LT(optimizedSigma.first, 0.1);
}

BOOST_AUTO_TEST_CASE(AsymmetricTopStandardization, *boost::unit_test::label("Shapes")) {
  std::vector<Shape> asymmetricTopsWithC2 {
    Shape::Bent,
    Shape::T,
    Shape::Seesaw
  };

  for(const Shape shape : asymmetricTopsWithC2) {
    auto shapeCoordinates = addOrigin(coordinates(shape));
    auto normalizedPositions = Continuous::normalize(shapeCoordinates);
    const Top top = standardizeTop(normalizedPositions);
    BOOST_CHECK_MESSAGE(
      top == Top::Asymmetric,
      "Expected asymmetric top for " << name(shape) << ", got "
      << topName(top) << " instead"
    );

    const unsigned highestAxisOrder = reorientAsymmetricTop(normalizedPositions);
    BOOST_CHECK_EQUAL(highestAxisOrder, 2);

    // Ensure rotation of highest order axis to z worked
    const double CnCSM = Continuous::Fixed::element(
      normalizedPositions,
      Elements::Rotation::Cn(Eigen::Vector3d::UnitZ(), 2)
    );
    BOOST_CHECK_MESSAGE(
      CnCSM < 1e-10,
      "Expected Cn of order 2 along z < 1e-10, got " << CnCSM << " instead."
    );
  }
}

BOOST_AUTO_TEST_CASE(ShapeMeasuresYieldForwardPermutation, *boost::unit_test::label("Shapes")) {
  const Shape testShape = Shape::Tetrahedron;

  auto shapeCoordinates = Continuous::normalize(
    addOrigin(coordinates(testShape))
  );

  Continuous::PositionCollection shuffled (3, shapeCoordinates.cols());
  BOOST_REQUIRE_MESSAGE(size(testShape) == 4, "Test setup is no longer valid");
  const std::vector<Vertex> shufflePermutation {{3_v, 4_v, 0_v, 2_v, 1_v}};
  for(unsigned i = 0; i < 5; ++i) {
    shuffled.col(shufflePermutation.at(i)) = shapeCoordinates.col(i);
  }

  auto faithful = Continuous::shapeFaithfulPaperImplementation(shuffled, testShape);
  auto alternate = Continuous::shapeAlternateImplementation(shuffled, testShape);
  auto heuristics = Continuous::shapeHeuristics(shuffled, testShape);

  BOOST_CHECK_CLOSE(faithful.measure, alternate.measure, 1);
  BOOST_CHECK_CLOSE(faithful.measure, heuristics.measure, 1);

  auto isBackwardsMapping = [](const std::vector<Vertex>& permutation) -> bool {
    /* A forward permutation maps from positions to shape indices.
     * A backwards permutation maps from shape indices to positions.
     *
     * Only backwards permutations have the (added) centroid at the back, and
     * hence their [0, S] = [0, N - 1] = [0, N) ranges are sub-permutations
     * (if ordered, identical to iota(N)) that can be rotated with shape
     * rotations.
     */
    return permutation.back() == permutation.size() - 1;
  };

  auto invert = [](const std::vector<Vertex>& p) -> std::vector<Vertex> {
    const unsigned P = p.size();
    std::vector<Vertex> inverse(P);
    for(Vertex i {0}; i < P; ++i) {
      inverse.at(p.at(i)) = i;
    }
    return inverse;
  };

  BOOST_CHECK(shufflePermutation == invert(faithful.mapping));
  BOOST_CHECK(shufflePermutation == invert(alternate.mapping));
  BOOST_CHECK(shufflePermutation == invert(heuristics.mapping));

  BOOST_CHECK_MESSAGE(
    !isBackwardsMapping(faithful.mapping),
    "Faithful mapping is not a forward mapping: " << Temple::condense(faithful.mapping)
  );

  BOOST_CHECK_MESSAGE(
    !isBackwardsMapping(alternate.mapping),
    "Alternate mapping is not a forward mapping: " << Temple::condense(alternate.mapping)
  );

  BOOST_CHECK_MESSAGE(
    !isBackwardsMapping(heuristics.mapping),
    "Heuristics mapping is not a forward mapping: " << Temple::condense(heuristics.mapping)
  );

  BOOST_CHECK_MESSAGE(
    faithful.mapping == alternate.mapping,
    "Faithful mapping does not match alternate mapping.\n"
    << "Faithful: " << Temple::condense(faithful.mapping) << "\n"
    << "Alternate: " << Temple::condense(alternate.mapping)
  );

  BOOST_CHECK_MESSAGE(
    faithful.mapping == heuristics.mapping,
    "Faithful mapping does not match heuristics mapping.\n"
    << "Faithful: " << Temple::condense(faithful.mapping) << "\n"
    << "Heuristics: " << Temple::condense(heuristics.mapping)
  );
}

BOOST_AUTO_TEST_CASE(ShapeMeasuresAlternateAlgorithm, *boost::unit_test::label("Shapes")) {
#ifdef NDEBUG
  constexpr unsigned testingShapeSizeLimit = 7;
#else
  constexpr unsigned testingShapeSizeLimit = 5;
#endif

  for(const Shape shape : allShapes) {
    if(size(shape) > testingShapeSizeLimit) {
      continue;
    }

    auto shapeCoordinates = Continuous::normalize(
      addOrigin(coordinates(shape))
    );
    const double unrotated = Continuous::shapeAlternateImplementation(shapeCoordinates, shape).measure;
    BOOST_CHECK_MESSAGE(
      unrotated < 1e-10,
      "Expected CShM < 1e-10 for unrotated coordinates of " << name(shape) << ", but got " << unrotated
    );
    randomlyRotate(shapeCoordinates);
    const double rotated = Continuous::shapeAlternateImplementation(shapeCoordinates, shape).measure;
    BOOST_CHECK_MESSAGE(
      rotated < 0.1,
      "Expected CShM < 1e-2 for rotated coordinates of " << name(shape) << ", but got " << rotated
    );

    for(unsigned i = 1; i < 5; ++i) {
      const double distortionNorm = 0.1 * i;
      auto distorted = shapeCoordinates;
      distort(distorted, distortionNorm);
      distorted = Continuous::normalize(distorted);

      const double faithful = Continuous::shapeFaithfulPaperImplementation(
        distorted,
        shape
      ).measure;
      const double alternate = Continuous::shapeAlternateImplementation(distorted, shape).measure;
      BOOST_CHECK_CLOSE(faithful, alternate, 1);
    }
  }
}

BOOST_AUTO_TEST_CASE(ShapeMeasuresHeuristics, *boost::unit_test::label("Shapes")) {
#ifdef NDEBUG
  constexpr unsigned testingShapeSizeLimit = 7;
#else
  constexpr unsigned testingShapeSizeLimit = 5;
#endif

  constexpr unsigned repeats = 5;

  for(const Shape shape : allShapes) {
    // Cannot use heuristics on fewer than five vertices
    if(size(shape) < 4) {
      continue;
    }

    if(size(shape) > testingShapeSizeLimit) {
      continue;
    }

    auto shapeCoordinates = Continuous::normalize(
      addOrigin(coordinates(shape))
    );

    for(unsigned i = 1; i < 6; ++i) {
      const double distortionNorm = 0.1 * i;

      std::vector<double> referenceValues;
      std::vector<double> errors;
      for(unsigned j = 0; j < repeats; ++j) {
        auto distorted = shapeCoordinates;
        distort(distorted, distortionNorm);
        distorted = Continuous::normalize(distorted);

        const double alternate = Continuous::shapeAlternateImplementation(distorted, shape).measure;
        referenceValues.push_back(alternate);
        const double heuristic = Continuous::shapeHeuristics(distorted, shape).measure;
        const double error = std::fabs(alternate - heuristic);

        if(error > 1e-2) {
          std::cout << "Error " << error << " for shape " << name(shape) << " and distortion " << distortionNorm << "\n";
        }
        errors.push_back(std::fabs(alternate - heuristic));
      }

      const double referenceAverage = Temple::accumulate(referenceValues, 0.0, std::plus<>()) / repeats;
      const double errorAverage = Temple::accumulate(errors, 0.0, std::plus<>()) / repeats;
      BOOST_CHECK_MESSAGE(
        errorAverage < 0.01 * referenceAverage,
        "Expected error average below 1% of reference value average, but mu(error) = "
        << errorAverage << " >= 0.01 * " << referenceAverage << " = "
        << (0.01 * referenceAverage) << " for shape " << name(shape) << " and distortion norm " << distortionNorm
      );
    }
  }
}

BOOST_AUTO_TEST_CASE(MinimumDistortionConstants, *boost::unit_test::label("Shapes")) {
  /* NOTES
   * - These constants are from https://pubs.acs.org/doi/10.1021/ja036479n
   *
   *   k_XY = sqrt( CShM_X(Y) ) = sqrt( CShM_Y(X) )
   *
   *   where CShM_X(Y) is the shape measure regarding shape X of coordinates of
   *   the ideal shape Y
   *
   * - VOC-5 is our square pyramid, their square pyramid has 105° base-apex angles
   */

  const std::vector<
    std::tuple<Shape, Shape, double>
  > minimumDistortionConstants {
    {Shape::Tetrahedron, Shape::Square, 5.774}, // T-4, SP-4

    /* NOTE: Seesaw / Sawhorse angle of "equatorial" vertices can vary,
     * unclear here from the paper and close, but mismatching with our
     * definition (120°, essentially a single-vacant trigonal bipyramid)
     */
//    {Shape::Tetrahedron, Shape::Seesaw, 3.129}, // T-4, SW-4 [sic, is SS-4]
//    {Shape::Seesaw, Shape::Square, 4.365}, // SW-4, SP-4

    {Shape::SquarePyramid, Shape::TrigonalBipyramid, 2.710}, // VOC-5, TBPY-5
    {Shape::SquarePyramid, Shape::Pentagon, 5.677}, // VOC-5, PP-5
    {Shape::TrigonalBipyramid, Shape::Pentagon, 6.088}, // TBPY-5, PP-5

    // NOTE: All of the below that are commented out are missing the required shapes

    {Shape::Octahedron, Shape::TrigonalPrism, 4.091}, // OC-6, TPR-6
    {Shape::Octahedron, Shape::PentagonalPyramid, 5.517}, // OC-6, PPY-6
//    {Shape::Octahedron, Shape::Hexagon, 5.774}, // OC-6, HP-6
    {Shape::TrigonalPrism, Shape::PentagonalPyramid, 4.125}, // TPR-6, PPY-6
//    {Shape::TrigonalPrism, Shape::Hexagon, 5.803}, // TPR-6, HP-6
//    {Shape::PentagonalPyramid, Shape::Hexagon, 5.352}, // PPY-6, HP-6

//    {Shape::CappedOctahedron, Shape::CappedTrigonalPrism, 1.236},
//    {Shape::CappedOctahedron, Shape::PentagonalBipyramid, 2.899},
//    {Shape::CappedOctahedron, Shape::HexagonalPyramid, 4.130},
//    {Shape::CappedOctahedron, Shape::Heptagon, 6.146},
//    {Shape::CappedTrigonalPrism, Shape::PentagonalBipyramid, 2.577},
//    {Shape::CappedTrigonalPrism, Shape::HexagonalPyramid, 4.467},
//    {Shape::CappedTrigonalPrism, Shape::Heptagon, 5.989},
//    {Shape::PentagonalBipyramid, Shape::HexagonalPyramid, 5.166},
//    {Shape::PentagonalBipyramid, Shape::Heptagon, 5.934},
//    {Shape::HexagonalPyramid, Shape::Heptagon, 5.047},

//    {Shape::Cube, Shape::TrigonalDodecahedron, 2.820},
//    {Shape::Cube, Shape::SquareAntiprism, 3.315},
//    {Shape::Cube, Shape::HexagonalBipyramid, 2.897},
//    {Shape::Cube, Shape::HeptagonalPyramid, 5.533},
//    {Shape::TrigonalDodecahedron, Shape::SquareAntiprism, 1.688},
//    {Shape::TrigonalDodecahedron, Shape::HexagonalBipyramid, 3.960},
//    {Shape::TrigonalDodecahedron, Shape::HeptagonalPyramid, 4.989},
//    {Shape::SquareAntiprism, Shape::HexagonalBipyramid, 4.296},
//    {Shape::SquareAntiprism, Shape::HeptagonalPyramid, 4.953},
//    {Shape::HexagonalBipyramid, Shape::HeptagonalPyramid, 4.865},
  };

  auto testF = [](const Shape a, const Shape b, const double expectedMinimumDistortion) {
    auto makeShapeCoordinates = [](const Shape shape) {
      return Continuous::normalize(
        addOrigin(coordinates(shape))
      );
    };

    const double ab = Continuous::shapeAlternateImplementation(makeShapeCoordinates(a), b).measure;
    const double ba = Continuous::shapeAlternateImplementation(makeShapeCoordinates(b), a).measure;

    BOOST_CHECK_CLOSE(ab, ba, 0.1);

    const double calculatedMinimumDistortion = std::sqrt((ab + ba) / 2);
    BOOST_CHECK_CLOSE(calculatedMinimumDistortion, expectedMinimumDistortion, 2);
  };

  Temple::forEach(minimumDistortionConstants, testF);
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary subtract
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/core/test/test_utils.h"
#include "fern/algorithm/algebra/elementary/subtract.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2,
    class Result>
using OutOfRangePolicy = fa::subtract::OutOfRangePolicy<Value1, Value2,
    Result>;

template<
    class Value1,
    class Value2,
    class Result>
struct VerifyWithinRange
{
    bool operator()(
        Value1 const& value1,
        Value2 const& value2)
    {
        fa::SequentialExecutionPolicy sequential;

        OutOfRangePolicy<Value1, Value2, Result> policy;
        Result result;

        fa::algebra::subtract(sequential, value1, value2, result);

        return policy.within_range(value1, value2, result);
    }
};

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    auto max_int32 = fern::max<int32_t>();
    auto min_int32 = fern::min<int32_t>();
    auto max_int64 = fern::max<int64_t>();
    auto min_int64 = fern::min<int64_t>();
    auto max_uint32 = fern::max<uint32_t>();
    auto min_uint32 = fern::min<uint32_t>();
    auto max_uint64 = fern::max<uint64_t>();
    // auto min_uint64 = fern::min<uint64_t>();
    // auto max_float32 = fern::max<float>();
    // auto min_float32 = fern::min<float>();
    auto max_float64 = fern::max<double>();
    // auto min_float64 = fern::min<double>();

    {
        // unsigned - unsigned
        OutOfRangePolicy<uint32_t, uint32_t, uint32_t> policy;
        BOOST_CHECK(!policy.within_range(5, 6, -1));
        BOOST_CHECK(policy.within_range(6, 5, 1));
        BOOST_CHECK(policy.within_range(0, 0, 0));
        BOOST_CHECK(policy.within_range(max_uint32, max_uint32,
            max_uint32 - max_uint32));
    }

    {
        // unsigned - signed
        {
            OutOfRangePolicy<uint32_t, int32_t, int64_t> policy;
            BOOST_CHECK(policy.within_range(5, 6, -1));
            BOOST_CHECK(policy.within_range(5, -6, 11));
            BOOST_CHECK(policy.within_range(0, 0, 0));
            BOOST_CHECK(policy.within_range(max_uint32, max_int32,
                static_cast<int64_t>(max_uint32) -
                static_cast<int64_t>(max_int32)));
            BOOST_CHECK(policy.within_range(max_uint32, -max_int32,
                static_cast<int64_t>(max_uint32) -
                static_cast<int64_t>(-max_int32)));
            BOOST_CHECK(policy.within_range(min_uint32, max_int32,
                static_cast<int64_t>(min_uint32) -
                static_cast<int64_t>(max_int32)));
            BOOST_CHECK(policy.within_range(min_uint32, -max_int32,
                static_cast<int64_t>(min_uint32) -
                static_cast<int64_t>(-max_int32)));
            BOOST_CHECK(policy.within_range(max_uint32, min_int32,
                static_cast<int64_t>(max_uint32) -
                static_cast<int64_t>(min_int32)));
            BOOST_CHECK(policy.within_range(max_uint32, -min_int32,
                static_cast<int64_t>(max_uint32) -
                static_cast<int64_t>(-min_int32)));
        }

        {
            OutOfRangePolicy<uint32_t, int64_t, int64_t> policy;
            BOOST_CHECK(policy.within_range(5, 6, -1));
            BOOST_CHECK(policy.within_range(5, -6, 11));
            BOOST_CHECK(policy.within_range(0, 0, 0));
            BOOST_CHECK(!policy.within_range(1, -max_int64,
                static_cast<int64_t>(1) - -max_int64));
        }

        {
            OutOfRangePolicy<uint64_t, int64_t, int64_t> policy;
            BOOST_CHECK(policy.within_range(5, 6, -1));
            BOOST_CHECK(policy.within_range(5, -6, 11));
            BOOST_CHECK(policy.within_range(0, 0, 0));
            BOOST_CHECK(!policy.within_range(1, -max_int64,
                static_cast<int64_t>(1) - -max_int64));
            BOOST_CHECK(policy.within_range(max_uint64, 0,
                static_cast<int64_t>(max_uint64) - static_cast<int64_t>(0)));
        }
    }

    {
        // signed - unsigned
        {
            VerifyWithinRange<int32_t, uint32_t, int64_t> verify;
            BOOST_CHECK_EQUAL(verify(5, 6), true);
            BOOST_CHECK_EQUAL(verify(-5, 6), true);
            BOOST_CHECK_EQUAL(verify(0, 0), true);

            BOOST_CHECK_EQUAL(verify(max_int32, max_uint32), true);
            BOOST_CHECK_EQUAL(verify(min_int32, max_uint32), true);
            BOOST_CHECK_EQUAL(verify(max_int32, min_uint32), true);
        }

        {
            VerifyWithinRange<int64_t, uint32_t, int64_t> verify;
            BOOST_CHECK_EQUAL(verify(5, 6), true);
            BOOST_CHECK_EQUAL(verify(-5, 6), true);
            BOOST_CHECK_EQUAL(verify(0, 0), true);
            BOOST_CHECK_EQUAL(verify(min_int64, 1), false);
            BOOST_CHECK_EQUAL(verify(-1, max_uint32), true);
        }
    }

    {
        // signed - signed
        OutOfRangePolicy<int32_t, int32_t, int32_t> policy;
        BOOST_CHECK(policy.within_range(5, 6, -1));
        BOOST_CHECK(policy.within_range(-5, -6, 1));
        BOOST_CHECK(policy.within_range(5, -6, 11));
        BOOST_CHECK(policy.within_range(0, 0, 0));
        BOOST_CHECK(!policy.within_range(max_int32, -1, max_int32 - -1));
        BOOST_CHECK(!policy.within_range(min_int32, 1, min_int32 - 1));
        BOOST_CHECK(!policy.within_range(1, min_int32, 1 - min_int32));
        BOOST_CHECK(!policy.within_range(-2, max_int32, -2 - max_int32));
    }

    {
        // float - float
        OutOfRangePolicy<double, double, double> policy;
        BOOST_CHECK(policy.within_range(5.0, 6.0, -1.0));
        BOOST_CHECK(policy.within_range(-5.0, -6.0, 1.0));
        BOOST_CHECK(policy.within_range(5.0, -6.0, 11.0));
        BOOST_CHECK(policy.within_range(0.0, 0.0, 0.0));
        BOOST_CHECK(!policy.within_range(-max_float64, max_float64,
            -max_float64 - max_float64));
        BOOST_CHECK(!policy.within_range(max_float64, -max_float64,
            max_float64 - -max_float64));
    }

    {
        // float - integer
        OutOfRangePolicy<double, int64_t, double> policy;
        BOOST_CHECK(policy.within_range(5.0, 6, -1.0));
        BOOST_CHECK(policy.within_range(-5.0, -6, 1.0));
        BOOST_CHECK(policy.within_range(5.0, -6, 11.0));
        BOOST_CHECK(policy.within_range(0.0, 0, 0.0));
        BOOST_CHECK(policy.within_range(-max_float64, max_int64,
            -max_float64 - max_int64));
        BOOST_CHECK(policy.within_range(max_float64, -max_int64,
            max_float64 - -max_int64));
    }

    {
        // integer - float
        OutOfRangePolicy<int64_t, double, double> policy;
        BOOST_CHECK(policy.within_range(5, 6.0, -1.0));
        BOOST_CHECK(policy.within_range(-5, -6.0, 1.0));
        BOOST_CHECK(policy.within_range(5, -6.0, 11.0));
        BOOST_CHECK(policy.within_range(0, 0.0, 0.0));
        BOOST_CHECK(policy.within_range(-max_int64, max_float64,
            -max_int64 - max_float64));
        BOOST_CHECK(policy.within_range(max_int64, -max_float64,
            max_int64 - -max_float64));
    }
}

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::subtract(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<int8_t, int8_t, int8_t>(-5, 6, -11);
    verify_value<int8_t, int8_t, int8_t>(-5, -6, 1);
    verify_value<uint8_t, uint8_t, uint8_t>(6, 5, 1);
    verify_value<double, float, double>(6.0, 5.0f, 1.0);
}


#include <boost/accumulators/statistics/weighted_sum_kahan.hpp>


/* ----------------------------------------------------------------------- *//**
 *
 * @file matrix_decomp.cpp
 *
 * @brief Compute decomposition of a matrix in a single node
 *
 *//* ----------------------------------------------------------------------- */

#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>
#include <utils/Math.hpp>
#include <Eigen/Core>
#include <Eigen/LU>

#include "matrix_decomp.hpp"

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

namespace linalg {

/**
 * @brief Transition state for building a matrix
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length 3, and all elemenets are 0. Handle::operator[] will
 * perform bounds checking.
 */
template <class Handle>
class MatrixComposeState {
    template <class OtherHandle>
    friend class MatrixComposeState;

public:
    MatrixComposeState(const AnyType &inArray)
            : mStorage(inArray.getAs<Handle>()) {
        rebind(static_cast<uint64_t>(mStorage[0]), static_cast<uint64_t>(mStorage[1]));
    }

    operator AnyType() const {
        return mStorage;
    }

    void initialize(const Allocator& inAllocator, uint64_t inNumRows, uint64_t inNumCols) {
        // Allocate the storage for the matrix
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(stateSize(inNumRows, inNumCols));
        rebind(inNumRows, inNumCols);
        numRows = inNumRows;
        numCols = inNumCols;
        matrix.fill(0);
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    MatrixComposeState &operator=(
        const MatrixComposeState<OtherHandle> &inOtherState) {
        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object
     */
    template <class OtherHandle>
    MatrixComposeState &operator+=(
        const MatrixComposeState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            numRows != inOtherState.numRows ||
            numCols != inOtherState.numCols)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");
        // we assume that the number of rows and number of cols remain the same
        // between the merged states. We only need to add the matrices together
        // since each row/element of the matrix is set only once.
        matrix += inOtherState.matrix;
        return *this;
    }

private:
    static inline size_t stateSize(uint64_t inNumRows, uint64_t inNumCols) {
        return static_cast<size_t>(2 + inNumRows * inNumCols);
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param inNumRows The number of rows
     * @param inNumCols The number of columns
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: numRows (number of rows)
     * - 1: numCols (number of columns)
     * - 2: sumOfPoints (matrix with \c numRows rows and \c numCols columns)
     */
    void rebind(uint64_t inNumRows, uint64_t inNumCols) {
        numRows.rebind(&mStorage[0]);
        numCols.rebind(&mStorage[1]);
        matrix.rebind(&mStorage[2], static_cast<Index>(inNumRows),
                      static_cast<Index>(inNumCols));

        madlib_assert(mStorage.size()
            >= stateSize(inNumRows, inNumCols),
            std::runtime_error("Out-of-bounds array access detected."));
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::ReferenceToUInt64 numCols;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap matrix;
};

AnyType
matrix_compose_dense_transition::run(AnyType& args) {
    MatrixComposeState<MutableArrayHandle<double> > state = args[0];
    uint32_t numRows = args[1].getAs<uint32_t>();
    Index row_id = args[2].getAs<uint32_t>();
    MappedColumnVector curr_row = args[3].getAs<MappedColumnVector>();

    if (state.numCols == 0)
        state.initialize(*this, numRows, static_cast<uint32_t>(curr_row.size()));
    else if (curr_row.size() != state.matrix.cols() ||
             state.numRows != static_cast<uint32_t>(state.matrix.rows()) ||
             state.numCols != static_cast<uint32_t>(state.matrix.cols()))
        throw std::invalid_argument("Invalid arguments: Dimensions of vectors "
                                    "not consistent.");
    if (row_id < 0 || row_id >= numRows)
            throw std::runtime_error("Invalid row id.");
    state.matrix.row(row_id) = curr_row;
    return state;
}

AnyType
matrix_compose_sparse_transition::run(AnyType& args) {
    MatrixComposeState<MutableArrayHandle<double> > state = args[0];
    uint32_t numRows = args[1].getAs<uint32_t>();
    uint32_t numCols = args[2].getAs<uint32_t>();
    Index row_id = args[3].getAs<uint32_t>();
    Index col_id = args[4].getAs<uint32_t>();
    double element = args[5].getAs<double>();

    if (state.numCols == 0)
        state.initialize(*this, numRows, numCols);
    else if (state.numRows != static_cast<uint32_t>(state.matrix.rows()) ||
             state.numCols != static_cast<uint32_t>(state.matrix.cols()))
        throw std::invalid_argument("Invalid arguments: Dimensions of vectors "
                                    "not consistent.");
    if (row_id < 0 || row_id >= numRows)
            throw std::runtime_error("Invalid row id.");
    if (col_id < 0 || col_id >= numCols)
            throw std::runtime_error("Invalid col id.");
    state.matrix(row_id, col_id) = element;
    return state;
}

/**
 * @brief Perform the preliminary aggregation function: Merge transition states
 */
AnyType
matrix_compose_merge::run(AnyType &args) {
    if (args[0].isNull()) { return args[1]; }
    if (args[1].isNull()) { return args[0]; }
    MatrixComposeState<MutableArrayHandle<double> > stateLeft = args[0];
    MatrixComposeState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

AnyType
matrix_inv::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix m_inverse = state.matrix.inverse().transpose();
    return m_inverse;
}

AnyType
matrix_eigen::run(AnyType& args) {
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const VectorXcd eigenvalues = state.matrix.eigenvalues();
    return MappedVectorXcd(eigenvalues.leftCols(static_cast<Index>(1)));
}

AnyType
matrix_cholesky::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const MatrixLDLT ldlt = state.matrix.ldlt();
    if (ldlt.info() != Success) {
        throw std::invalid_argument("Invalida arguments: Cholesky decomposition of input matrix"
                " does not exist");
    }
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix m_p(PermutationMatrix(ldlt.transpositionsP()));
    const Matrix m_l = ldlt.matrixL();
    const Matrix m_d(ldlt.vectorD().asDiagonal());
    Matrix m_cholesky(state.matrix.rows(), state.matrix.cols() * 3);
    m_cholesky.block(0, 0, state.matrix.rows(), state.matrix.cols()) << m_p;
    m_cholesky.block(0, state.matrix.cols(), state.matrix.rows(), state.matrix.cols()) << m_l;
    m_cholesky.block(0, state.matrix.cols() * 2, state.matrix.rows(), state.matrix.cols()) << m_d;
    const Matrix res = m_cholesky.transpose();
    return res;
}

AnyType
matrix_qr::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const HouseholderQR qr = state.matrix.householderQr();
    const Matrix &R = qr.matrixQR().triangularView<Upper>();
    const Matrix &Q =  qr.householderQ();

    madlib_assert(Q.rows() == Q.cols() && Q.cols() == R.rows(),
        std::runtime_error("Error QR decomposition result."));
    Matrix m(Q.rows(), Q.cols() + R.cols());
    m.block(0, 0, Q.rows(), Q.cols()) << Q;
    m.block(0, Q.cols(), R.rows(), R.cols()) << R;
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix & res = m.transpose();
    return res;
}

AnyType
matrix_rank::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    return static_cast<int64_t>(state.matrix.fullPivLu().rank());
}

AnyType
matrix_lu::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const FullPivLU &lu = state.matrix.fullPivLu();

    Matrix l = Matrix::Identity(state.numRows, state.numRows);
    l.block(0, 0, lu.matrixLU().rows(), lu.matrixLU().cols()).triangularView<StrictlyLower>() = lu.matrixLU();
    const Matrix &u = lu.matrixLU().triangularView<Upper>();
    const Matrix &p(lu.permutationP());
    const Matrix &q(lu.permutationQ());
    Matrix m(static_cast<Index>(std::max(state.numRows, state.numCols)),
             static_cast<Index>(state.numRows * 2 + state.numCols * 2));
    m.block(0, 0, state.numRows, state.numRows) << p;
    m.block(0, state.numRows, state.numRows, state.numRows) << l;
    m.block(0, state.numRows * 2, state.numRows, state.numCols) << u;
    m.block(0, state.numRows * 2 + state.numCols, state.numCols, state.numCols) << q;

    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix res = m.transpose();
    return res;
}

AnyType
matrix_nuclear_norm::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const JacobiSVD &svd = state.matrix.jacobiSvd(EigenvaluesOnly);
    const Matrix u = svd.matrixU();
    const Matrix v = svd.matrixV();
    double norm = 0;
    for (Index i = 0; i < svd.singularValues().rows(); ++i)
        norm += svd.singularValues()(i);
    return norm;
}

AnyType
matrix_pinv::run(AnyType& args) {
    if (args.isNull()) {
        return Null();
    }
    using namespace std;
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const JacobiSVD &svd = state.matrix.jacobiSvd(ComputeFullU|ComputeFullV);
    const Matrix u = svd.matrixU();
    const Matrix v = svd.matrixV();
    Matrix s(svd.singularValues().asDiagonal());
    double pinvtoler = 1.e-6;
    for (Index i = 0; i < s.rows(); ++i) {
        for (Index j = 0; j < s.cols(); ++j) {
            // for very small singular values we treat the inverse as zero
            if (s(i, j) > pinvtoler)
                s(i, j) = 1.0 / s(i, j);
            else s(i, j) = 0;
        }
    }
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix &res = (v * s * u.transpose()).transpose();
    return res;
}

} // namespace linalg

} // namespace modules

} // namespace regress


#include <iostream>
#include <math.h>
#include <thread>
#include <mutex>
#include <chrono>
#include <vector>
#include <functional>
#include <boost/filesystem.hpp>
#include <pcl/io/pcd_io.h>

#include "pcl_viewer.h"
#include "pcl_segment.h"
#include "pcl_filter.h"
#include "pcl_octree.h"
#include "pcl_viewpoint.h"

using namespace asv3d;
using namespace std;

// use static mutex to lock thread when operating the pcl visuliazer
static std::mutex mtx;

void PrintHelp();
int  ParseArguments(int argc, char** argv,
  std::string& dir,
  std::string& trajectory,
  bool& save,
  bool& filter,
  double& distance,
  double& resolution,
  int& display,
  int& sampleType,
  std::vector<double>& limitBox,
  string& configFile);

WSPointCloudPtr FilterPointCloud(const WSPointCloudPtr cloud,
  double resolution);

void GenerateViewpoints(PCLViewer* viewer,
  const WSPointCloudPtr cloud,
  double distance,
  double resolution,
  int display,
  const string& configFile,
  int sampleType = 0);

void UpdatePointCloud(PCLViewer* viewer,
  const std::string& dir,
  bool bFilter,
  double resolution);

void DisplayTrajectory(PCLViewer* viewer,
  const std::string& dir,
  const std::string& file,
  double resolution,
  int type);

void SegmentPointCloud(PCLViewer* viewer,
  const std::string& dir,
  const std::vector<double>& limitBox,
  bool bFilter);

void GenerateScanningPath(PCLViewer* viewer,
  const WSPointCloudPtr cloud,
  double distance,
  double resolution
);

double Distance(const WSPoint& pt1, const WSPoint& pt2)
{
  return sqrt((pt1.x-pt2.x)*(pt1.x-pt2.x)+(pt1.y-pt2.y)*(pt1.y-pt2.y)+(pt1.z-pt2.z)*(pt1.z-pt2.z));
}

void AddCubes(PCLViewer* viewer,const std::vector<WSPoint>& points, const std::string& nameprefix, double voxelLen, int vp, double r, double g, double b)
{
  for (size_t i = 0; i < points.size(); ++i)
  {
    WSPoint c = points[i];
    std::string name = nameprefix;
    name.append("-").append(std::to_string(i));
    viewer->AddCube(c,0.5*voxelLen,name,r,g,b,vp);
  }
}

int main(int argc, char** argv) try
{
  bool bSave = false;
  bool bFilter = false;
  std::string dir = "";
  std::string trajectory = "";
  double resolution = 1.0;
  int display = -5;
  double distance = -1;
  int sampleType = 0;
  std::vector<double> limitBox;
  string configFile = "";
  int task = ParseArguments(argc,argv,dir,
    trajectory,
    bSave,
    bFilter,
    distance,
    resolution,
    display,
    sampleType,
    limitBox,
    configFile);

  PCLViewer viewer("3D Point Cloud Viewer");
  if(task == 1)
  {
    //std::thread t(DisplayTrajectory, &viewer, dir, trajectory, resolution, sampleType);
    DisplayTrajectory(&viewer, dir, trajectory, resolution, sampleType);
    while (!viewer.IsStop()) {
      mtx.lock();
      viewer.SpinOnce();
      mtx.unlock();
    }
    //t.join();
  }
  else if (task == 2)
  {
    UpdatePointCloud(&viewer, dir, bFilter,resolution);
    while (!viewer.IsStop()) {
      mtx.lock();
      viewer.SpinOnce();
      mtx.unlock();
    }
    //t.join();
  }
  else if (task == 3)
  {
    WSPointCloudPtr cloud = viewer.LoadPointCloud(dir);
    GenerateViewpoints(&viewer, cloud, distance, resolution, display, configFile, sampleType);
    while(!viewer.IsStop())
    {
      mtx.lock();
      viewer.SpinOnce();
      mtx.unlock();
    }
  }
  else if (task == 4)
  {
    SegmentPointCloud(&viewer, dir, limitBox, bFilter);
    while(!viewer.IsStop())
    {
      mtx.lock();
      viewer.SpinOnce();
      mtx.unlock();
    }
  }
  else if (task == 5)
  {
    WSPointCloudPtr cloud = viewer.LoadPointCloud(dir);
    GenerateScanningPath(&viewer,cloud,distance,resolution);
    while(!viewer.IsStop())
    {
      mtx.lock();
      viewer.SpinOnce();
      mtx.unlock();
    }
  }
  else
  {
    PrintHelp();
  }

  return task;
}
catch (const std::exception& e)
{
  std::cout << e.what() << std::endl;
  return -1;
}

void UpdatePointCloud(PCLViewer* viewer, const std::string& dir, bool bFilter,double resolution)
{
    // add a new thread for spin the viewer
    //std::thread t(ViewerSpin, viewer);
    std::vector<std::string> files;
    WSPointCloudPtr cloud(new WSPointCloud());

    std::vector<std::string> allfiles;
    boost::filesystem::directory_iterator itr(dir);
    for (; itr != boost::filesystem::directory_iterator(); ++itr)
    {
      if (boost::filesystem::is_regular_file(itr->status()));
        allfiles.push_back(itr->path().string());
    }
    int all = allfiles.size();
    bool bNewCloud = false;
    for (int i = 0; i < all; i++)
    {
        std::string file = allfiles[i];
        std::vector<std::string>::iterator end = files.end();
        if (std::find(files.begin(),files.end(),file) == files.end())
        {
            std::cout << "pcl == load " << std::to_string(i) << "/" << all << " point cloud from " << file << std::endl;
            WSPointCloudPtr temp(new WSPointCloud());
            int res = pcl::io::loadPCDFile(file, *temp);
            if(res < 0)
              std::cout << "pcl == failed to load point cloud." << std::endl;
            if (bFilter)
              temp = FilterPointCloud(temp,resolution);

            mtx.lock();
            *cloud += *temp;
            mtx.unlock();

            bNewCloud = true;
            files.push_back(file);
        }
      }

    mtx.lock();
    viewer->AddPointCloud(cloud);
    viewer->SavePointCloud(viewer->PointCloud(),dir);
    mtx.unlock();
}

void DisplayTrajectory(PCLViewer* viewer, const std::string& dir, const std::string& file, double resolution, int type)
{
  PCLViewPoint viewCreator;
  std::vector<Eigen::Affine3f> cameras;
  if (!file.empty())
  {
    viewCreator.LoadFromFile(file,cameras);
    std::cout << cameras.size() << " viewpoints found in trajectory." << std::endl;
  }

  WSPointCloudPtr cloud = viewer->LoadPointCloud(dir);
  mtx.lock();
  viewer->AddPointCloud(cloud,0);
  mtx.unlock();

  PCLOctree octree(cloud,0.5*resolution,3);

  double voxelLen = octree.VoxelSideLength();
  WSPointCloudPtr vCloud = octree.VoxelCentroidCloud();
  std::vector<WSPoint> centroids;
  for (size_t i = 0; i < vCloud->points.size(); ++i)
    centroids.push_back(vCloud->points[i]);

  std::vector<int> allVoxels;
  int totalVoxel = octree.VoxelIndices(allVoxels);

  std::vector<WSPointCloudPtr> outsidePolygons;
  octree.FindOutsidePolygons(outsidePolygons);

  if (file.empty())
  {
    std::cout << "outsidePolygons count:" << outsidePolygons.size() << std::endl;
    mtx.lock();
    // AddCubes(viewer, centroids, "octree_voxel", voxelLen, 0, 0.0,0.0,1.0);
    for (size_t i = 0; i < outsidePolygons.size(); ++i)
    {
      std::string name("polygon_");
      name.append(std::to_string(i));
      WSPointCloudPtr pts = outsidePolygons[i];
      // std::cout << name << std::endl;
      // viewer->AddPolygon(pts,name,0.0,0.0,1.0);
    }
    mtx.unlock();
  }

  double distance = 0.0;
  WSPoint startPt, endPt;
  std::vector<int> coveredVoxels;
  for (size_t i = 0; i < cameras.size(); ++i)
  {
      Eigen::Affine3f camera = cameras[i];
      if (i == 0) {
        endPt.x = camera.matrix()(0,3);
        endPt.y = camera.matrix()(1,3);
        endPt.z = camera.matrix()(2,3);
        startPt = endPt;
      } else {
        startPt = endPt;
        endPt.x = camera.matrix()(0,3);
        endPt.y = camera.matrix()(1,3);
        endPt.z = camera.matrix()(2,3);
      }

      distance += Distance(startPt,endPt);

      std::vector<int> indices;
      WSPointCloudPtr voxelCloud = viewCreator.CameraViewVoxels(octree, camera, indices);
      std::vector<WSPoint> vCenters;
      for (size_t j = 0; j < voxelCloud->points.size(); ++j)
      {
        WSPoint vCenter = voxelCloud->points[j];
        int idx = octree.VoxelIndex(vCenter);
        if (std::find(coveredVoxels.begin(), coveredVoxels.end(), idx) == coveredVoxels.end())
        {
          coveredVoxels.push_back(idx);
          vCenters.push_back(vCenter);
        }
      }
      mtx.lock();
      // std::string text("viewpoint: ");
      // text.append(std::to_string(i+1)).append("/").append(std::to_string(cameras.size()));
      //double dCoverage = 100.0*(double(coveredVoxels.size()) / double(totalVoxel));
      //std::string coverage(", total coverage: ");
      //coverage.append(std::to_string(dCoverage)).append(" %");
      //text.append(coverage);
      //viewer->AddText(text, "text");
      if (type == -1)
      {
        viewer->AddCoordinateSystem(camera,i,2.0,0,true);
      }
      else if (type == -2)
      {
        AddCubes(viewer, vCenters, std::to_string(i), voxelLen, 0, 1.0,0.0,0.0);
      }
      else if (type == -3)
      {
        if (i == 0 || i == cameras.size()-1){
          viewer->AddCoordinateSystem(camera,i,2.0,0,true);
        }
        else
        {
            viewer->AddCoordinateSystem(camera,i,1.0,0,true);
        }
        viewer->AddLine(startPt,endPt,i,0.0,0.0,0.0);
      }
      else if (type == -4)
      {
        if (i == 0 || i == cameras.size()-1){
          viewer->AddCoordinateSystem(camera,i,2.0,0,true);
        }
        else
        {
          viewer->AddCoordinateSystem(camera,i,1.0,0,true);
        }
        viewer->AddLine(startPt,endPt,i,0.0,0.0,0.0);
        AddCubes(viewer, vCenters, std::to_string(i), voxelLen, 0, 1.0,0.0,0.0);
      }
      mtx.unlock();
      // Sleep for 2 seconds for the ply file
    //  std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  std::cout << "total voxels " << totalVoxel << "covered voxels" << coveredVoxels.size() << '\n';
  std::cout << "trajectory length: " << distance << " meters." << std::endl;
}

// generate viewpoints with distance to the aircraft and resolution of octree
void GenerateViewpoints(PCLViewer* viewer,
  const WSPointCloudPtr srcCloud,
  double distance,
  double resolution,
  int display,
  const string& configFile,
  int sampleType)
{
  if (nullptr == srcCloud)
    return;

  // load configuration
  typedef std::vector<double> BBox;
  std::vector<std::pair<Eigen::Vector3f,BBox> > segments;
  std::ifstream config(configFile);
  std::string line;
  std::cout << "read config " << segments.size() << std::endl;

  // the viewpoint z limited in range of [height_min, height_max]
  double height_max = 0.0;
  double height_min = 0.0;
  while (std::getline(config, line))
  {
    std::cout << line << std::endl;
    std::stringstream linestream(line);
    std::string nx,ny,nz,xmin,xmax,ymin,ymax,zmin,zmax;
    linestream >> nx >> ny >> nz >> xmin >> xmax >> ymin >> ymax >> zmin >> zmax >> height_min >> height_max;
    BBox box;
    box.push_back(std::stod(xmin));
    box.push_back(std::stod(xmax));
    box.push_back(std::stod(ymin));
    box.push_back(std::stod(ymax));
    box.push_back(std::stod(zmin));
    box.push_back(std::stod(zmax));
    Eigen::Vector3f refNormal(std::stod(nx),std::stod(ny),std::stod(nz));
    segments.push_back(std::make_pair(refNormal,box));
  }
  config.close();

  ///////////////////////////////////////////////////////////////////////////
  // viewpoint creator
  PCLViewPoint viewCreator;

  // create camera along the voxel normals
  std::vector<Eigen::Affine3f> cameras;
  PCLOctree octree(srcCloud,resolution);
  for (int i = 0; i < segments.size(); ++i)
  {
    Eigen::Vector3f refNormal = segments[i].first;
    BBox bbox = segments[i].second;
    viewCreator.GenerateCameraPositions(octree,distance,refNormal,bbox,cameras,height_min,height_max);
  }
  std::cout << cameras.size() << " camere positions generated." << std::endl;

  ////////////////////////////////////////////////////////////////////////////
  // voxel in view frustum culling
  // use a higher resolution to calculate voxel coverage
  PCLOctree octree2(srcCloud,0.5*resolution,3);
  std::vector<int> voxelCoveredVec;
  std::map<int, std::vector<int> > viewVoxelMap;
  for (size_t i = 0; i < cameras.size(); ++i)
  {
    std::vector<int> visibleVoxels;
    WSPointCloudPtr viewCloud = viewCreator.CameraViewVoxels(octree2,cameras[i],visibleVoxels);
    viewVoxelMap.insert(std::make_pair(static_cast<int>(i), visibleVoxels));
    for (size_t j = 0; j < visibleVoxels.size(); ++j)
    { // calculate covered voxels
      int vIndex = visibleVoxels[j];
      if (std::find(voxelCoveredVec.begin(),voxelCoveredVec.end(),vIndex) == voxelCoveredVec.end())
        voxelCoveredVec.push_back(vIndex);
    }
  }

  ////////////////////////////////////////////////////////////////////////////
  // voxel coverage
  std::vector<int> voxelUncoveredVec;
  std::vector<int> allVoxelVec;
  int total = octree2.VoxelIndices(allVoxelVec);
  for (size_t i = 0; i < total; i++)
  {
    if (std::find(voxelCoveredVec.begin(), voxelCoveredVec.end(), allVoxelVec[i]) == voxelCoveredVec.end())
      voxelUncoveredVec.push_back(allVoxelVec[i]);
  }
  double coverage = 100.0*(double(voxelCoveredVec.size())/double(total));
  std::cout << "total voxel: " << total << " voxel covered/uncovered " << voxelCoveredVec.size()
            << "/" << voxelUncoveredVec.size() << " coverage: " << coverage << " %" << std::endl;

  /////////////////////////////////////////////////////////////////////////////
  // save viewpoints
  std::string fileName("/home/yufeng/Temp/viewpoint");
  fileName.append(std::to_string(distance)).append("_").append(std::to_string(resolution)).append(".txt");
  viewCreator.SaveToFile(fileName, cameras, viewVoxelMap);
  std::cout << "save to " << fileName << std::endl;

  ////////////////////////////////////////////////////////////////////////////
  // display
  int vp = viewer->CreateViewPort(0,0,1,1);
  viewer->AddPointCloud(srcCloud,vp);
  // display voxel cube, camere view and frustum visible voxels
  double voxelLen = octree2.VoxelSideLength();
  if (display == -1)
  {
    for (size_t i = 0; i < cameras.size(); ++i)
      viewer->AddCoordinateSystem(cameras[i],i);
  }
  else if (display == -2)
  {
    WSPointCloudPtr vCloud = octree2.VoxelCentroidCloud();
    std::vector<WSPoint> centroids;
    for (size_t i = 0; i < vCloud->points.size(); ++i)
      centroids.push_back(vCloud->points[i]);
    AddCubes(viewer, centroids, "octree_voxel", voxelLen, vp, 0.0,0.0,1.0);
  }
  else if (display == -3)
  {
    std::vector<WSPoint> centroids;
    for (size_t i = 0; i < voxelCoveredVec.size(); ++i)
      centroids.push_back(octree2.VoxelCentroid(voxelCoveredVec[i]));
    AddCubes(viewer, centroids, "covered voxel", voxelLen, vp, 0.0,1.0,0.0);
  }
  else if (display == -4)
  {
    std::vector<WSPoint> centroids;
    for (size_t i = 0; i < voxelUncoveredVec.size(); ++i)
      centroids.push_back(octree2.VoxelCentroid(voxelUncoveredVec[i]));
    AddCubes(viewer, centroids, "uncovered voxel", voxelLen, vp, 1.0,0.0,0.0);
  }
  else if (display == -5)
  {
    int camereIndex = 161;
    std::string name("view_frustum");
    name.append("_").append(std::to_string(0));
    std::vector<int> visibleVoxelIndices;
    WSPointCloudPtr viewCloud = viewCreator.CameraViewVoxels(octree2,cameras[camereIndex],visibleVoxelIndices);
    std::vector<WSPoint> centroids;
    for (size_t i = 0; i < viewCloud->points.size(); ++i)
      centroids.push_back(viewCloud->points[i]);
    AddCubes(viewer, centroids, name, voxelLen, vp, 1.0,0.0,0.0);
    viewer->AddCoordinateSystem(cameras[camereIndex],display);
  }
}

void SegmentPointCloud(PCLViewer* viewer, const std::string& dir, const std::vector<double>& bbox, bool bFilter)
{
  WSPointCloudPtr cloud = viewer->LoadPointCloud(dir);
  if (!cloud)
    return;

  if (bbox.empty())
  {
    std::vector<WSPointCloudPtr> segClouds;
    PCLSegment segment(cloud);
    bool success = segment.Compute(segClouds);
    if (!success)
      return;

    std::cout << segClouds.size() << "segments" << std::endl;
    for (int i = 0; i < segClouds.size(); ++i)
    {
      WSPointCloudPtr segCloud = segClouds[i];
      std::string file = dir+"segment_"+std::to_string(i)+".pcd";
      int res = pcl::io::savePCDFileASCII(file, *segCloud);
      if (res >= 0)
        std::cout << "save point cloud" << i << "as" << file << std::endl;
    }

    int vp = viewer->CreateViewPort(0,0,1,1);
    viewer->AddPointCloud(cloud,vp);

  }
  else
  {
    std::cout << "filter point cloud in bounding box" << bbox[0] << " "<< bbox[1]<< " "
    << bbox[2]<< " " << bbox[3] << " "<< bbox[4] << " "<< bbox[5]<< std::endl;
    PCLFilter filter;
    cloud = filter.FilterPCLPointInBBox(cloud,bbox,bFilter);
    int vp = viewer->CreateViewPort(0,0,1,1);
    viewer->AddPointCloud(cloud,vp);
  }
}

// generate continous scanning path with input Cloud
// 1. slice the cloud with plane
// 2. find all point on the plane
// 3. offset the point
// 4. conncet all the point form a polyline
void GenerateScanningPath(PCLViewer* viewer, const WSPointCloudPtr cloud, double distance, double resolution)
{
  // the point cloud bounding box is
  // x: -5 to -5
  // y: -23 to -13
  // z : 4.5 to 8
  int vp = viewer->CreateViewPort(0,0,1,1);
  viewer->AddPointCloud(cloud, vp);

  std::vector<double> bbox;
  bbox.push_back(0);
  bbox.push_back(5);
  bbox.push_back(-23);
  bbox.push_back(-13);
  bbox.push_back(4.6);
  bbox.push_back(8);
  PCLFilter filter;
  WSPointCloudPtr newCloud = filter.FilterPCLPointInBBox(cloud,bbox,0);
  // WSPointCloudPtr newCloud = filter.FilterPCLPoint(newCloud,0.001);
  double z = 4.6;
  double x = 0;
  double y = -23.0+0.05;
  Eigen::Vector3f normal(0,1,0);
  int n = 1;
  std::vector<WSPoint> ftPts;
  while (y < -13)
  {
    Eigen::Vector3f root(x,y,z);
    WSPointCloudPtr pts = filter.SlicePoints(newCloud, root, normal);
    pts = filter.SortPointsInZ(pts,0.3,4.5,8);
    int numberPt = pts->points.size();
    if (n%2 == 0)
    {
      for (int i = numberPt-1; i >= 0; i--)
        ftPts.push_back(pts->points[i]);
    }
    else
    {
      for (int i = 0; i < numberPt; ++i)
        ftPts.push_back(pts->points[i]);
    }
    y = y + resolution;
    n++;
  }

  // offset
  std::vector<WSPoint> offsetPts;
  for (size_t i = 0; i < ftPts.size(); ++i)
  {
    WSPoint pt = ftPts[i];
    WSNormal nm = filter.PointNormal(pt,cloud,0.3,Eigen::Vector3f(1,0,1));

    WSPoint offset;
    offset.x = pt.x+nm.normal_x*distance;
    offset.y = pt.y+nm.normal_y*distance;
    offset.z = pt.z+nm.normal_z*distance;
    offsetPts.push_back(offset);
  }

  // add polyline path
  for (size_t i = 0; i < offsetPts.size(); ++i)
  {
    if (i > 1)
      viewer->AddLine(offsetPts[i-1],offsetPts[i],i,1.0,0.0,0.0,vp);
  }
}

////////////////////////////////////////////////////////////////
WSPointCloudPtr FilterPointCloud(const WSPointCloudPtr srcCloud, double resolution)
{
  PCLFilter filter;
  WSPointCloudPtr cloud(new WSPointCloud);
  // filter z < 0.02 and z > 10 point
  std::cout << "Filtering: pass through z out of [0.05,20]..." << std::endl;
  cloud = filter.FilterPassThrough(srcCloud,"z",0.05,20);
  /*
  remove outlier
  The number of neighbors to analyze for each point is set to 50,
  and the standard deviation multiplier to 1. What this means is
  that all points who have a distance larger than 1 standard
  deviation of the mean distance to the query point will be marked
  as outliers and removed.
  */
  std::cout << "Filtering: outlier with 50 samples in 1 standard deviation..." << std::endl;
  cloud = filter.FilterPCLPointSOR(cloud,50,1);
  // downsampling with neighbor distance 0.1
  std::cout << "Filtering: downsampling with " << resolution << " meter neighbor distance..." << std::endl;
  cloud = filter.FilterPCLPoint(cloud,resolution);
  return cloud;
}

void PrintHelp()
{
  std::cout << "This app is used for visualizing the point cloud by loading the '.ply' files in a directory.\n";
  std::cout << "Command line with providing a directory containing the .ply files:\n";
  std::cout << "    1. view point cloud:   cmd [-view|-v] [file_directory] [octree resolution] [display_type] [trajectory]\n";
  std::cout << "    2. merge pointcloud:   cmd [-merge|-m] [file_directory] [filter] [downsample resolution]\n";
  std::cout << "    3. create viewpoints:  cmd [-trajectory|-t] [file_directory] [viewpoint distance] [octree resolution] [display_type] [config]\n";
  std::cout << "       display_type: '-4': uncovered voxels; '-3': covered voxels; '-2': add octree voxels; '-1': add camera positions;\n";
  std::cout << "    4. segment pointcloud: cmd [-segment|-s] [file_directory] [xmin] [xmax] [ymin] [ymax] [zmin] [zmax]\n";
  std::cout << "    5. create continuous trajectory: cmd [-c] [file_directory] [offset distance] [margin]\n";
}

int ParseArguments(int argc, char** argv,
                  std::string& dir,
                  std::string& trajectory,
                  bool& save,
                  bool& filter,
                  double& distance,
                  double& resolution,
                  int& display,
                  int& sampleType,
                  std::vector<double>& limitBox,
                  string& configFile
                )
{
  if (argc < 2)
    return 0;
  std::string task = std::string(argv[1]);
  if (task.compare("-v") == 0 || task.compare("-view") == 0)
  {
    dir = std::string(argv[2]);
    if (argc > 3)
      resolution = std::stod(argv[3]);
    if (argc > 4)
      sampleType = std::stoi(argv[4]);
    if (argc > 5)
      trajectory = std::string(argv[5]);

    return 1;
  }
  else if (task.compare("-m") == 0 || task.compare("-merge") == 0)
  {
    save = true;
    dir = std::string(argv[2]);
    if (argc > 3)
      filter = std::string(argv[3]).compare("1")==0;
    if (argc > 4)
      resolution = std::stod(argv[4]);
    return 2;
  }
  else if (task.compare("-t") == 0 || task.compare("-trajectory") == 0)
  {
    dir = std::string(argv[2]);
    if (argc > 3)
      distance = std::stod(argv[3]);
    if (argc > 4)
      resolution = std::stod(argv[4]);
    if (argc > 5)
      display = std::stoi(argv[5]);
    if (argc > 6)
      configFile = std::string(argv[6]);
    return 3;
  }
  else if (task.compare("-s")==0 || task.compare("-segment") == 0)
  {
    save = true;
    dir = std::string(argv[2]);
    if (argc > 8)
    {
      limitBox.push_back(std::stod(argv[3]));
      limitBox.push_back(std::stod(argv[4]));
      limitBox.push_back(std::stod(argv[5]));
      limitBox.push_back(std::stod(argv[6]));
      limitBox.push_back(std::stod(argv[7]));
      limitBox.push_back(std::stod(argv[8]));
    }
    if (argc > 9)
      filter = std::string(argv[9]).compare("1")==0;

    return 4;
  }
  else if (task.compare("-c")==0)
  {
    dir = std::string(argv[2]);
    if (argc > 3)
      distance = std::stod(argv[3]);
    if (argc > 4)
      resolution = std::stod(argv[4]);
    return 5;
  }
  else
  {
    return 0;
  }
}