/****************************************************************************
 * Copyright (c) 2012-2020 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/
#include "ArborX_EnableDeviceTypes.hpp" // ARBORX_DEVICE_TYPES
#include "ArborX_EnableViewComparison.hpp"
#include <ArborX_DetailsAlgorithms.hpp>
#include <ArborX_DetailsKokkosExt.hpp>  // clz
#include <ArborX_DetailsMortonCode.hpp> // expandBits, morton3D
#include <ArborX_DetailsSortUtils.hpp>  // sortObjects
#include <ArborX_DetailsTreeConstruction.hpp>

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <array>
#include <bitset>
#include <functional>
#include <limits>
#include <sstream>
#include <vector>

#define BOOST_TEST_MODULE DetailsTreeConstruction

namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE_TEMPLATE(morton_codes, DeviceType, ARBORX_DEVICE_TYPES)
{
  std::vector<ArborX::Point> points = {
      {{0.0, 0.0, 0.0}},          {{0.25, 0.75, 0.25}}, {{0.75, 0.25, 0.25}},
      {{0.75, 0.75, 0.25}},       {{1.33, 2.33, 3.33}}, {{1.66, 2.66, 3.66}},
      {{1024.0, 1024.0, 1024.0}},
  };
  int const n = points.size();
  // lower left front corner corner of the octant the points fall in
  std::vector<std::array<unsigned int, 3>> anchors = {
      {{0, 0, 0}}, {{0, 0, 0}}, {{0, 0, 0}},         {{0, 0, 0}},
      {{1, 2, 3}}, {{1, 2, 3}}, {{1023, 1023, 1023}}};
  auto fun = [](std::array<unsigned int, 3> const &anchor) {
    using ArborX::Details::expandBits;
    unsigned int i = std::get<0>(anchor);
    unsigned int j = std::get<1>(anchor);
    unsigned int k = std::get<2>(anchor);
    return 4 * expandBits(i) + 2 * expandBits(j) + expandBits(k);
  };
  std::vector<unsigned int> ref(n, std::numeric_limits<unsigned int>::max());
  for (int i = 0; i < n; ++i)
    ref[i] = fun(anchors[i]);
  // using points rather than boxes for convenience here but still have to
  // build the axis-aligned bounding boxes around them
  Kokkos::View<ArborX::Box *, DeviceType> boxes("boxes", n);
  auto boxes_host = Kokkos::create_mirror_view(boxes);
  for (int i = 0; i < n; ++i)
    ArborX::Details::expand(boxes_host(i), points[i]);
  Kokkos::deep_copy(boxes, boxes_host);

  typename DeviceType::execution_space space{};
  ArborX::Box scene_host;
  ArborX::Details::TreeConstruction::calculateBoundingBoxOfTheScene(
      space, boxes, scene_host);

  BOOST_TEST(ArborX::Details::equals(
      scene_host, {{{0., 0., 0.}}, {{1024., 1024., 1024.}}}));

  Kokkos::View<unsigned int *, DeviceType> morton_codes("morton_codes", n);
  ArborX::Details::TreeConstruction::assignMortonCodes(
      space, boxes, morton_codes, scene_host);
  auto morton_codes_host = Kokkos::create_mirror_view(morton_codes);
  Kokkos::deep_copy(morton_codes_host, morton_codes);
  BOOST_TEST(morton_codes_host == ref, tt::per_element());
}

template <typename DeviceType>
class FillK
{
public:
  FillK(Kokkos::View<unsigned int *, DeviceType> k)
      : _k(k)
  {
  }

  KOKKOS_INLINE_FUNCTION
  void operator()(int const i) const { _k[i] = 4 - i; }

private:
  Kokkos::View<unsigned int *, DeviceType> _k;
};

BOOST_AUTO_TEST_CASE_TEMPLATE(indirect_sort, DeviceType, ARBORX_DEVICE_TYPES)
{
  // need a functionality that sort objects based on their Morton code and
  // also returns the indices in the original configuration

  // dummy unsorted Morton codes and corresponding sorted indices as reference
  // solution
  //
  using ExecutionSpace = typename DeviceType::execution_space;
  unsigned int const n = 4;
  Kokkos::View<unsigned int *, DeviceType> k("k", n);
  // Fill K with 4, 3, 2, 1
  FillK<DeviceType> fill_k_functor(k);
  Kokkos::parallel_for("fill_k", Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       fill_k_functor);

  std::vector<size_t> ref = {3, 2, 1, 0};
  // sort Morton codes and object ids
  auto ids = ArborX::Details::sortObjects(ExecutionSpace{}, k);

  auto k_host = Kokkos::create_mirror_view(k);
  Kokkos::deep_copy(k_host, k);
  auto ids_host = Kokkos::create_mirror_view(ids);
  Kokkos::deep_copy(ids_host, ids);

  // check that they are sorted
  for (unsigned int i = 0; i < n; ++i)
    BOOST_TEST(k_host[i] == i + 1);
  // check that ids are properly ordered
  BOOST_TEST(ids_host == ref, tt::per_element());
}

BOOST_AUTO_TEST_CASE(number_of_leading_zero_bits)
{
  using KokkosExt::clz;
  BOOST_TEST(clz(0) == 32);
  BOOST_TEST(clz(1) == 31);
  BOOST_TEST(clz(2) == 30);
  BOOST_TEST(clz(3) == 30);
  BOOST_TEST(clz(4) == 29);
  BOOST_TEST(clz(5) == 29);
  BOOST_TEST(clz(6) == 29);
  BOOST_TEST(clz(7) == 29);
  BOOST_TEST(clz(8) == 28);
  BOOST_TEST(clz(9) == 28);
  // bitwise exclusive OR operator to compare bits
  BOOST_TEST(clz(1 ^ 0) == 31);
  BOOST_TEST(clz(2 ^ 0) == 30);
  BOOST_TEST(clz(2 ^ 1) == 30);
  BOOST_TEST(clz(3 ^ 0) == 30);
  BOOST_TEST(clz(3 ^ 1) == 30);
  BOOST_TEST(clz(3 ^ 2) == 31);
  BOOST_TEST(clz(4 ^ 0) == 29);
  BOOST_TEST(clz(4 ^ 1) == 29);
  BOOST_TEST(clz(4 ^ 2) == 29);
  BOOST_TEST(clz(4 ^ 3) == 29);
}

template <typename DeviceType>
class FillFi
{
public:
  KOKKOS_INLINE_FUNCTION
  FillFi(Kokkos::View<unsigned int *, DeviceType> fi)
      : _fi(fi)
  {
  }

  KOKKOS_INLINE_FUNCTION
  void operator()(int const i) const
  {
    // NOTE: Morton codes below are **not** unique
    unsigned int fi_array[] = {0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144};

    _fi[i] = fi_array[i];
  }

private:
  Kokkos::View<unsigned int *, DeviceType> _fi;
};

template <typename DeviceType>
class ComputeResults
{
public:
  KOKKOS_INLINE_FUNCTION
  ComputeResults(Kokkos::View<unsigned int *, DeviceType> fi,
                 Kokkos::View<int *, DeviceType> results)
      : _fi(fi)
      , _results(results)
  {
  }

  KOKKOS_INLINE_FUNCTION
  void operator()(int const i) const
  {
    int index_1[] = {0, 0, 1, 1, 1, 2, 2, 0, 12, 12};
    int index_2[] = {0, 1, 0, 1, 2, 1, 2, -1, 12, 13};

    _results[i] = ArborX::Details::TreeConstruction::commonPrefix(
        _fi, index_1[i], index_2[i]);
  }

private:
  Kokkos::View<unsigned int *, DeviceType> _fi;
  Kokkos::View<int *, DeviceType> _results;
};

BOOST_AUTO_TEST_CASE_TEMPLATE(common_prefix, DeviceType, ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;
  int const n = 13;
  Kokkos::View<unsigned int *, DeviceType> fi("fi", n);
  FillFi<DeviceType> fill_fi_functor(fi);
  Kokkos::parallel_for("fill_fi", Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       fill_fi_functor);

  int const n_tests = 10;
  Kokkos::View<int *, DeviceType> results("results", n_tests);

  ComputeResults<DeviceType> compute_results_functor(fi, results);
  Kokkos::parallel_for("compute_results",
                       Kokkos::RangePolicy<ExecutionSpace>(0, n_tests),
                       compute_results_functor);

  auto results_host = Kokkos::create_mirror_view(results);
  Kokkos::deep_copy(results_host, results);

  auto fi_host = Kokkos::create_mirror_view(fi);
  Kokkos::deep_copy(fi_host, fi);

  BOOST_TEST(results_host[0] == 32 + 32);
  BOOST_TEST(results_host[1] == 31);
  BOOST_TEST(results_host[2] == 31);
  // duplicate Morton codes
  BOOST_TEST(fi_host[1] == 1);
  BOOST_TEST(fi_host[1] == fi_host[2]);
  BOOST_TEST(results_host[3] == 64);
  BOOST_TEST(results_host[4] == 32 + 30);
  BOOST_TEST(results_host[5] == 62);
  BOOST_TEST(results_host[6] == 64);
  // by definition \delta(i, j) = -1 when j \notin [0, n-1]
  BOOST_TEST(results_host[7] == -1);
  BOOST_TEST(results_host[8] == 64);
  BOOST_TEST(results_host[9] == -1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(example_tree_construction, DeviceType,
                              ARBORX_DEVICE_TYPES)
{
  if (!Kokkos::SpaceAccessibility<
          Kokkos::HostSpace, typename DeviceType::memory_space>::accessible)
    return;

  // This is the example from the articles by Karras.
  // See
  // https://devblogs.nvidia.com/parallelforall/thinking-parallel-part-iii-tree-construction-gpu/
  int const n = 8;
  Kokkos::View<unsigned int *, DeviceType> sorted_morton_codes(
      "sorted_morton_codes", n);
  std::vector<std::string> s{
      "00001", "00010", "00100", "00101", "10011", "11000", "11001", "11110",
  };
  for (int i = 0; i < n; ++i)
  {
    std::bitset<6> b(s[i]);
    std::cout << b << "  " << b.to_ulong() << "\n";
    sorted_morton_codes(i) = b.to_ulong();
  }

  // reference solution for a recursive traversal from top to bottom
  // starting from root, visiting first the left child and then the right one
  std::ostringstream ref;
  ref << "I0"
      << "I3"
      << "I1"
      << "L0"
      << "L1"
      << "I2"
      << "L2"
      << "L3"
      << "I4"
      << "L4"
      << "I5"
      << "I6"
      << "L5"
      << "L6"
      << "L7";
  std::cout << "ref=" << ref.str() << "\n";

  // hierarchy generation
  using ArborX::Box;
  using ArborX::Details::makeLeafNode;
  using ArborX::Details::Node;
  Kokkos::View<Node *, DeviceType> leaf_nodes("leaf_nodes", n);
  Kokkos::View<Node *, DeviceType> internal_nodes("internal_nodes", n - 1);
  for (int i = 0; i < n; ++i)
    leaf_nodes(i) = makeLeafNode(i, Box{});
  auto getNodePtr = [&leaf_nodes, &internal_nodes](int i) {
    return i < n - 1 ? &internal_nodes(i) : &leaf_nodes(i - n + 1);
  };
  std::function<void(Node const *, std::ostream &)> traverseRecursive;
  traverseRecursive = [&leaf_nodes, &internal_nodes, &traverseRecursive,
                       &getNodePtr](Node const *node, std::ostream &os) {
    if (node->isLeaf())
    {
      os << "L" << node - leaf_nodes.data();
    }
    else
    {
      os << "I" << node - internal_nodes.data();
      for (Node const *child : {getNodePtr(node->children.first),
                                getNodePtr(node->children.second)})
        traverseRecursive(child, os);
    }
  };

  Kokkos::View<int *, DeviceType> parents("parents", 2 * n + 1);
  Kokkos::deep_copy(parents, -1);

  typename DeviceType::execution_space space{};
  ArborX::Details::TreeConstruction::generateHierarchy(
      space, sorted_morton_codes, leaf_nodes, internal_nodes, parents);

  BOOST_TEST(parents(0) == -1);

  Node const *root = internal_nodes.data();

  std::ostringstream sol;
  traverseRecursive(root, sol);
  std::cout << "sol=" << sol.str() << "\n";

  BOOST_TEST(sol.str().compare(ref.str()) == 0);
}


// SPDX-License-Identifier: BSD-2-Clause

#ifndef KEYFRAME_HPP
#define KEYFRAME_HPP

#include <boost/optional.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/ros.h>

namespace g2o {
class VertexSE3;
class HyperGraph;
class SparseOptimizer;
} // namespace g2o

namespace hdl_graph_slam {

/**
 * @brief KeyFrame (pose node)
 */
struct KeyFrame {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using PointT = pcl::PointXYZI;
  using Ptr = std::shared_ptr<KeyFrame>;

  KeyFrame(const ros::Time &stamp, const Eigen::Isometry3d &odom,
           double accum_distance,
           const pcl::PointCloud<PointT>::ConstPtr &cloud);
  KeyFrame(const std::string &directory, g2o::HyperGraph *graph);
  virtual ~KeyFrame();

  void save(const std::string &directory);
  bool load(const std::string &directory, g2o::HyperGraph *graph);

  long id() const;
  Eigen::Isometry3d estimate() const;

public:
  ros::Time stamp; // timestamp 时间戳
  Eigen::Isometry3d
      odom; // odometry (estimated by scan_matching_odometry) 里程计
  double accum_distance; // accumulated distance from the first node (by
                         // scan_matching_odometry)从第一个节点累计距离
  pcl::PointCloud<PointT>::ConstPtr cloud; // point cloud
  boost::optional<Eigen::Vector4d>
      floor_coeffs; // detected floor's coefficients  检测地面的系数
  boost::optional<Eigen::Vector3d>
      utm_coord; // UTM coord obtained by GPS 通过GPS 获取UTM坐标

  boost::optional<Eigen::Vector3d> acceleration;   //
  boost::optional<Eigen::Quaterniond> orientation; //

  g2o::VertexSE3 *node; // node instance
};

/**
 * @brief KeyFramesnapshot for map cloud generation
 */
struct KeyFrameSnapshot {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  using PointT = KeyFrame::PointT;
  using Ptr = std::shared_ptr<KeyFrameSnapshot>;

  KeyFrameSnapshot(const KeyFrame::Ptr &key);
  KeyFrameSnapshot(const Eigen::Isometry3d &pose,
                   const pcl::PointCloud<PointT>::ConstPtr &cloud);

  ~KeyFrameSnapshot();

public:
  Eigen::Isometry3d pose; // pose estimated by graph optimization
  pcl::PointCloud<PointT>::ConstPtr cloud; // point cloud
};

} // namespace hdl_graph_slam

#endif // KEYFRAME_HPP


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

// Copyright (c) 2007-2012 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)

#ifndef BOOST_GEOMETRY_TEST_AREA_HPP
#define BOOST_GEOMETRY_TEST_AREA_HPP

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/read.hpp>

template <typename Geometry>
void test_area(Geometry const& geometry,
            typename bg::default_area_result<Geometry>::type expected_area)
{
    typename bg::default_area_result<Geometry>::type area = bg::area(geometry);

#ifdef GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << " "
        << typeid(typename bg::default_area_result<Geometry>::type).name()
        << " "
        << "area : " << bg::area(geometry)
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(area, expected_area, 0.0001);

    // Test with explicitly defined strategies
    bg::strategy::area::surveyor
        <
            typename bg::point_type<Geometry>::type
        > strategy1;

    area = bg::area(geometry, strategy1);

    bg::strategy::area::surveyor
        <
            typename bg::point_type<Geometry>::type,
            typename bg::coordinate_type<Geometry>::type
        > strategy2;

    area = bg::area(geometry, strategy2);

}

template <typename Geometry>
void test_geometry(std::string const& wkt,
            typename bg::default_area_result<Geometry>::type expected_area)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    test_area(geometry, expected_area);
}

template <typename Geometry>
void test_empty_input(Geometry const& geometry)
{
    try
    {
        bg::area(geometry);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}

#endif


//==============================================================================
//         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_META_INT_C_HPP_INCLUDED
#define BOOST_SIMD_SDK_META_INT_C_HPP_INCLUDED

#include <boost/config.hpp>

namespace boost { namespace simd { namespace meta
{
 template<typename Type, Type Value> struct int_c
 {
   BOOST_STATIC_CONSTANT(Type, value = Value);
   typedef int_c type;
   typedef Type   value_type;
   operator value_type() const { return Value; }
 };
} } }

#endif


/**
 * @file crypto.hpp
 * @author Bensuperpc (bensuperpc@gmail.com)
 * @brief
 * @version 1.0.0
 * @date 2021-04-01
 *
 * MIT License
 *
 */

/*
** Source:
https://stackoverflow.com/questions/178265/what-is-the-most-hard-to-understand-piece-of-c-code-you-know
https://cs.uwaterloo.ca/~m32rober/rsqrt.pdf
https://github.com/bavlayan/Encrypt-Decrypt-with-OpenSSL---RSA
*/

#ifndef CRYPTO_HPP_
#define CRYPTO_HPP_

#define BUFFSIZE 16384

#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>

// For MD5 and SHA
#include <boost/crc.hpp> //CRC32
#include <openssl/md5.h>
#include <openssl/sha.h>
#include <string_view>

//#include "openssl_rsa.h"

extern "C" {
#include "banned.h"
};

#define BUFFSIZE 16384

namespace my {
namespace crypto {
/**
 * @brief Get the md5hash object
 *
 * @param str
 * @return std::string
 */
std::string get_md5hash(const std::string &str);
// void get_md5hash_from_string(const unsigned char *, size_t &);
/**
 * @brief Get the md5hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_md5hash_from_string(const std::string &str);

/**
 * @brief Get the sha1hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha1hash(const std::string &str);
// void get_sha1hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha1hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha1hash_from_string(const std::string &str);

/**
 * @brief Get the sha224hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha224hash(const std::string &str);
// void get_sha224hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha224hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha224hash_from_string(const std::string &str);

/**
 * @brief Get the sha256hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha256hash(const std::string &str);
// void get_sha256hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha256hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha256hash_from_string(const std::string &str);

/**
 * @brief Get the sha384hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha384hash(const std::string &str);
// void get_sha384hash_from_string(const unsigned char *, size_t &);

/**
 * @brief Get the sha384hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha384hash_from_string(const std::string &str);

/**
 * @brief Get the sha512hash object
 *
 * @param str
 * @return std::string
 */
std::string get_sha512hash(const std::string &str);
// void get_sha512hash_from_string(const unsigned char *, size_t &);
/**
 * @brief Get the sha512hash from string object
 *
 * @param str
 * @return std::string
 */
std::string get_sha512hash_from_string(const std::string &str);
} // namespace crypto
} // namespace my
// https://www.quora.com/How-can-I-get-the-MD5-or-SHA-hash-of-a-file-in-C

std::string my::crypto::get_md5hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[MD5_DIGEST_LENGTH]; // == 16

  std::stringstream ss;
  std::string md5string;

  std::ifstream ifs(fname, std::ifstream::binary);

  MD5_CTX md5Context;

  MD5_Init(&md5Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    MD5_Update(&md5Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = MD5_Final(digest, &md5Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  md5string = ss.str();

  return md5string;
}

std::string my::crypto::get_md5hash_from_string(const std::string &str) {
  unsigned char result[MD5_DIGEST_LENGTH];
  MD5((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}

std::string my::crypto::get_sha1hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[SHA_DIGEST_LENGTH];

  std::stringstream ss;
  std::string sha1string;

  std::ifstream ifs(fname, std::ifstream::binary);

  SHA_CTX sha1Context;

  SHA1_Init(&sha1Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    SHA1_Update(&sha1Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = SHA1_Final(digest, &sha1Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  sha1string = ss.str();

  return sha1string;
}

std::string my::crypto::get_sha1hash_from_string(const std::string &str) {
  unsigned char result[SHA_DIGEST_LENGTH];
  SHA1((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}

std::string my::crypto::get_sha224hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[SHA224_DIGEST_LENGTH];

  std::stringstream ss;
  std::string sha224string;

  std::ifstream ifs(fname, std::ifstream::binary);

  SHA256_CTX sha224Context;

  SHA224_Init(&sha224Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    SHA256_Update(&sha224Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = SHA256_Final(digest, &sha224Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  sha224string = ss.str();

  return sha224string;
}

std::string my::crypto::get_sha224hash_from_string(const std::string &str) {
  unsigned char result[SHA224_DIGEST_LENGTH];
  SHA224((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}

std::string my::crypto::get_sha256hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[SHA256_DIGEST_LENGTH];

  std::stringstream ss;
  std::string sha256string;

  std::ifstream ifs(fname, std::ifstream::binary);

  SHA256_CTX sha256Context;

  SHA256_Init(&sha256Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    SHA256_Update(&sha256Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = SHA256_Final(digest, &sha256Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  sha256string = ss.str();

  return sha256string;
}

std::string my::crypto::get_sha256hash_from_string(const std::string &str) {
  unsigned char result[SHA256_DIGEST_LENGTH];
  SHA256((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}

std::string my::crypto::get_sha384hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[SHA384_DIGEST_LENGTH];

  std::stringstream ss;
  std::string sha384string;

  std::ifstream ifs(fname, std::ifstream::binary);

  SHA512_CTX sha384Context;

  SHA384_Init(&sha384Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    SHA384_Update(&sha384Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = SHA384_Final(digest, &sha384Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  sha384string = ss.str();

  return sha384string;
}

std::string my::crypto::get_sha384hash_from_string(const std::string &str) {
  unsigned char result[SHA384_DIGEST_LENGTH];
  SHA384((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}

std::string my::crypto::get_sha512hash(const std::string &fname) {

  char buffer[BUFFSIZE];
  unsigned char digest[SHA512_DIGEST_LENGTH];

  std::stringstream ss;
  std::string sha512string;

  std::ifstream ifs(fname, std::ifstream::binary);

  SHA512_CTX sha512Context;

  SHA512_Init(&sha512Context);

  while (ifs.good()) {
    ifs.read(buffer, BUFFSIZE);
    SHA512_Update(&sha512Context, buffer, static_cast<size_t>(ifs.gcount()));
  }

  ifs.close();

  auto &&res = SHA512_Final(digest, &sha512Context);

  if (res == 0) // hash failed
    return {};  // or raise an exception

  // ss << std::hex << std::uppercase << std::setfill('0');
  ss << std::hex << std::setfill('0');
  for (unsigned char uc : digest)
    ss << std::setw(2) << (int)uc;

  sha512string = ss.str();

  return sha512string;
}

std::string my::crypto::get_sha512hash_from_string(const std::string &str) {
  unsigned char result[SHA512_DIGEST_LENGTH];
  SHA512((const unsigned char *)str.c_str(), str.size(), result);

  std::ostringstream sout;
  sout << std::hex << std::setfill('0');
  for (long long c : result) {
    sout << std::setw(2) << (long long)c;
  }
  return sout.str();
}
/*
void my::crypto::get_sha512hash_from_string(const unsigned char *cstr, unsigned
char *out, size_t &size)
{
    unsigned char digest[SHA512_DIGEST_LENGTH];
    MD5Context context;
    MD5Init(&context);
    MD5Update(&context, string, strlen(string));
    MD5Final(digest, &context);

    unsigned char hash[SHA512_DIGEST_LENGTH]; // == 16
    SHA512(cstr, sizeof(*cstr) - 1, hash);
    unsigned char * hash_ptr = hash;
}
*/

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "Viewer.h"

//#include <chrono>
#include <thread>

#include <Eigen/LU>

#include <cmath>
#include <cstdio>
#include <sstream>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <limits>
#include <cassert>

#include <igl/project.h>
//#include <igl/get_seconds.h>
#include <igl/readOBJ.h>
#include <igl/readOFF.h>
#include <igl/adjacency_list.h>
#include <igl/writeOBJ.h>
#include <igl/writeOFF.h>
#include <igl/massmatrix.h>
#include <igl/file_dialog_open.h>
#include <igl/file_dialog_save.h>
#include <igl/quat_mult.h>
#include <igl/axis_angle_to_quat.h>
#include <igl/trackball.h>
#include <igl/two_axis_valuator_fixed_up.h>
#include <igl/snap_to_canonical_view_quat.h>
#include <igl/unproject.h>
#include <igl\circulation.h>
#include <igl/serialize.h>
#include<igl\edge_collapse_is_valid.h>
#include <igl\edge_flaps.h>
#include <igl\vertex_triangle_adjacency.h>
#include <igl/collapse_edge.h>

// Internal global variables used for glfw event handling
//static igl::opengl::glfw::Viewer * __viewer;
static double highdpi = 1;
static double scroll_x = 0;
static double scroll_y = 0;

namespace igl
{
namespace opengl
{
namespace glfw
{

  void Viewer::Init(const std::string config)
  {

    //  load_mesh_from_configuration(config);

  }

  IGL_INLINE Viewer::Viewer():
    data_list(1),
    selected_data_index(0),
    next_data_id(1),
	isPicked(false),
	isActive(false)
  {
    data_list.front().id = 0;

    // Temporary variables initialization
   // down = false;
  //  hack_never_moved = true;
    scroll_position = 0.0f;

    // Per face
    data().set_face_based(false);

#ifndef IGL_VIEWER_VIEWER_QUIET
    const std::string usage(R"(igl::opengl::glfw::Viewer usage:
  [drag]  Rotate scene
  A,a     Toggle animation (tight draw loop)
  F,f     Toggle face based
  I,i     Toggle invert normals
  L,l     Toggle wireframe
  O,o     Toggle orthographic/perspective projection
  T,t     Toggle filled faces
  [,]     Toggle between cameras
  1,2     Toggle between models
  ;       Toggle vertex labels
  :       Toggle face labels
  P    Enable simplefication
  x    Disable simplefication )"
);
    std::cout<<usage<<std::endl;
#endif
  }

  IGL_INLINE Viewer::~Viewer()
  {
  }

  /*
  in order to perform a certian contraction we will need to calculate the cost of a contraction
  to define this cost we attemp to characterize the error at each vertex in the mesh .

  to do this : we associate a symmatric 4x4 matrix named Q with each vertex .

  and we define the error at a vertix v to be

  v = = [vx vy vz 1]T  to be the quadratic form tirangle(v) = vTQv.

  meaning that the error is equal to vTQv.

  let symbol and name a given constraction (V1,v2) as v~

  * now we mush derive a new matrix that we call Q~ which approximates the error at v~
  *
  *  the rule is that Q~ = Q1 + Q2 - WHERE Q1 is the associted matrix for v1 and Q2 is the associated matrix of v2
  *
  *
  * in order to do the constraction we must find a position for v~  - the simple scheme would be to either choose
  * 1. v1
  * 2. v2
  * 3. v1+v2 \2
  *
  *
  * now it depends on which of these have the lowest value at triangelV
  *
  *
  *  meaning which of the following has the lowest value
  *
  * 1. v1T X  Q1 X v1
  *
  * 2. v2T X  Q2 X v2
  *
  * 3. ((v1+v2)/2)T  x Q~ x (v1+v2)
  *
  *
  * now it is mentioned how to calculate the v~
  *
  *
  *
  *
  *
  *

  */

  void calculate_cost(
      std::vector<Eigen::Matrix<double, 4, 4>>& Qs,
      const int e,
      const Eigen::MatrixXd& V,
      const Eigen::MatrixXi& /*F*/,
      const Eigen::MatrixXi& E,
      const Eigen::VectorXi& /*EMAP*/,
      const Eigen::MatrixXi& /*EF*/,
      const Eigen::MatrixXi& /*EI*/,
      double& cost,
      Eigen::RowVectorXd& p)
  {
      auto Q1 = Qs[E.row(e)[0]];
      auto Q2 = Qs[E.row(e)[1]];
      Eigen::Matrix4d Q = Q1 + Q2;
      Q.row(3) = Eigen::Vector4d(0, 0, 0, 1);

      bool invertable = false;
      Eigen::Matrix<double, 4, 4> Qi;
      Q.computeInverseWithCheck(Qi, invertable, 0);

      Eigen::Matrix<double, 4, 1> a;
      if (invertable) {
          // this is the v~
          a = Qi * Eigen::Matrix<double, 4, 1>(0, 0, 0, 1);
      }
      else {
          // we need to choose the midpoint
          a = (V.row(E.row(e)[0]) + V.row(E.row(e)[1])) / 2;
          a(3) = 1;
      }

      cost = a.transpose() * (Q1 + Q2) * a;
      // new point s
      p = Eigen::RowVector3d(a(0), a(1), a(2));
  }

  //Assingment 1 task 6
  IGL_INLINE void Viewer::init_objs_simpelified() {

      data().Q = new std::set<std::pair<double, int> >(); // priority Q contains the cost of edges <cost , number of edge >
      data().EMAP = new Eigen::VectorXi();               // connects faces to edges

      /*
      E - matrix of edges
      |1 , 3 |
      |2 , 5 |
      |3, 1  |

       source , destenation

      */

      data().E = new Eigen::MatrixXi();                   // this is the Edges -> edge is represented by <index of source vertex, index of destinations vertex> - matrix []

      data().EI = new Eigen::MatrixXi();                  //connects edge to vertex index in triangle (0 1 2)

      data().EF = new Eigen::MatrixXi();                    //connects edges to faces this is a matrix that have [index of first face , index of second face ]

      data().C = new Eigen::MatrixXd();                 //position of the new vertex after collapsing the corresponding edge

      data().V1 = new Eigen::MatrixXd(data().V);            // the new back up for v

      data().F1 = new Eigen::MatrixXi(data().F);            // the new backup for F

      data().Qs = std::vector<Eigen::Matrix<double, 4, 4>>();
      data().Qit = new std::vector<std::set<std::pair<double, int> >::iterator >(); // saved iterator for every dege so that

      //this gets the EMAP EF E EI ready
      edge_flaps(*data().F1, *data().E, *data().EMAP, *data().EF, *data().EI);

      data().Qit->resize(data().E->rows());

      data().C->resize(data().E->rows(), data().V.cols());

      Eigen::VectorXd costs(data().E->rows());

      data().Q->clear();

      auto VF = std::vector<std::vector<int> >();
      auto VFi = std::vector<std::vector<int> >();

      vertex_triangle_adjacency(data().V, data().F, VF, VFi);

      // this is for getting the Q ready
      for (int v = 0; v < data().V.rows(); v++) {

          std::vector<int> faces = VF[v];
          Eigen::Matrix4d Q = Eigen::Matrix4d::Zero();

          for (int face : faces) {
              auto n = data().F_normals.row(face).normalized();
              float d = 0;
              for (int j = 0; j < 3; j++)
              d += (-n[j] * data().V.row(v)[j]);
              Eigen::RowVector4d p(n[0], n[1], n[2], d);
              p = p.transpose();
              Q += p.transpose() * p;
          }

          data().Qs.push_back(Q);
      }
      for (int e = 0; e < data().E->rows(); e++)
      {
     // here i want to calculate all the edges cost
          double cost = 0;
          Eigen::RowVectorXd p(1, 3);

          calculate_cost(data().Qs, e, *data().V1, *data().F1, *data().E, *data().EMAP, *data().EF, *data().EI, cost, p);
          data().C->row(e) = p;
          (*data().Qit)[e] = (data().Q->insert(std::pair<double, int>(cost, e))).first;
      }

  }

  // ASSIGNMENT 1 TASK 7 & TASK 9

  IGL_INLINE void Viewer::simplify_mesh(int num_of_faces_to_delete) {

      printf("# of faces to delete %d \n", num_of_faces_to_delete);
      bool something_collapsed = false;
      int num_of_collapsed_edges = 0;

      for (int i = 0; i < num_of_faces_to_delete; i++) {

          if (!collapse_edge(calculate_cost, data().Qs,
              *data().V1,*data().F1,
              *data().E, *data().EMAP,
              *data().EF, *data().EI ,
              *data().Q, *data().Qit,
              * data().C))
          {
              break;

          }
          something_collapsed = true;
          data().num_collapsed++;

      }
      if (something_collapsed) {
          printf("succesfully collapsed # %d of edges \n",data().num_collapsed);
          data().clear();
          data().set_mesh(*data().V1, *data().F1);
          data().set_face_based(true);
          data().dirty = 157;

      }

  }
  IGL_INLINE bool Viewer::collapse_edge(
      const std::function<void(
          std::vector<Eigen::Matrix<double, 4, 4>>& Qs,
          const int,
          const Eigen::MatrixXd&,
          const Eigen::MatrixXi&,
          const Eigen::MatrixXi&,
          const Eigen::VectorXi&,
          const Eigen::MatrixXi&,
          const Eigen::MatrixXi&,
          double&,
          Eigen::RowVectorXd&)>& cost_and_placement,
      std::vector<Eigen::Matrix<double, 4, 4>>& Qs,
      Eigen::MatrixXd& V,
      Eigen::MatrixXi& F,
      Eigen::MatrixXi& E,
      Eigen::VectorXi& EMAP,
      Eigen::MatrixXi& EF,
      Eigen::MatrixXi& EI,
      std::set<std::pair<double, int> >& Q,
      std::vector<std::set<std::pair<double, int> >::iterator >& Qit,
      Eigen::MatrixXd& C)
   {

      printf("in collapse edge func \n");

      using namespace Eigen;

      int e;
      int e1;
      int e2;
      int f1;
      int f2;

      if (Q.empty()) {
          printf("Q is empty");
          return false;
      }
      printf("QUEUE SIZE IS %d", Q.size());

      // now we know that the qeueu is not empty and we have some data in it
      // we want to get the first pair , meaning the first edge - with the lowest cost?

      std::pair<double, int> p = *(Q.begin());

      if (p.first == std::numeric_limits<double>::infinity())
      {
          printf("its infinity - returning \n");
          // min cost edge is infinite cost
          return false;
      }

      // the first edge is not ininity and we want to remove it
      // remember that the data of first edge is saved in p
      Q.erase(Q.begin());
      e = p.second;
      Qit[e] = Q.end();
      std::vector<int> N = igl::circulation(e, true, EMAP, EF, EI);
      std::vector<int> Nd = igl::circulation(e, false, EMAP, EF, EI);
      N.insert(N.begin(), Nd.begin(), Nd.end());

      bool collapsed = this->collapse(e, C.row(e), V, F, E, EMAP, EF, EI, Qs, e1, e2, f1, f2);

      if (collapsed) {
          double cost_of_Edge = p.first;

          printf("we collapsed the following edges : e -> %d , cost : %f     e1 -> %d  , cost: %f   e2 ->  , cost:    ", e, cost_of_Edge, e1, Qit[e1]->first);
          // Erase the two, other collapsed edges
          Q.erase(Qit[e1]);
          Qit[e1] = Q.end();
          Q.erase(Qit[e2]);
          Qit[e2] = Q.end();

          // update local neighbors
          // loop over original face neighbors
          for (auto n : N)
          {
              if (F(n, 0) != IGL_COLLAPSE_EDGE_NULL ||
                  F(n, 1) != IGL_COLLAPSE_EDGE_NULL ||
                  F(n, 2) != IGL_COLLAPSE_EDGE_NULL)
              {
                  for (int v = 0; v < 3; v++)
                  {

                      const int ei = EMAP(v * F.rows() + n);

                      Q.erase(Qit[ei]);

                      double cost;
                      RowVectorXd place;
                      cost_and_placement(Qs, ei, V, F, E, EMAP, EF, EI, cost, place);

                      Qit[ei] = Q.insert(std::pair<double, int>(cost, ei)).first;
                      C.row(ei) = place;
                  }
              }
          }
      }
      else
      {

          p.first = std::numeric_limits<double>::infinity();
          Qit[e] = Q.insert(p).first;
      }
      return collapsed;
  }

  IGL_INLINE bool Viewer::collapse(
      const int e,
      const Eigen::RowVectorXd& p,
      Eigen::MatrixXd& V,
      Eigen::MatrixXi& F,
      Eigen::MatrixXi& E,
      Eigen::VectorXi& EMAP,
      Eigen::MatrixXi& EF,
      Eigen::MatrixXi& EI,
      std::vector<Eigen::Matrix<double, 4, 4>>& Qs,
      int& a_e1,
      int& a_e2,
      int& a_f1,
      int& a_f2)
  {
      // Assign this to 0 rather than, say, -1 so that deleted elements will get
                 // draw as degenerate elements at vertex 0 (which should always exist and
                 // never get collapsed to anything else since it is the smallest index)
      using namespace Eigen;
      using namespace std;
      const int eflip = E(e, 0) > E(e, 1);
      // source and destination
      const int s = eflip ? E(e, 1) : E(e, 0);
      const int d = eflip ? E(e, 0) : E(e, 1);

      if (!igl::edge_collapse_is_valid(e, F, E, EMAP, EF, EI))
      {
          return false;
      }

      // Important to grab neighbors of d before monkeying with edges
      const std::vector<int> nV2Fd = igl::circulation(e, !eflip, EMAP, EF, EI);

      // The following implementation strongly relies on s<d
      assert(s < d && "s should be less than d");
      Qs[s] = Qs[s] + Qs[d];
      // move source and destination to midpoint
      V.row(s) = p;
      V.row(d) = p;

      // Helper function to replace edge and associate information with NULL
      const auto& kill_edge = [&E, &EI, &EF](const int e)
      {
          E(e, 0) = IGL_COLLAPSE_EDGE_NULL;
          E(e, 1) = IGL_COLLAPSE_EDGE_NULL;
          EF(e, 0) = IGL_COLLAPSE_EDGE_NULL;
          EF(e, 1) = IGL_COLLAPSE_EDGE_NULL;
          EI(e, 0) = IGL_COLLAPSE_EDGE_NULL;
          EI(e, 1) = IGL_COLLAPSE_EDGE_NULL;
      };

      // update edge info
      // for each flap
      const int m = F.rows();
      for (int side = 0; side < 2; side++)
      {
          const int f = EF(e, side);
          const int v = EI(e, side);
          const int sign = (eflip == 0 ? 1 : -1) * (1 - 2 * side);
          // next edge emanating from d
          const int e1 = EMAP(f + m * ((v + sign * 1 + 3) % 3));
          // prev edge pointing to s
          const int e2 = EMAP(f + m * ((v + sign * 2 + 3) % 3));
          assert(E(e1, 0) == d || E(e1, 1) == d);
          assert(E(e2, 0) == s || E(e2, 1) == s);
          // face adjacent to f on e1, also incident on d
          const bool flip1 = EF(e1, 1) == f;
          const int f1 = flip1 ? EF(e1, 0) : EF(e1, 1);
          assert(f1 != f);
          assert(F(f1, 0) == d || F(f1, 1) == d || F(f1, 2) == d);
          // across from which vertex of f1 does e1 appear?
          const int v1 = flip1 ? EI(e1, 0) : EI(e1, 1);
          // Kill e1
          kill_edge(e1);
          // Kill f
          F(f, 0) = IGL_COLLAPSE_EDGE_NULL;
          F(f, 1) = IGL_COLLAPSE_EDGE_NULL;
          F(f, 2) = IGL_COLLAPSE_EDGE_NULL;
          // map f1's edge on e1 to e2
          assert(EMAP(f1 + m * v1) == e1);
          EMAP(f1 + m * v1) = e2;
          // side opposite f2, the face adjacent to f on e2, also incident on s
          const int opp2 = (EF(e2, 0) == f ? 0 : 1);
          assert(EF(e2, opp2) == f);
          EF(e2, opp2) = f1;
          EI(e2, opp2) = v1;
          // remap e2 from d to s
          E(e2, 0) = E(e2, 0) == d ? s : E(e2, 0);
          E(e2, 1) = E(e2, 1) == d ? s : E(e2, 1);
          if (side == 0)
          {
              a_e1 = e1;
              a_f1 = f;
          }
          else
          {
              a_e2 = e1;
              a_f2 = f;
          }
      }

      // finally, reindex faces and edges incident on d. Do this last so asserts
      // make sense.
      //
      // Could actually skip first and last, since those are always the two
      // collpased faces.
      for (auto f : nV2Fd)
      {
          for (int v = 0; v < 3; v++)
          {
              if (F(f, v) == d)
              {
                  const int flip1 = (EF(EMAP(f + m * ((v + 1) % 3)), 0) == f) ? 1 : 0;
                  const int flip2 = (EF(EMAP(f + m * ((v + 2) % 3)), 0) == f) ? 0 : 1;
                  assert(
                      E(EMAP(f + m * ((v + 1) % 3)), flip1) == d ||
                      E(EMAP(f + m * ((v + 1) % 3)), flip1) == s);
                  E(EMAP(f + m * ((v + 1) % 3)), flip1) = s;
                  assert(
                      E(EMAP(f + m * ((v + 2) % 3)), flip2) == d ||
                      E(EMAP(f + m * ((v + 2) % 3)), flip2) == s);
                  E(EMAP(f + m * ((v + 2) % 3)), flip2) = s;
                  F(f, v) = s;
                  break;
              }
          }
      }
      // Finally, "remove" this edge and its information
      kill_edge(e);

      return true;
  }
  IGL_INLINE bool Viewer::check_if_infinity(double first) {
      return first == std::numeric_limits<double>::infinity();

  }

  IGL_INLINE bool Viewer::load_mesh_from_file(
      const std::string & mesh_file_name_string)
  {

    // Create new data slot and set to selected
    if(!(data().F.rows() == 0  && data().V.rows() == 0))
    {
      append_mesh();
    }
    data().clear();

    size_t last_dot = mesh_file_name_string.rfind('.');
    if (last_dot == std::string::npos)
    {
      std::cerr<<"Error: No file extension found in "<<
        mesh_file_name_string<<std::endl;
      return false;
    }

    std::string extension = mesh_file_name_string.substr(last_dot+1);

    if (extension == "off" || extension =="OFF")
    {
      Eigen::MatrixXd V;
      Eigen::MatrixXi F;
      if (!igl::readOFF(mesh_file_name_string, V, F))
        return false;
      data().set_mesh(V,F);
    }
    else if (extension == "obj" || extension =="OBJ")
    {
      Eigen::MatrixXd corner_normals;
      Eigen::MatrixXi fNormIndices;

      Eigen::MatrixXd UV_V;
      Eigen::MatrixXi UV_F;
      Eigen::MatrixXd V;
      Eigen::MatrixXi F;

      if (!(
            igl::readOBJ(
              mesh_file_name_string,
              V, UV_V, corner_normals, F, UV_F, fNormIndices)))
      {
        return false;
      }

      data().set_mesh(V,F);
      if (UV_V.rows() > 0)
      {
          data().set_uv(UV_V, UV_F);
      }

    }
    else
    {
      // unrecognized file type
      printf("Error: %s is not a recognized file type.\n",extension.c_str());
      return false;
    }

    data().compute_normals();
    data().uniform_colors(Eigen::Vector3d(51.0/255.0,43.0/255.0,33.3/255.0),
                   Eigen::Vector3d(255.0/255.0,228.0/255.0,58.0/255.0),
                   Eigen::Vector3d(255.0/255.0,235.0/255.0,80.0/255.0));

    // Alec: why?
    if (data().V_uv.rows() == 0)
    {
      data().grid_texture();
    }

    //for (unsigned int i = 0; i<plugins.size(); ++i)
    //  if (plugins[i]->post_load())
    //    return true;

    return true;
  }

  IGL_INLINE bool Viewer::save_mesh_to_file(
      const std::string & mesh_file_name_string)
  {
    // first try to load it with a plugin
    //for (unsigned int i = 0; i<plugins.size(); ++i)
    //  if (plugins[i]->save(mesh_file_name_string))
    //    return true;

    size_t last_dot = mesh_file_name_string.rfind('.');
    if (last_dot == std::string::npos)
    {
      // No file type determined
      std::cerr<<"Error: No file extension found in "<<
        mesh_file_name_string<<std::endl;
      return false;
    }
    std::string extension = mesh_file_name_string.substr(last_dot+1);
    if (extension == "off" || extension =="OFF")
    {
      return igl::writeOFF(
        mesh_file_name_string,data().V,data().F);
    }
    else if (extension == "obj" || extension =="OBJ")
    {
      Eigen::MatrixXd corner_normals;
      Eigen::MatrixXi fNormIndices;

      Eigen::MatrixXd UV_V;
      Eigen::MatrixXi UV_F;

      return igl::writeOBJ(mesh_file_name_string,
          data().V,
          data().F,
          corner_normals, fNormIndices, UV_V, UV_F);
    }
    else
    {
      // unrecognized file type
      printf("Error: %s is not a recognized file type.\n",extension.c_str());
      return false;
    }
    return true;
  }

  IGL_INLINE bool Viewer::load_scene()
  {
    std::string fname = igl::file_dialog_open();
    if(fname.length() == 0)
      return false;
    return load_scene(fname);
  }

  IGL_INLINE bool Viewer::load_scene(std::string fname)
  {
   // igl::deserialize(core(),"Core",fname.c_str());
    igl::deserialize(data(),"Data",fname.c_str());
    return true;
  }

  IGL_INLINE bool Viewer::save_scene()
  {
    std::string fname = igl::file_dialog_save();
    if (fname.length() == 0)
      return false;
    return save_scene(fname);
  }

  IGL_INLINE bool Viewer::save_scene(std::string fname)
  {
    //igl::serialize(core(),"Core",fname.c_str(),true);
    igl::serialize(data(),"Data",fname.c_str());

    return true;
  }

  IGL_INLINE void Viewer::open_dialog_load_mesh()
  {
    std::string fname = igl::file_dialog_open();

    if (fname.length() == 0)
      return;

    this->load_mesh_from_file(fname.c_str());
  }

  IGL_INLINE void Viewer::open_dialog_save_mesh()
  {
    std::string fname = igl::file_dialog_save();

    if(fname.length() == 0)
      return;

    this->save_mesh_to_file(fname.c_str());
  }

  IGL_INLINE ViewerData& Viewer::data(int mesh_id /*= -1*/)
  {
    assert(!data_list.empty() && "data_list should never be empty");
    int index;
    if (mesh_id == -1)
      index = selected_data_index;
    else
      index = mesh_index(mesh_id);

    assert((index >= 0 && index < data_list.size()) &&
      "selected_data_index or mesh_id should be in bounds");
    return data_list[index];
  }

  IGL_INLINE const ViewerData& Viewer::data(int mesh_id /*= -1*/) const
  {
    assert(!data_list.empty() && "data_list should never be empty");
    int index;
    if (mesh_id == -1)
      index = selected_data_index;
    else
      index = mesh_index(mesh_id);

    assert((index >= 0 && index < data_list.size()) &&
      "selected_data_index or mesh_id should be in bounds");
    return data_list[index];
  }

  IGL_INLINE int Viewer::append_mesh(bool visible /*= true*/)
  {
    assert(data_list.size() >= 1);

    data_list.emplace_back();
    selected_data_index = data_list.size()-1;
    data_list.back().id = next_data_id++;
    //if (visible)
    //    for (int i = 0; i < core_list.size(); i++)
    //        data_list.back().set_visible(true, core_list[i].id);
    //else
    //    data_list.back().is_visible = 0;
    return data_list.back().id;
  }

  IGL_INLINE bool Viewer::erase_mesh(const size_t index)
  {
    assert((index >= 0 && index < data_list.size()) && "index should be in bounds");
    assert(data_list.size() >= 1);
    if(data_list.size() == 1)
    {
      // Cannot remove last mesh
      return false;
    }
    data_list[index].meshgl.free();
    data_list.erase(data_list.begin() + index);
    if(selected_data_index >= index && selected_data_index > 0)
    {
      selected_data_index--;
    }

    return true;
  }

  IGL_INLINE size_t Viewer::mesh_index(const int id) const {
    for (size_t i = 0; i < data_list.size(); ++i)
    {
      if (data_list[i].id == id)
        return i;
    }
    return 0;
  }

  // ASSIGNMENT 1 - TASK 4

  IGL_INLINE bool Viewer::load_mesh_from_configuration(const std::string config , bool assignment2) {

      if (assignment2)
          return true;
          //init_obj_collision();

      else {
          std::string mesh_path;
          std::fstream infile;
          infile.open("configuration.txt");
          if (!infile) {
              std::cout << "Can't open file configuration.txt\n";
              return false;
          }
          else {
              while (getline(infile, mesh_path)) {
                  std::cout << "opening " << mesh_path << std::endl;
                  this->load_mesh_from_file(mesh_path);
                 // if (enable_simplefication) this->init_simplefication_objs();
              }
              infile.close();
              return true;
          }
      }
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////// ASSIGNMENT 2 //////////////////////////////////////////////////////////////////////

  IGL_INLINE void Viewer::collission_init(const std::string config) {

          std::string mesh_path;
          std::fstream infile;
        //  this->animating_collision = false;
          infile.open(config);
          if (!infile) {
              std::cout << "Can't open file configuration.txt\n";
              return;
          }
          int mesh = 0;

          printf("both opened \n");

          /// <summary>
          ///  in config we have 2 cubes
          /// cube 1 and cube 2
          ///
          /// the while will go over both and init trees for both object .
          ///
          ///
          ///
          /// </summary>
     /*   while (getline(infile, mesh_path)) {

              this->load_mesh_from_file(mesh_path);
              this->data().MyTranslate(Eigen::Vector3f((mesh - 0.5) * 0.5, 0, 0), false);

              igl::AABB<Eigen::MatrixXd, 3>* tree = new igl::AABB<Eigen::MatrixXd, 3>();
              tree->init(this->data().V, this->data().F);
              this->trees.push_back(tree);
              this->tree_roots.push_back(tree);
              this->last_box.push_back(&tree->m_box);

              data().original_V = new Eigen::MatrixXd(this->data().V);
              data().original_F = new Eigen::MatrixXi(this->data().F);

              this->data().show_overlay_depth = false;
              this->data().show_lines = false;
              this->data().point_size = 1;
              this->data().line_width = 1;
              Eigen::RowVector3d red_color(1, 0, 0);
              this->draw_m_box(this->selected_data_index, tree->m_box, red_color);
              mesh++;
          }*/

  }

  Eigen::Matrix4d Viewer::CalcParentsTrans(int indx)
  {
	  Eigen::Matrix4d prevTrans = Eigen::Matrix4d::Identity();

	  for (int i = indx; parents[i] >= 0; i = parents[i])
	  {
		  //std::cout << "parent matrix:\n" << scn->data_list[scn->parents[i]].MakeTrans() << std::endl;
		  prevTrans = data_list[parents[i]].MakeTransd() * prevTrans;
	  }

	  return prevTrans;
  }

} // end namespace
} // end namespace

}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_equal_with_equal_nans.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_equal_with_equal_nans integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::is_equal_with_equal_nans;
  using r_t = decltype(is_equal_with_equal_nans(T(), T()));

   // return type conformity test
  STF_TYPE_IS(r_t,bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_equal_with_equal_nans(bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_equal_with_equal_nans(bs::One<T>(),bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_equal_with_equal_nans(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
} // end of test for integer_

STF_CASE_TPL (" is_equal_with_equal_nans real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::is_equal_with_equal_nans;
  using r_t = decltype(is_equal_with_equal_nans(T(), T()));

   // return type conformity test
  STF_TYPE_IS(r_t,bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_equal_with_equal_nans(bs::Inf<T>(), bs::Inf<T>()), r_t(true));
  STF_EQUAL(is_equal_with_equal_nans(bs::Minf<T>(), bs::Minf<T>()), r_t(true));
  STF_EQUAL(is_equal_with_equal_nans(bs::Nan<T>(), bs::Nan<T>()), r_t(true));
  STF_EQUAL(is_equal_with_equal_nans(bs::One<T>(),bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_equal_with_equal_nans(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
} // end of test for floating_

STF_CASE ( "is_equal_with_equal_nans bool")
{
  namespace bs = boost::simd;
  using bs::is_equal_with_equal_nans;

  // return type conformity test
  STF_EXPR_IS(is_equal_with_equal_nans(bool(), bool()), bool);

  // specific values tests
  STF_EQUAL(is_equal_with_equal_nans(true, false), false);
  STF_EQUAL(is_equal_with_equal_nans(false, true), false);
  STF_EQUAL(is_equal_with_equal_nans(true, true), true);
  STF_EQUAL(is_equal_with_equal_nans(false, false), true);
}


//
//  electron_optimisation_algorithm.hpp
//  indigo-bondorder
//
//  Created by Welsh, Ivan on 13/09/17.
//  Copyright © 2017 Allison Group. All rights reserved.
//

#ifndef ELECTRON_OPTIMISATION_ALGORITHM_HPP
#define ELECTRON_OPTIMISATION_ALGORITHM_HPP

#include <cstdint>
#include <map>
#include <vector>

#include <boost/dynamic_bitset/dynamic_bitset.hpp>

#include "../../classes/electron_graph.hpp"
#include "../electron_optimisation.hpp"
#include "../../api.hpp"

namespace indigo_bondorder {
  namespace algorithm {

    typedef boost::dynamic_bitset<> ElnDist;

    class ElectronOptimisationAlgorithm {
    protected:
      ElectronOpt* parent_;
      // Result storage
      std::vector<std::pair<ElnDist, uint32_t>> minDistributions_;
      std::map<MolVertPair, ElnVertex> mvp2ev_;
      Score minScore_;
      Score upperLimit_;

      uint32_t eneBitmask_;   // Used for checking bond energies w/wo charge
      ElnDist previousDist_;

    private:
      ElectronOptimisationAlgorithm() = default;

    public:
      ElectronOptimisationAlgorithm(ElectronOpt* parent);
      virtual ~ElectronOptimisationAlgorithm() {}

      //virtual void Initalise();
      virtual void Run() = 0;
      bool ApplyElectronAssignment(Uint);
      inline size_t GetResultCount() { return minDistributions_.size(); }
      inline Score GetResultEnergy() { return minScore_; }
      void PopulateMVP2EV();

    protected:
      void SetElectronDistribution(ElnDist& dist);
      void DetermineFormalCharges();
      ElnDist CalculateUpperLimit();
      Score CalculateVertexEnergy(ElnVertex& v);
      Score CalculateDistributionEnergy(ElnDist);

    };

  }
}

#endif /* ELECTRON_OPTIMISATION_ALGORITHM_HPP */


#ifndef __CLOSED_FORM_MODELS_H__
#define __CLOSED_FORM_MODELS_H__

#include <armadillo>

#include <besiq/stats/log_scale.hpp>
#include <besiq/stats/snp_count.hpp>

/**
 * This class represents a general model that can compute a
 * model likelihood for two snps.
 */
class closed_form_model
{
public:
    /**
     * Constructor.
     *
     * @param df The degrees of freedom.
     */
    closed_form_model(unsigned int df)
      : m_df( df )
    {
    }

    /**
     * Returns the degrees of freedom of this model.
     *
     * @return The degrees of freedom of this model.
     */
    unsigned int df()
    {
        return m_df;
    }

    /**
     * Computes the likelihood of the snps and phenotypes under this
     * model.
     *
     * @param count The number of counts for each genotype.
     *
     * @return The likelihood of the snps.
     */
    virtual log_double prob(const arma::mat &count) = 0;

private:

    /**
     * Degrees of freedom.
     */
    unsigned int m_df;
};

#endif /* End of __CLOSED_FORM_MODELS_H__ */


/*****************************************************************************/
/*  Copyright (c) 2015, Alessandro Pieropan                                  */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include "../include/pose_estimation.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/math/constants/constants.hpp>
#include "../include/utilities.h"
#include <eigen3/Eigen/Geometry>
#include <algorithm>
#include <stdexcept>
#include <iostream>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace fato {

void getPoseRansac(const std::vector<cv::Point3f>& model_points,
                   const std::vector<cv::Point2f>& tracked_points,
                   const cv::Mat& camera_model, int iterations, float distance,
                   vector<int>& inliers, Mat& rotation, Mat& translation) {
  if (model_points.size() > 4) {
    Mat rotation_vect;
    solvePnPRansac(model_points, tracked_points, camera_model,
                   Mat::zeros(1, 8, CV_32F), rotation_vect, translation, false,
                   iterations, distance, model_points.size(), inliers, CV_P3P);

    try {
      Rodrigues(rotation_vect, rotation);
      rotation.convertTo(rotation, CV_32FC1);
    } catch (cv::Exception& e) {
      cout << "Error estimating ransac rotation: " << e.what() << endl;
    }

  } else {
    rotation = Mat(3, 3, CV_32FC1, 0.0f);
    translation = Mat(1, 3, CV_32FC1, 0.0f);
  }
}

void getMatrices(const std::vector<cv::Point2f>& prev_pts,
                 const std::vector<float>& prev_depth,
                 const std::vector<cv::Point2f>& next_pts, float nodal_x,
                 float nodal_y, float focal_x, float focal_y,
                 Eigen::MatrixXf& A, Eigen::VectorXf& b) {
  int valid_points = prev_pts.size();

  A = Eigen::MatrixXf(valid_points * 2, 6);
  b = Eigen::VectorXf(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < valid_points; ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }
}

void getPoseFromFlow(const std::vector<cv::Point2f>& prev_pts,
                     const std::vector<float>& prev_depth,
                     const std::vector<cv::Point2f>& next_pts, float nodal_x,
                     float nodal_y, float focal_x, float focal_y,
                     vector<float>& translation, vector<float>& rotation) {
  int valid_points = prev_pts.size();

  Eigen::MatrixXf A(valid_points * 2, 6);
  Eigen::VectorXf b(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < prev_pts.size(); ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);
  // solving least square to find the 6 unknown parameters: tx,ty, tz, wx, wy,
  // wz
  Eigen::VectorXf Y = (A.transpose() * A).ldlt().solve(A.transpose() * b);

  //  cout << "LQ: " << fixed << setprecision(3) << Y[0] << " " << Y[1] << " "
  //  << Y[2] << " "
  //                 << Y[3] << " " << Y[4] << " " << Y[5] << endl;

  translation[0] = Y[0];
  translation[1] = Y[1];
  translation[2] = Y[2];

  rotation[0] = Y[3];
  rotation[1] = Y[4];
  rotation[2] = Y[5];
}

Eigen::VectorXf getPoseFromFlowRobust(const std::vector<cv::Point2f>& prev_pts,
                           const std::vector<float>& prev_depth,
                           const std::vector<cv::Point2f>& next_pts,
                           float nodal_x, float nodal_y, float focal_x,
                           float focal_y, int num_iters,
                           std::vector<float>& translation,
                           std::vector<float>& rotation,
                           std::vector<int>& outliers) {
  Eigen::MatrixXf X;
  Eigen::VectorXf y;

  getMatrices(prev_pts, prev_depth, next_pts, nodal_x, nodal_y, focal_x,
              focal_y, X, y);

  // during tracking possible to initialize with the previous estimation???
  Eigen::VectorXf beta = (X.transpose() * X).ldlt().solve(X.transpose() * y);

  Eigen::VectorXf W;
  Eigen::MatrixXf XW(X.rows(), X.cols());

  for (auto i = 0; i < num_iters; ++i) {
    // cout << "iter " << i;// << endl;

    Eigen::VectorXf residuals = ((X * beta) - y);
    residuals = residuals.array().abs();

    // cout << residuals << endl;

    std::vector<float> res_vector(residuals.rows(), 0);

    for (int j = 0; j < residuals.rows(); ++j) {
      res_vector.at(j) = residuals(j);
    }

    sort(res_vector.begin(), res_vector.end());

    float residual_scale;
    if (res_vector.size() % 2 != 0)
      residual_scale = res_vector.at(res_vector.size() / 2);
    else
      residual_scale = res_vector.at(res_vector.size() / 2) +
                       res_vector.at(res_vector.size() / 2 + 1) / 2.0f;

    // avoid division by 0
    residual_scale = max(residual_scale, 0.0001f);

    residual_scale *=
        6.9460;  // constant defined in the IRLS and bisquare distance

    W = residuals / residual_scale;

    for (auto j = 0; j < W.rows(); ++j) {
      if (W(j) > 1) {
        W(j) = 0;
      } else {
        auto tmp = 1 - W(j) * W(j);
        W(j) = tmp * tmp;
      }
    }

    for (auto j = 0; j < XW.rows(); ++j) {
      for (auto k = 0; k < XW.cols(); ++k) {
        XW(j, k) = W(j) * X(j, k);
      }
    }

    beta = (XW.transpose() * X).ldlt().solve(XW.transpose() * y);
  }

  // cout << "\n here" << endl;

  int num_points = W.rows() / 2;

  for (auto j = 0; j < num_points; ++j) {
    int id = 2 * j;
    if (W(id) == 0 || W(id + 1) == 0) {
      outliers.push_back(j);
    }
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);

  translation[0] = beta[0];
  translation[1] = beta[1];
  translation[2] = beta[2];

  rotation[0] = beta[3];
  rotation[1] = beta[4];
  rotation[2] = beta[5];

  return beta;
}

void getPose2D(const std::vector<cv::Point2f*>& model_points,
               const std::vector<cv::Point2f*>& tracked_points, float& scale,
               float& angle) {
  vector<double> angles;
  vector<float> scales;

  angles.reserve(model_points.size() * 2);
  scales.reserve(model_points.size() * 2);

  const double pi = boost::math::constants::pi<double>();

  for (size_t i = 0; i < model_points.size(); ++i) {
    for (size_t j = 0; j < model_points.size(); j++) {
      // computing angle
      Point2f a = *tracked_points.at(i) - *tracked_points.at(j);
      Point2f b = *model_points.at(i) - *model_points.at(j);
      double val = atan2(a.y, a.x) - atan2(b.y, b.x);

      if (abs(val) > pi) {
        int sign = (val < 0) ? -1 : 1;
        val = val - sign * 2 * pi;
      }
      angles.push_back(val);

      if (i == j) continue;

      // computing scale
      auto tracked_dis =
          getDistance(tracked_points.at(i), tracked_points.at(j));
      auto model_dis = getDistance(model_points.at(i), model_points.at(j));
      if (model_dis != 0) scales.push_back(tracked_dis / model_dis);
    }
  }

  sort(angles.begin(), angles.end());
  sort(scales.begin(), scales.end());

  auto angles_size = angles.size();
  auto scale_size = scales.size();

  if (angles_size == 0)
    angle = 0;

  else if (angles_size % 2 == 0)
    angle = (angles[angles_size / 2 - 1] + angles[angles_size / 2]) / 2;
  else
    angle = angles[angles_size / 2];

  if (scale_size == 0)
    scale = 1;
  else if (scale_size % 2 == 0)
    scale = (scales[scale_size / 2 - 1] + scales[scale_size / 2]) / 2;
  else
    scale = scales[scale_size / 2];
}

// TODO: implement ransac approach for better SVD estimation
Mat getRigidTransform(Mat& a, Mat& b) {
  int numRows = a.rows;

  vector<float> srcM = {0, 0, 0};
  vector<float> dstM = {0, 0, 0};

  // compute centroid of the two sets of points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      srcM[j] += a.at<float>(i, j);
      dstM[j] += b.at<float>(i, j);
    }
  }

  for (int i = 0; i < 3; i++) {
    srcM[i] = srcM[i] / static_cast<float>(numRows);
    dstM[i] = dstM[i] / static_cast<float>(numRows);
  }

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  // cout << "H\n" << H << endl;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

Mat getRigidTransform(Mat& a, Mat& b, vector<float>& cA, vector<float>& cB) {
  int numRows = a.rows;

  vector<float>& srcM = cA;
  vector<float>& dstM = cB;

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

void rotateBBox(const vector<Point3f>& bBox, const Mat& rotation,
                vector<Point3f>& updatedBBox) {
  Mat a(4, 3, CV_32FC1);
  Mat b, b_T, a_T;

  for (size_t i = 0; i < 4; i++) {
    a.at<float>(i, 0) = bBox.at(i).x;
    a.at<float>(i, 1) = bBox.at(i).y;
    a.at<float>(i, 2) = bBox.at(i).z;
  }

  transpose(a, a_T);

  b_T = rotation * a_T;
  transpose(b_T, b);

  for (size_t i = 0; i < 4; i++) {
    updatedBBox.push_back(
        Point3f(b.at<float>(i, 0), b.at<float>(i, 1), b.at<float>(i, 2)));
  }
}

void rotatePoint(const Point3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point.x;
  a.at<float>(1) = point.y;
  a.at<float>(2) = point.z;
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation, Vec3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint[0] = b.at<float>(0);
  updatedPoint[1] = b.at<float>(1);
  updatedPoint[2] = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotationVecToMat(const Mat& vec, Mat& mat) {}

/**************************************************************/
/*           POSE CLASS                                       */
/**************************************************************/

Pose::Pose() { pose_ = Matrix4d(4, 4); }

Pose::Pose(Mat& r_mat, Mat& t_vect) {
  pose_ = Matrix4d(4, 4);

  if (t_vect.cols != 3 && t_vect.rows != 3) {
    cout << t_vect.cols << " " << t_vect.rows << endl;
    throw std::runtime_error(
        "Pose: bad translation vector format, 3x1 accepted");
  }

  Mat tmp_rot, tmp_t;
  r_mat.convertTo(tmp_rot, CV_64FC1);
  t_vect.convertTo(tmp_t, CV_64FC1);

  if (tmp_rot.cols == 3 && tmp_rot.rows == 3) {
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = tmp_rot.at<double>(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else if (r_mat.cols == 3 && r_mat.cols == 1) {
    Eigen::Matrix3d rot_view;
    rot_view =
        Eigen::AngleAxisd(tmp_rot.at<double>(2), Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(1), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(0), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else {
    throw std::runtime_error(
        "Pose: bad rotation matrix format, 3x3 or 1x3 accepted");
  }

  init_rot = r_mat;
  init_tr = t_vect;
}

Pose::Pose(Matrix4d& pose) {
  pose_ = Matrix4d(4, 4);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      pose_(i, j) = pose(i, j);
    }
  }
}

Pose::Pose(std::vector<double>& beta) {
  if (beta.size() != 6) {
    throw std::runtime_error("Pose: bad parameters size, should be 6");
  }

  pose_ = Matrix4d(4, 4);

  Eigen::Matrix3d rot_view;
  rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      pose_(i, j) = rot_view(i, j);
    }
    pose_(3, i) = 0;
  }
  pose_(3, 3) = 1;

  pose_(0, 3) = beta.at(0);
  pose_(1, 3) = beta.at(1);
  pose_(2, 3) = beta.at(2);

  init_beta_ = beta;
}

Pose::Pose(VectorXf &beta)
{
    if (beta.rows() != 6) {
      throw std::runtime_error("Pose: bad parameters size, should be 6");
    }

    init_beta_.resize(6,0);

    for(auto i = 0; i < 6; ++i)
        init_beta_[i] = static_cast<double>(beta[i]);

    pose_ = Matrix4d(4, 4);

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(init_beta_.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(init_beta_.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(init_beta_.at(3), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;

    pose_(0, 3) = init_beta_.at(0);
    pose_(1, 3) = init_beta_.at(1);
    pose_(2, 3) = init_beta_.at(2);
}

std::pair<cv::Mat, cv::Mat> Pose::toCV() const {
  cv::Mat rotation = cv::Mat(3, 3, CV_64FC1, 0.0f);
  cv::Mat translation = cv::Mat(1, 3, CV_64FC1, 0.0f);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      rotation.at<double>(i, j) = pose_(i, j);
    }
    translation.at<double>(i) = pose_(i, 3);
  }

  return pair<cv::Mat, cv::Mat>(rotation, translation);
}

std::pair<Eigen::Matrix3d, Eigen::Vector3d> Pose::toEigen() const
{
    Eigen::Matrix3d rotation_temp;
    Eigen::Vector3d translation_vect;
    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
          rotation_temp(i,j) = pose_(i,j);
        }
        translation_vect(i) = pose_(i,3);
    }

    return pair<Eigen::Matrix3d, Eigen::Vector3d>(rotation_temp, translation_vect);
}

glm::mat4 Pose::toGL() const
{

    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
          Rx_180(1, 1) = -1.0;
          Rx_180(2, 2) = -1.0;
    auto pose = Rx_180 * pose_;

    glm::mat4 pose_glm;

    for(auto i = 0; i < 4; ++i)
    {
        for(auto j = 0; j < 4; ++j)
        {
          pose_glm[j][i] = pose(i,j);
        }
    }

    return pose_glm;
}

void Pose::transform(Eigen::Matrix4d& transform) { pose_ = transform * pose_; }

void Pose::transform(std::vector<double> &beta)
{

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

    Eigen::Matrix4d transform;

    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
            transform(i,j) = rot_view(i,j);
        }
        transform(i,3) = beta[i];
        transform(3,i) = 0;
    }
    transform(3,3) = 1;

//    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
//    Rx_180(1, 1) = -1.0;
//    Rx_180(2, 2) = -1.0;

//    transform = Rx_180 * transform;

    pose_ = transform * pose_;
}

vector<double> Pose::getBeta() {
  Eigen::Matrix3d tmp_mat(3, 3);

  for (auto i = 0; i < 3; ++i) {
    for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = pose_(i, j);
  }

  Eigen::Vector3d angles = tmp_mat.eulerAngles(2, 1, 0);

  vector<double> tmp = {pose_(0, 3), pose_(1, 3), pose_(2, 3),
                        angles(2),   angles(1),   angles(0)};

  return tmp;
}

vector<double> Pose::translation() const
{
    return vector<double>{pose_(0, 3), pose_(1, 3), pose_(2, 3)};
}

Eigen::Quaternionf Pose::rotation() const
{
    Eigen::Matrix3f tmp_mat(3, 3);

    for (auto i = 0; i < 3; ++i) {
      for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = (float)pose_(i, j);
    }

    return Quaternionf(tmp_mat);
}

string Pose::str() const {
  stringstream ss;
  ss << fixed << setprecision(3);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      ss << pose_(i, j) << " ";
    }
    ss << "\n";
  }

  return ss.str();
}

}  // end namespace


/* Copyright 2020 Gopinath Chennupati, Raviteja Vangara, Namita Kharat, Erik Skau and Boian Alexandrov,
Triad National Security, LLC. All rights reserved
This program was produced under U.S. Government contract 89233218CNA000001 for Los Alamos National Laboratory (LANL), which is operated by Triad National Security, LLC for the U.S. Department of Energy/National Nuclear Security Administration. All rights in the program are reserved by Triad National Security, LLC, and the U.S. Department of Energy/National Nuclear Security Administration. The Government is granted for itself and others acting on its behalf a nonexclusive, paid-up, irrevocable worldwide license in this material to reproduce, prepare derivative works, distribute copies to the public, perform publicly and display publicly, and to permit others to do so.

This file is modified from parsecommandline.hpp file of Ramakrishnan Kannan's Planc library to include additional function parseNMFkOpts() for distnmfk library
*/

#ifndef COMMON_PARSECOMMANDLINE_HPP_
#define COMMON_PARSECOMMANDLINE_HPP_

#include <getopt.h>
#include <armadillo>
#include <iostream>
#include <sstream>
#include <string>
#include "parsecommand.h"

namespace planc {
class ParseCommandLine {
 private:
  int m_argc;
  char **m_argv;

  // common to all algorithms.
  algotype m_lucalgo;
  normtype m_input_normalization;
  bool m_compute_error;
  //double objective_err;
  int m_num_it;
  int m_num_k_blocks;
  bool m_dim_tree;

  // file names
  std::string m_Afile_name;
  std::string m_outputfile_name;
  // std::string m_init_file_name;
  std::string m_output_silhouettes;

  // nmf related values
  UWORD m_k;
  UWORD m_globalm;
  UWORD m_globaln;
  UWORD m_upper_k;
  UWORD m_perturbs;

  // algo related values
  FVEC m_regW;
  FVEC m_regH;
  float m_sparsity;

  // distnmf related values
  int m_pr;
  int m_pc;

  // dist ntf
  int m_num_modes;
  UVEC m_dimensions;
  UVEC m_proc_grids;
  FVEC m_regularizers;

  void parseArrayofString(const char opt, const char *input) {
    std::stringstream ss(input);
    std::string s;
    int i = 0;
    if (this->m_num_modes == 0) {
      while (getline(ss, s, ' ')) {
        i++;
      }
      this->m_num_modes = i;
      this->m_dimensions = arma::zeros<UVEC>(this->m_num_modes);
      this->m_regularizers = arma::zeros<FVEC>(2 * this->m_num_modes);
      this->m_proc_grids = arma::ones<UVEC>(this->m_num_modes);
    }
    i = 0;
    ss.clear();
    ss.str(input);
    while (getline(ss, s, ' ')) {
      switch (opt) {
        case 'd':
          this->m_dimensions(i++) = ::atoi(s.c_str());
          break;
        case 'r':
          this->m_regularizers(i++) = ::atof(s.c_str());
          break;
        case 'p':
          this->m_proc_grids(i++) = ::atoi(s.c_str());
          break;
        default:
          INFO << "wrong option::" << opt << "::values::" << input << std::endl;
      }
    }
  }

 public:
  /**
   * Constructor that takes the number of arguments and the
   * command line parameters.
   * @param[in] argc - number of arguments
   * @param[in] **argv - command line parameters.
   */

  //double objective_err;
  ParseCommandLine(int argc, char **argv) : m_argc(argc), m_argv(argv) {
    this->m_num_modes = 0;
    this->m_pr = 1;
    this->m_pc = 1;
    this->m_regW = arma::zeros<FVEC>(2);
    this->m_regH = arma::zeros<FVEC>(2);
    this->m_num_k_blocks = 1;
    this->m_k = 20;
    this->m_num_it = 20;
    this->m_lucalgo = ANLSBPP;
    this->m_compute_error = 0;
    this->m_input_normalization = NONE;
    this->m_dim_tree = 1;
   // this->objective_err = 0;
  }
  /// parses the command line parameters
  void parseplancopts() {
    int opt, long_index;
    while ((opt = getopt_long(this->m_argc, this->m_argv,
                              "a:d:e:i:k:o:p:r:s:t:", plancopts,
                              &long_index)) != -1) {
      switch (opt) {
        case 'a':
          this->m_lucalgo = static_cast<algotype>(atoi(optarg));
          break;
        case 'e':
          this->m_compute_error = atoi(optarg);
          break;
        case 'i': {
          std::string temp = std::string(optarg);
          this->m_Afile_name = temp;
          break;
        }
        case 'k':
          this->m_k = atoi(optarg);
          break;
        case 'o': {
          std::string temp = std::string(optarg);
          this->m_outputfile_name = temp;
          break;
        }
        case 'd':
        case 'r':
        case 'p':
          parseArrayofString(opt, optarg);
          break;
        case 's':
          this->m_sparsity = atof(optarg);
          break;
        case 't':
          this->m_num_it = atoi(optarg);
          break;
        case NUMKBLOCKS:
          this->m_num_k_blocks = atoi(optarg);
          break;
        case NORMALIZATION: {
          std::string temp = std::string(optarg);
          if (temp.compare("l2") == 0) {
            this->m_input_normalization = normtype::L2NORM;
          } else if (temp.compare("max") == 0) {
            this->m_input_normalization = normtype::MAXNORM;
          }
          break;
        }
        case DIMTREE:
          this->m_dim_tree = atoi(optarg);
          break;
        default:
          std::cout << "failed while processing argument:" << optarg
                    << std::endl;
          print_usage();
          exit(EXIT_FAILURE);
      }
    }
    // properly initialize the values for nmf cases now.
    if (this->m_num_modes == 2) {
      this->m_globalm = this->m_dimensions(0);
      this->m_globaln = this->m_dimensions(1);
      this->m_regW(0) = this->m_regularizers(0);
      this->m_regW(1) = this->m_regularizers(1);
      this->m_regH(0) = this->m_regularizers(2);
      this->m_regH(1) = this->m_regularizers(3);
      this->m_pr = this->m_proc_grids(0);
      this->m_pc = this->m_proc_grids(1);
    }
  }

    /// parses the command line parameters
  void parseNMFkOpts() {
    int opt, long_index;
    while ((opt = getopt_long(this->m_argc, this->m_argv,
                              "a:d:e:i:k:o:p:r:s:t:u:l:h:", nmfkopts,
                              &long_index)) != -1) {
      switch (opt) {
        case 'a':
          this->m_lucalgo = static_cast<algotype>(atoi(optarg));
          break;
        case 'e':
          this->m_compute_error = atoi(optarg);
          break;
        case 'i': {
          std::string temp = std::string(optarg);
          this->m_Afile_name = temp;
          break;
        }
        case 'k':
          this->m_k = atoi(optarg);
          break;
        case 'o': {
          std::string temp = std::string(optarg);
          this->m_outputfile_name = temp;
          break;
        }
        case 'd':
        case 'r':
        case 'p':
          parseArrayofString(opt, optarg);
          break;
        case 's':
          this->m_sparsity = atof(optarg);
          break;
        case 't':
          this->m_num_it = atoi(optarg);
          break;
        case 'u': {       // 'u' upper limit of rank 'k'
          this->m_upper_k = atoi(optarg);
          break;
        }
        case 'l': {       // 'l' number of perturbations
          this->m_perturbs = atoi(optarg);
          break;
        }
        case 'h': {       // 'h' save silhouettes
          std::string temp = std::string(optarg);
          this->m_output_silhouettes = temp;
          break;
        }
        case NUMKBLOCKS:
          this->m_num_k_blocks = atoi(optarg);
          break;
        case NORMALIZATION: {
          std::string temp = std::string(optarg);
          if (temp.compare("l2") == 0) {
            this->m_input_normalization = normtype::L2NORM;
          } else if (temp.compare("max") == 0) {
            this->m_input_normalization = normtype::MAXNORM;
          }
          break;
        }
        case DIMTREE:
          this->m_dim_tree = atoi(optarg);
          break;
        default:
          std::cout << "failed while processing argument:" << optarg
                    << std::endl;
          print_usage();
          exit(EXIT_FAILURE);
      }
    }
    // properly initialize the values for nmf cases now.
    if (this->m_num_modes == 2) {
      this->m_globalm = this->m_dimensions(0);
      this->m_globaln = this->m_dimensions(1);
      this->m_regW(0) = this->m_regularizers(0);
      this->m_regW(1) = this->m_regularizers(1);
      this->m_regH(0) = this->m_regularizers(2);
      this->m_regH(1) = this->m_regularizers(3);
      this->m_pr = this->m_proc_grids(0);
      this->m_pc = this->m_proc_grids(1);
    }
  }

  /// print the configuration received through the command line paramters

  void printConfig() {
    std::cout << "a::" << this->m_lucalgo << "::i::" << this->m_Afile_name
              << "::k::" << this->m_k << "::m::" << this->m_globalm
              << "::n::" << this->m_globaln << "::t::" << this->m_num_it
              << "::pr::" << this->m_pr << "::pc::" << this->m_pc
              << "::error::" << this->m_compute_error << "::regW::"
              << "l2::" << this->m_regW(0) << "::l1::" << this->m_regW(1)
              << "::regH::"
              << "l2::" << this->m_regH(0) << "::l1::" << this->m_regH(1)
              << "::num_k_blocks::" << m_num_k_blocks
              << "::dimensions::" << this->m_dimensions
              << "::procs::" << this->m_proc_grids
              << "::regularizers::" << this->m_regularizers
              << "::input normalization::" << this->m_input_normalization
              << "::dimtree::" << this->m_dim_tree << std::endl;
  }

  void printNMFkConfig() {
    std::cout << "a::" << this->m_lucalgo << "::i::" << this->m_Afile_name
              << "::k::" << this->m_k << "::m::" << this->m_globalm
              << "::n::" << this->m_globaln << "::t::" << this->m_num_it
              << "::pr::" << this->m_pr << "::pc::" << this->m_pc
              << "::error::" << this->m_compute_error << "::regW::"
              << "l2::" << this->m_regW(0) << "::l1::" << this->m_regW(1)
              << "::regH::"
              << "l2::" << this->m_regH(0) << "::l1::" << this->m_regH(1)
              << "::num_k_blocks::" << m_num_k_blocks
              << "::dimensions::" << this->m_dimensions
              << "::procs::" << this->m_proc_grids
              << "::regularizers::" << this->m_regularizers
              << "::upperk::" <<this->m_upper_k
              << "::perturbs::" <<this->m_perturbs
              << "::outputsilhouettes::" <<this->m_output_silhouettes
              << "::input normalization::" << this->m_input_normalization
              << "::dimtree::" << this->m_dim_tree << std::endl;
  }

  void print_usage() {
    INFO << std::endl;
    INFO << "distnmf usage:" << std::endl;
    INFO << "for short arguments like -i do not use equals sign, eg -t 10"
         << std::endl
         << "for long arguments like --pr give key=value pair, eg --pr=4"
         << std::endl
         << "algorithm codes 0-MU2D, 1-HALS2D, 2-ANLSBPP2D, 3-NAIVEANLSBPP"
         << std::endl;
    // mpirun -np 12 distnmf algotype lowrank m n numIteration pr pc
    INFO << "Usage 1: mpirun -np 6 distnmf -a 0/1/2/3 -k 50"
         << "-i rand_uniform/rand_normal/rand_lowrank --dimtree 1"
         << "-d \"21600 14400\" -t 10 -p \"3 2\" "
         << "--normalization \"l2\" "
         << "-r \"0.0001 0 0 0.0001\" " << std::endl;
    // mpirun -np 12 distnmf algotype lowrank AfileName numIteration pr pc
    INFO << "Usage 2: mpirun -np 6 distnmf -a 0/1/2/3 -k 50 --dimtree 1"
         << "-i Ainput -t 10 -p \"3 2\" "
         << "--normalization \"max\" "
         << "-r \"0.0001 0 0 0.0001\" " << std::endl;
    // mpirun -np 12 distnmf algotype lowrank Afile nmfoutput numIteration pr pc
    INFO << "Usage 3: mpirun -np 6 distnmf -a 0/1/2/3 -k 50 --dimtree 1"
         << "-i Ainput -o nmfoutput -t 10 -p \"3 2\" "
         << "-r \"0.0001 0 0 0.0001\" " << std::endl;
    // mpirun -np 12 distnmf algotype lowrank Afile nmfoutput numIteration pr pc
    // s
    INFO << "Usage 4: mpirun -np 6 distnmf -a 0/1/2/3 -k 50 --dimtree 1"
         << "-i Ainput -o nmfoutput -t 10 -p \"3 2\" --sparsity=0.3"
         << "-r \"0.0001 0 0 0.0001\" " << std::endl;
  }
  /// returns the low rank. Passed as parameter --lowrank or -k
  UWORD lowrankk() { return m_k; }
  /// return global rows. Passed as parameter -d
  UWORD globalm() { return m_globalm; }
  //// returns the global columns. Passed as parameter -d
  UWORD globaln() { return m_globaln; }
  /**
   * L2 regularization as the first parameter and L1 as second
   * for left lowrank factor W. Passed as parameter --regularizer
   * with pair of values in double quotes for W and H "l2W l1W l2H l1H"
   */
  FVEC regW() { return m_regW; }
  /**
   * L2 regularization as the first parameter and L1 as second
   * for right lowrank factor H. Passed as parameter --regularizer
   * with pair of values in double quotes for W and H "l2W l1W l2H l1H"
   */
  FVEC regH() { return m_regH; }
  /// Returns the NMF algorithm to run. Passed as parameter --algo or -a
  algotype lucalgo() { return m_lucalgo; }
  /**
   *  Returns the process grid configuration.
   * Passed as parameter --processors or -p
   */

  UVEC processor_grids() { return m_proc_grids; }
  /**
   * Returns the vector regularizers for all the modes.
   * It will 2 times the mode values. The first entry is
   * L2 regularization and second value is L1 for every mode.
   * Passed as parameter --regularizers "L2 L1" for every mode.
   */
  FVEC regularizers() { return m_regularizers; }
  /**
   *  Returns vector of dimensions for every mode.
   * Passed as parameter -d or --dimensions
   */
  UVEC dimensions() { return m_dimensions; }
  int num_k_blocks() { return m_num_k_blocks; }
  /// Returns number of iterations. passed as -t or --iter
  int iterations() { return m_num_it; }
  /// Input parameter for generating sparse matrix. Passed as -s or --sparsity
  float sparsity() { return m_sparsity; }
  /// Returns input file name. Passed as -i or --input
  std::string input_file_name() { return m_Afile_name; }
  /**
   * Returns output file name. Passed as -o or --output.
   * Every mode will appended as _mode.
   */
  std::string output_file_name() { return m_outputfile_name; }
  /**
   * Returns the number of processor rows.
   * Used for distributed NMF. The first parameter of -p.
   */
  int pr() { return m_pr; }
    /**
   * Returns the number of processor columns.
   * Used for distributed NMF. The second parameter of -p.
   */
  int pc() { return m_pc; }
  /**
   * Returns the upper limit of lower rank k.
   * Used for distributed NMFk. The parameter is -u.
   */
  int upper_limit_k() { return m_upper_k; }
    /**
   * Returns the number of perturbations of input matrix A.
   * Used for distributed NMFk. The parameter is -l.
   */
  int perturbs() { return m_perturbs; }
  /**
   * Returns output file name for silhouettes. Passed as -h or --silhouettesoutput.
   */
  std::string output_silhouettes() { return m_output_silhouettes; }
  /// Returns number of modes in tensors. For matrix it is two.
  int num_modes() { return m_num_modes; }
  /**
   * Enable dimension tree or not. By default we use dimension trees
   * for more than three modes. Passed as parameter --dimtree 1
   */
  bool dim_tree() { return m_dim_tree; }
  /// Returns whether to compute error not. Passed as parameter -e or --error
  bool compute_error() { return m_compute_error; }
  //double objective_error() { return objective_err; }
  /// To column normalize the input matrix.
  normtype input_normalization() { return this->m_input_normalization; }
};  // ParseCommandLine
}  // namespace planc

#endif  // COMMON_PARSECOMMANDLINE_HPP_


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_IMAG_INCLUDE
#define MTL_IMAG_INCLUDE

#include <complex>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>

namespace mtl {

namespace sfunctor {

    template <typename Value>
    struct imag
    {
	typedef Value result_type;

	static inline result_type apply(const Value& v)
	{
	    using math::zero;
	    return zero(v);
	}
	result_type operator()(const Value& v) const
	{
	    using math::zero;
	    return zero(v);
	}
    };

    template <typename Value>
    struct imag<std::complex<Value> >
    {
	typedef Value result_type;

	static inline result_type apply(const std::complex<Value>& v)
	{
	    return std::imag(v);
	}
	result_type operator()(const std::complex<Value>& v) const
	{
	    return std::imag(v);
	}
    };

}

/// Imaginary part of scalars (including non-complex)
template <typename Value>
typename mtl::traits::enable_if_scalar<Value, typename sfunctor::imag<Value>::result_type>::type
inline imag(const Value& v)
{
    return sfunctor::imag<Value>::apply(v);
}

namespace vec {

    /// Imaginary part of an vector
    template <typename Vector>
    typename mtl::traits::enable_if_vector<Vector, imag_view<Vector> >::type
    inline imag(const Vector& v)
    {
	return imag_view<Vector>(v);
    }
}

namespace mat {

    /// Imaginary part of an vector
    template <typename Matrix>
    typename mtl::traits::enable_if_matrix<Matrix, imag_view<Matrix> >::type
    inline imag(const Matrix& v)
    {
	return imag_view<Matrix>(v);
    }
}

} // namespace mtl

#endif // MTL_IMAG_INCLUDE