// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/ros2_pcl_utils/blob/master/LICENSE

#include "pcl_utils/pcl_image_overlay_node.hpp"

#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/image_encodings.hpp>

#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "pcl_utils/camera.hpp"
#include "pcl_utils/pcl_iterator.hpp"

namespace cbr
{

struct PclImageOverlayNode::Impl
{
  std::atomic<bool> calib_received{false};
  sensor_msgs::msg::CameraInfo cam{};

  std::atomic<bool> frame_received{false};
  std::string img_frame{};

  std::mutex points_accum_mtx;
  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>
  points_accum{};

  std::size_t k_max_size{};
  float k_cmap_min{}, k_cmap_range{};
  int k_draw_radius{};
};

PclImageOverlayNode::PclImageOverlayNode(const rclcpp::NodeOptions & opts)
: Node("image_overlay", opts), pImpl(std::make_unique<Impl>())
{
  declare_parameter<int>("max_size", 10000);
  pImpl->k_max_size = get_parameter("max_size").as_int();

  declare_parameter<double>("cmap_min", 2.);
  pImpl->k_cmap_min = get_parameter("cmap_min").as_double();
  declare_parameter<double>("cmap_max", 20.);
  pImpl->k_cmap_range = get_parameter("cmap_max").as_double() - pImpl->k_cmap_min;
  declare_parameter<int>("draw_radius", 2);
  pImpl->k_draw_radius = get_parameter("draw_radius").as_int();

  sub_img_ = create_subscription<sensor_msgs::msg::Image>(
    "image", rclcpp::SystemDefaultsQoS(),
    std::bind(&PclImageOverlayNode::cb_img_, this, std::placeholders::_1));

  sub_calib_ = create_subscription<sensor_msgs::msg::CameraInfo>(
    "camera_info", rclcpp::SystemDefaultsQoS(),
    [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
      if (!pImpl->calib_received) {
        pImpl->cam = *msg;
        pImpl->calib_received = true;
        RCLCPP_INFO(get_logger(), "Received camera info");
      }
    });

  sub_pcl_ = create_subscription<sensor_msgs::msg::PointCloud2>(
    "pointcloud", rclcpp::SensorDataQoS(),
    std::bind(&PclImageOverlayNode::cb_pcl_, this, std::placeholders::_1));

  pub_overlay_ = create_publisher<sensor_msgs::msg::Image>(
    "image_overlay", rclcpp::SystemDefaultsQoS());

  tf2_buf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buf_);

  RCLCPP_INFO(get_logger(), "Started node");
}

PclImageOverlayNode::~PclImageOverlayNode()
{
  RCLCPP_INFO(get_logger(), "Closing node");
}

void PclImageOverlayNode::cb_img_(sensor_msgs::msg::Image::UniquePtr msg)
{
  if (!pImpl->frame_received) {
    pImpl->img_frame = msg->header.frame_id;
    pImpl->frame_received = true;
    RCLCPP_INFO(get_logger(), "Got image frame %s", pImpl->img_frame.c_str());
  }

  if (!pImpl->calib_received) {
    return;
  }

  {
    if (msg->encoding != sensor_msgs::image_encodings::RGB8) {
      RCLCPP_WARN(get_logger(), "Only RGB8 supported");
    }

    // draw points on image
    cv::Mat img(msg->height, msg->width, CV_8UC3, msg->data.data());

    // draw points on image
    std::lock_guard lock(pImpl->points_accum_mtx);
    for (const auto & pt_CAM : pImpl->points_accum) {
      // project to pixel coordinaimagetes
      Eigen::Vector2f pt_PX = cameraProject(pt_CAM, pImpl->cam);
      const auto cl = std::clamp<float>(
        static_cast<float>((pt_CAM.z() - pImpl->k_cmap_min) /
        pImpl->k_cmap_range),
        0., 1.);
      cv::circle(
        img, cv::Point2f(pt_PX.x(), pt_PX.y()),
        pImpl->k_draw_radius, cv::Scalar(255 * cl, 0, 255 * (1 - cl))
      );
    }

    pImpl->points_accum.clear();
  }

  // publish image
  pub_overlay_->publish(std::move(msg));
}

void PclImageOverlayNode::cb_pcl_(sensor_msgs::msg::PointCloud2::UniquePtr msg)
{
  if (!pImpl->frame_received) {
    return;
  }

  if (!tf2_buf_->canTransform(pImpl->img_frame, msg->header.frame_id, rclcpp::Time(0))) {
    return;
  }

  // transform cloud to sensor frame
  auto tf = tf2_buf_->lookupTransform(
    pImpl->img_frame, msg->header.frame_id, rclcpp::Time(0)
    ).transform;

  Eigen::Quaternionf q_CAM_LIDAR(tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z);
  Eigen::Vector3f t_CAM_LIDAR(tf.translation.x, tf.translation.y, tf.translation.z);

  PclIterator it(*msg);

  std::lock_guard lock(pImpl->points_accum_mtx);

  if (pImpl->points_accum.size() > pImpl->k_max_size) {
    RCLCPP_WARN_THROTTLE(
      get_logger(), *get_clock(), 5000,
      "Accumulated %lu points without getting in image, resetting...",
      pImpl->points_accum.size());
    pImpl->points_accum.clear();
  }

  for (auto i = 0u; i != msg->width; ++i, ++it) {
    if (it.intensity() >= 0) {
      pImpl->points_accum.push_back(q_CAM_LIDAR * *it + t_CAM_LIDAR);
    }
  }
}

}  // namespace cbr

RCLCPP_COMPONENTS_REGISTER_NODE(cbr::PclImageOverlayNode)


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#ifndef TCHECKER_ALGORITHMS_COVREACH_ACCEPTING_HH
#define TCHECKER_ALGORITHMS_COVREACH_ACCEPTING_HH

#include <string>

#include <boost/dynamic_bitset.hpp>

#include "tchecker/basictypes.hh"
#include "tchecker/fsm/details/system.hh"

/*!
 \file accepting.hh
 \brief Accepting conditions for covering reachability algorithm
 */

namespace tchecker {

  namespace covreach {

    /*!
     \class accepting_labels_t
     \brief Accepting conditon based on labels
     \tparam NODE_PTR : type of pointer to node
     */
    template <class NODE_PTR>
    class accepting_labels_t {
    public:
      /*!
       \brief Constructor
       \param index : maps label names to label identifiers
       \param accepting_labels : range of label names, should dereference to std::string
       */
      template <class LABELS_RANGE>
      accepting_labels_t(tchecker::label_index_t const & index, LABELS_RANGE const & accepting_labels)
      : _accepting_labels(index.size()), _node_labels(index.size())
      {
        for (std::string const & label : accepting_labels)
          _accepting_labels[index.key(label)] = 1;
      }

      /*!
       \brief Copy constructor
       */
      accepting_labels_t(tchecker::covreach::accepting_labels_t<NODE_PTR> const &) = default;

      /*!
       \brief Move constructor
       */
      accepting_labels_t(tchecker::covreach::accepting_labels_t<NODE_PTR> &&) = default;

      /*!
       \brief Destructor
       */
      ~accepting_labels_t() = default;

      /*!
       \brief Assignment operator
       */
      tchecker::covreach::accepting_labels_t<NODE_PTR> &
      operator= (tchecker::covreach::accepting_labels_t<NODE_PTR> const &) = default;

      /*!
       \brief Move-assignment operator
       */
      tchecker::covreach::accepting_labels_t<NODE_PTR> &
      operator= (tchecker::covreach::accepting_labels_t<NODE_PTR> &&) = default;

      /*!
       \brief Predicate
       \param node : a node
       \return true if node is accepting w.r.t. accepting labels, false otherwise
       */
      bool operator() (NODE_PTR const & node)
      {
        _node_labels.reset();
        for (auto const * loc : node->vloc())
          for (tchecker::label_id_t id : loc->labels())
            _node_labels[id] = 1;
        return _accepting_labels.is_subset_of(_node_labels);
      }
    private:
      boost::dynamic_bitset<> _accepting_labels;  /*!< Set of accepting labels */
      boost::dynamic_bitset<> _node_labels;       /*!< Set of node labels */
    };

  } // end of namespace covreach

} // end of namespace tchecker

#endif // TCHECKER_ALGORITHMS_COVREACH_ACCEPTING_HH


/*
 * odeint_rk4_array
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/timer.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "lorenz.hpp"

typedef boost::timer timer_type;

typedef boost::array< double , 3 > state_type;

using namespace boost::numeric::odeint;

//typedef boost::numeric::odeint::runge_kutta4_classic< state_type > rk4_odeint_type;

// use the never resizer explicitely for optimal performance with gcc,
// for the intel compiler this doesnt matter and the above definition
// gives the same performance
typedef runge_kutta4_classic< state_type , double , state_type , double ,
                              array_algebra, default_operations, never_resizer > rk4_odeint_type;

const int loops = 21;
const int num_of_steps = 20000000;
const double dt = 1E-10;

int main()
{
    double min_time = 1E6; // something big
    rk4_odeint_type stepper;
    std::clog.precision(16);
    std::cout.precision(16);
    for( int n=0; n<loops; n++ )
    {
        state_type x = {{ 8.5, 3.1, 1.2 }};
        double t = 0.0;
        timer_type timer;
        for( size_t i = 0 ; i < num_of_steps ; ++i )
        {
            stepper.do_step( lorenz(), x, t, dt );
            t += dt;
        }
        min_time = std::min( timer.elapsed() , min_time );
        std::clog << timer.elapsed() << '\t' << x[0] << std::endl;
    }
    std::cout << "Minimal Runtime: " << min_time << std::endl;
}


/*
Copyright (c) 2014, Florian Sittel (www.lettis.net)
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.

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 OWNER 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 "covariance.hpp"
#include "file_io.hpp"
#include "matrix.hpp"
#include "util.hpp"

#include <string>
#include <omp.h>
#include <boost/program_options.hpp>

namespace b_po = boost::program_options;

int main(int argc, char* argv[]) {
  bool verbose = false;
  bool periodic = false;
  bool dynamic_shift = false;
  bool use_correlation = false;

  b_po::options_description desc (std::string(argv[0]).append(
    "\n\n"
    "Calculate principle components from large data files.\n"
    "Input data should be given as textfiles\n"
    "with whitespace separated columns or,"
    " alternatively as GROMACS .xtc-files.\n"
    "\n"
    "options"));

  desc.add_options()
    ("help,h", "show this help")
    // input
    ("file,f", b_po::value<std::string>(),
        "input (required): either whitespace-separated ASCII"
        " or GROMACS xtc-file.")
    ("cov-in,C", b_po::value<std::string>()->default_value(""),
        "input (optional): file with already calculated covariance matrix")
    ("vec-in,V", b_po::value<std::string>()->default_value(""),
        "input (optional): file with already computed eigenvectors")
    ("stats-in,S", b_po::value<std::string>()->default_value(""),
        "input (optional): mean values, sigmas and boundary shifts"
        " (shifts only for periodic)."
        " Provide this, if you want to project new data onto a previously"
        " computed principal space."
        " If you do not define the stats of the previous run, means,"
        " sigmas and shifts"
        " will be re-computed and the resulting projections"
        " will not be comparable to the previous ones.")
    // output
    ("proj,p", b_po::value<std::string>()->default_value(""),
        "output (optional): file for projected data")
    ("cov,c", b_po::value<std::string>()->default_value(""),
        "output (optional): file for covariance matrix")
    ("vec,v", b_po::value<std::string>()->default_value(""),
        "output (optional): file for eigenvectors")
    ("stats,s", b_po::value<std::string>()->default_value(""),
        "output (optional): mean values, sigmas and boundary shifts"
        " (shifts only for periodic)")
    ("val,l", b_po::value<std::string>()->default_value(""),
        "output (optional): file for eigenvalues")
    // parameters
    ("norm,N", b_po::value(&use_correlation)->zero_tokens(),
        "if set, use correlation instead of covariance by"
        " normalizing input (default: false)")
    ("buf,b", b_po::value<std::size_t>()->default_value(2),
        "max. allocatable RAM [Gb] (default: 2)")
    ("periodic,P", b_po::value(&periodic)->zero_tokens(),
        "compute covariance and PCA on a torus"
        " (i.e. for periodic data like dihedral angles)")
    ("dynamic-shift,D", b_po::value(&dynamic_shift)->zero_tokens(),
        "use dynamic shifting for periodic projection correction."
        " (default: fale, i.e. simply shift to region of lowest density)")
    ("verbose", b_po::value(&verbose)->zero_tokens(),
        "verbose mode (default: not set)")
    ("nthreads,n", b_po::value<std::size_t>()->default_value(0),
        "number of OpenMP threads to use."
        " if set to zero, will use value of OMP_NUM_THREADS (default: 0)");

  b_po::variables_map args;
  try {
    b_po::store(b_po::parse_command_line(argc, argv, desc), args);
  } catch (b_po::error e) {
    std::cerr << e.what() << std::endl;
  }
  b_po::notify(args);

  if (args.count("help")) {
    std::cerr << desc << std::endl;
    return 1;
  }

  ////

  try {
    if (args.count("file")) {
      auto check_file_given = [&args](std::string opt) {
        return (args[opt].as<std::string>().compare("") != 0);
      };
      bool projection_file_given = check_file_given("proj");
      bool eigenvec_file_given = check_file_given("vec");
      bool eigenval_file_given = check_file_given("val");
      bool covmat_file_given = check_file_given("cov");
      bool stats_file_given = check_file_given("stats");
      bool input_covmat_file_given = check_file_given("cov-in");
      bool input_eigenvec_file_given = check_file_given("vec-in");
      bool input_stats_file_given = check_file_given("stats-in");

      bool need_covmat = projection_file_given
                      || eigenval_file_given
                      || eigenvec_file_given
                      || covmat_file_given;
      if ( need_covmat
        || stats_file_given) {

        std::size_t nthreads = args["nthreads"].as<std::size_t>();
        if (nthreads > 0) {
          // if not set explicitly (i.e. nthreads == 0), num_threads will
          // be initialized to value of environment variable OMP_NUM_THREADS
          omp_set_num_threads(nthreads);
        }
        std::string file_input = args["file"].as<std::string>();
        std::size_t mem_buf_size = FastPCA::gigabytes_to_bytes(
                                      args["buf"].as<std::size_t>());
        FastPCA::SymmetricMatrix<double> cov;
        FastPCA::Matrix<double> vecs;
        FastPCA::Matrix<double> stats;
        //// input
        if (input_stats_file_given) {
          // -S ?
          verbose
            && std::cerr << "loading stats (shifts, means, sigmas) from file"
                         << std::endl;
          FastPCA::DataFileReader<double>
            fh_shifts(args["stats-in"].as<std::string>());
          stats = fh_shifts.next_block();
        } else {
          verbose
            && std::cerr << "computing stats (means, sigmas, ...)"
                         << std::endl;
          if (periodic) {
            verbose
              && dynamic_shift
              && std::cerr << "using dynamic shifting";
            verbose
              && ( ! dynamic_shift)
              && std::cerr << "using static shifting";
            verbose && std::cerr <<" for periodic barrier correction"
                                 << std::endl;
            stats = FastPCA::Periodic::stats(file_input
                                           , mem_buf_size
                                           , dynamic_shift);
          } else {
            stats = FastPCA::stats(file_input
                                 , mem_buf_size);
          }
        }
        if (input_eigenvec_file_given && input_covmat_file_given) {
          // -V and -C ?
          std::cerr << "error: Providing the covariance matrix and"
                       " the eigenvectors does not make sense."
                    << std::endl
                    << "       If we have the vectors, we do not need"
                       " the covariance matrix,"
                    << std::endl
                    << "       therefore the options '-C' and '-V'"
                       " are mutually exclusive."
                    << std::endl;
          return EXIT_FAILURE;
        } else if (input_eigenvec_file_given) {
          // -V ?
          verbose && std::cerr << "loading eigenvectors from file"
                               << std::endl;
          FastPCA::DataFileReader<double>
            fh_vecs(args["vec-in"].as<std::string>());
          vecs = fh_vecs.next_block();
          if (projection_file_given && (! input_stats_file_given)) {
            std::cerr << "warning: You specified a projection output and gave"
                      << std::endl
                      << "         pre-computed eigenvectors."
                      << std::endl
                      << "         Projection is, however,"
                         " centered to the mean."
                      << std::endl
                      << "         Without stats (means, sigma,"
                         " [dihedral shifts]),"
                      << std::endl
                      << "         you will in general get"
                         " non-comparable results"
                      << std::endl
                      << "         to previous projections!"
                      << std::endl;
          }
        } else {
          if (need_covmat) {
            if (input_covmat_file_given) {
              // -C ?
              verbose
                && std::cerr << "loading covariance matrix from file"
                             << std::endl;
              FastPCA::DataFileReader<double>
                cov_in(args["cov-in"].as<std::string>());
              cov = FastPCA::SymmetricMatrix<double>(
                      cov_in.next_block(cov_in.n_cols()));
            } else {
              if (periodic) {
                verbose
                  && ( ! use_correlation)
                  && std::cerr << "constructing covariance matrix"
                                  " for periodic data"
                               << std::endl;
                verbose
                  && use_correlation
                  && std::cerr << "constructing correlation matrix"
                                  " for periodic data"
                               << std::endl;
                cov = FastPCA::Periodic::covariance_matrix(file_input
                                                         , mem_buf_size
                                                         , use_correlation
                                                         , stats);
              } else {
                verbose
                  && ( ! use_correlation)
                  && std::cerr << "constructing covariance matrix"
                               << std::endl;
                verbose
                  && use_correlation
                  && std::cerr << "constructing correlation matrix"
                               << std::endl;
                cov = FastPCA::covariance_matrix(file_input
                                               , mem_buf_size
                                               , use_correlation
                                               , stats);
              }
            }
            vecs = cov.eigenvectors();
          }
        }
        //// output
        // -c ?
        if (covmat_file_given) {
          verbose
            && ( ! use_correlation)
            && std::cerr << "writing covariance matrix"
                         << std::endl;
          verbose
            && use_correlation
            && std::cerr << "writing correlation matrix"
                         << std::endl;
          std::string covmat_file = args["cov"].as<std::string>();
          FastPCA::DataFileWriter<double>(covmat_file)
            .write(FastPCA::Matrix<double>(cov));
        }
        // -v ?
        if (eigenvec_file_given) {
          verbose
            && std::cerr << "solving eigensystem/writing eigenvectors matrix"
                         << std::endl;
          std::string eigenvec_file = args["vec"].as<std::string>();
          FastPCA::DataFileWriter<double>(eigenvec_file)
            .write(cov.eigenvectors());
        }
        // -l ?
        if (eigenval_file_given) {
          verbose
            && std::cerr << "solving eigensystem/writing eigenvalues matrix"
                         << std::endl;
          std::string eigenval_file = args["val"].as<std::string>();
          FastPCA::DataFileWriter<double>(eigenval_file)
            .write(cov.eigenvalues());
        }
        // -p ?
        if (projection_file_given) {
          std::string projection_file = args["proj"].as<std::string>();
          if (periodic) {
            verbose
              && std::cerr << "computing projections for periodic data"
                           << std::endl;
            FastPCA::Periodic::calculate_projections(file_input
                                                   , projection_file
                                                   , vecs
                                                   , mem_buf_size
                                                   , use_correlation
                                                   , stats);
          } else {
            verbose
              && std::cerr << "computing projections"
                           << std::endl;
            FastPCA::calculate_projections(file_input
                                         , projection_file
                                         , vecs
                                         , mem_buf_size
                                         , use_correlation
                                         , stats);
          }
        }
        // -s ?
        if (stats_file_given) {
          verbose
            && std::cerr << "writing stats"
                         << std::endl;
          FastPCA::DataFileWriter<double>(args["stats"].as<std::string>())
            .write(stats);
        }
      } else {
        std::cerr << "please specify at least one output file!"
                  << std::endl
                  << desc
                  << std::endl;
        return EXIT_FAILURE;
      }
    } else {
      std::cerr << desc << std::endl;
      return EXIT_FAILURE;
    }
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    std::cerr << desc << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


#include "qangle.hpp"
#include "vector.hpp"

#include "core.hpp"

//#include <boost/algorithm/clamp.hpp>

QAngle::QAngle( float x, float y, float z )
	: m_x( x )
	, m_y( y )
	, m_z( z )
{ }

QAngle::QAngle( const QAngle& angles )
	: m_x( angles[ 0 ] )
	, m_y( angles[ 1 ] )
	, m_z( angles[ 2 ] )
{ }

QAngle::QAngle( const float* angles )
	: m_x( angles[ 0 ] )
	, m_y( angles[ 1 ] )
	, m_z( angles[ 2 ] )
{ }

auto QAngle::Init( float x /* = 0.f */, float y /* = 0.f */, float z /* = 0.f */ ) -> void
{
	m_x = x;
	m_y = y;
	m_z = z;
}

auto QAngle::ToArray() -> float*
{
	return reinterpret_cast< float* >( this );
}

auto QAngle::ToArray() const -> const float*
{
	return reinterpret_cast< const float* >( this );
}

auto QAngle::At( unsigned int index ) -> float&
{
	auto data = ToArray();
	return data[ index ];
}

auto QAngle::At( unsigned int index ) const -> const float
{
	auto data = ToArray();
	return data[ index ];
}

auto QAngle::IsEqual( const QAngle& angles ) const -> bool
{
	return ( m_x == angles[ 0 ] &&
			 m_y == angles[ 1 ] &&
			 m_z == angles[ 2 ] );
}

auto QAngle::Negate() -> void
{
	m_x = -m_x;
	m_y = -m_y;
	m_z = -m_z;
}

auto QAngle::Dot( const QAngle& angles ) const -> float
{
	return ( m_x * angles[ 0 ] +
			 m_y * angles[ 1 ] +
			 m_z * angles[ 2 ] );
}

auto QAngle::LengthSquared() const -> float
{
	return Dot( *this );
}

auto QAngle::Length() const -> float
{
	return std::sqrt( LengthSquared() );
}

auto QAngle::Add( const QAngle& angles ) -> void
{
	m_x += angles[ 0 ];
	m_y += angles[ 1 ];
	m_z += angles[ 2 ];
}

auto QAngle::Subtract( const QAngle& angles ) -> void
{
	m_x -= angles[ 0 ];
	m_y -= angles[ 1 ];
	m_z -= angles[ 2 ];
}

auto QAngle::Multiply( const QAngle& angles ) -> void
{
	m_x *= angles[ 0 ];
	m_y *= angles[ 1 ];
	m_z *= angles[ 2 ];
}

auto QAngle::Divide( const QAngle& angles ) -> void
{
	m_x /= angles[ 0 ];
	m_y /= angles[ 1 ];
	m_z /= angles[ 2 ];
}

auto QAngle::Add( float value ) -> void
{
	m_x += value;
	m_y += value;
	m_z += value;
}

auto QAngle::Subtract( float value ) -> void
{
	m_x -= value;
	m_y -= value;
	m_z -= value;
}

auto QAngle::Multiply( float value ) -> void
{
	m_x *= value;
	m_y *= value;
	m_z *= value;
}

auto QAngle::Divide( float value ) -> void
{
	m_x /= value;
	m_y /= value;
	m_z /= value;
}

auto QAngle::DistanceSquared( const QAngle& angles ) const -> float
{
	auto data = *this;
	data.Subtract( angles );
	return data.LengthSquared();
}

auto QAngle::Distance( const QAngle& angles ) const -> float
{
	auto data = *this;
	data.Subtract( angles );
	return data.Length();
}
// up / down
#define	PITCH	0
// left / right
#define	YAW		1
// tangage
#define	ROLL	2
auto QAngle::Normalize() -> void
{
	/*m_y = AngleNormalize(m_y);
	auto AngleNormalize90 = [](float angle) -> float
	{
		angle = std::fmod(angle, 180.f);

		if (angle > 90.f)
			angle -= 180.f;
		else if (angle < -90.f)
			angle += 180.f;

		return angle;
	};
	m_x = AngleNormalize90(m_x);
	m_x = std::clamp(m_x, -89.f, 89.f);
	m_z = 0.f;*/

	while (m_x > 89.0f)
		m_x -= 180.f;

	while (m_x < -89.0f)
		m_x += 180.f;

	while (m_y > 180.f)
		m_y -= 360.f;

	while (m_y < -180.f)
		m_y += 360.f;

	m_z = 0.f;

	/*int RemainderYaw = (int)m_y / 360;
	float ExcessYaw = RemainderYaw * 360.f;
	float Yaw = m_y - ExcessYaw;
	if (Yaw > 180.f) {
		Yaw -= 360.f;
	}
	if (Yaw < -180.f) {
		Yaw += 360.f;
	}

	int RemainderP = (int)m_x / 360;
	float ExcessP = RemainderP * 360.f;
	float Pitch = m_x - ExcessP;

	if (Pitch > 180.f) {
		Pitch -= 360.f;
	}
	else if (Pitch < -180.f) {
		Pitch += 360.f;
	}
	if (Pitch > 90.f) {
		Pitch -= 180.f;
	}
	else if (Pitch < -90.f) {
		Pitch += 180.f;
	}
	if (Pitch > 89.f) {
		Pitch = 89.f;
	}
	else if (Pitch < -89.f) {
		Pitch = -89.f;
	}
	m_y = Yaw;
	m_x = Pitch;
	m_z = 0.f;

	return;*/
}

auto QAngle::Clamp() -> void
{
	m_x = std::clamp( m_x, -89.f, 89.f );
	m_y = std::clamp( m_y, -180.f, 180.f );
	m_z = 0.f;
}

auto QAngle::ToVector(Vector* forward, Vector* side , Vector* up ) const -> Vector
{
	auto rad_pitch = ToRadians( m_x );
	auto rad_yaw = ToRadians( m_y );
	auto rad_roll = ToRadians( m_z );

	auto sin_pitch = std::sin( rad_pitch );
	auto sin_yaw = std::sin( rad_yaw );
	auto sin_roll = std::sin( rad_roll );

	auto cos_pitch = std::cos( rad_pitch );
	auto cos_yaw = std::cos( rad_yaw );
	auto cos_roll = std::cos( rad_roll );
	if ( forward )
	{
		 forward->At(0) = cos_pitch * cos_yaw;
		 forward->At(1) = cos_pitch * sin_yaw;
		 forward->At(2) = -sin_pitch;
	}

	if( side )
	{
		side->At( 0 ) = -1.0f * sin_roll * sin_pitch * cos_yaw + -1.0f * cos_roll * -sin_yaw;
		side->At( 1 ) = -1.0f * sin_roll * sin_pitch * sin_yaw + -1.0f * cos_roll * cos_yaw;
		side->At( 2 ) = -1.0f * sin_roll * cos_pitch;
	}

	if( up )
	{
		up->At( 0 ) = cos_roll * sin_pitch * cos_yaw + -sin_roll * -sin_yaw;
		up->At( 1 ) = cos_roll * sin_pitch * sin_yaw + -sin_roll * cos_yaw;
		up->At( 2 ) = cos_roll * cos_pitch;
	}

	return { cos_pitch * cos_yaw, cos_pitch * sin_yaw, -sin_pitch };
}

auto QAngle::ToVectorTranspose( Vector* side /*= nullptr*/, Vector* up /*= nullptr*/ ) const -> Vector
{
	auto rad_pitch = ToRadians( m_x );
	auto rad_yaw = ToRadians( m_y );
	auto rad_roll = ToRadians( m_z );

	auto sin_pitch = std::sin( rad_pitch );
	auto sin_yaw = std::sin( rad_yaw );
	auto sin_roll = std::sin( rad_roll );

	auto cos_pitch = std::cos( rad_pitch );
	auto cos_yaw = std::cos( rad_yaw );
	auto cos_roll = std::cos( rad_roll );

	if( side )
	{
		side->At( 0 ) = cos_pitch * sin_yaw;
		side->At( 1 ) = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
		side->At( 2 ) = cos_roll * sin_pitch * sin_yaw + -sin_roll * cos_yaw;
	}

	if( up )
	{
		up->At( 0 ) = -sin_pitch;
		up->At( 1 ) = sin_roll * cos_pitch;
		up->At( 2 ) = cos_roll * cos_pitch;
	}

	return { cos_pitch * cos_yaw,
			 sin_roll * sin_pitch * cos_yaw + cos_roll * -sin_yaw,
			 cos_roll * sin_pitch * cos_yaw + -sin_roll * -sin_yaw };
}

auto QAngle::operator = ( const QAngle& angles ) -> QAngle&
{
	Init( angles[ 0 ], angles[ 1 ], angles[ 2 ] );
	return *this;
}

auto QAngle::operator [] ( unsigned int index ) -> float&
{
	return At( index );
}

auto QAngle::operator [] ( unsigned int index ) const -> const float
{
	return At( index );
}

auto QAngle::operator == ( const QAngle& angles ) const -> bool
{
	return IsEqual( angles );
}

auto QAngle::operator != ( const QAngle& angles ) const -> bool
{
	return !IsEqual( angles );
}

auto QAngle::operator += ( const QAngle& angles ) -> QAngle&
{
	Add( angles );
	return *this;
}

auto QAngle::operator -= ( const QAngle& angles ) -> QAngle&
{
	Subtract( angles );
	return *this;
}

auto QAngle::operator *= ( const QAngle& angles ) -> QAngle&
{
	Multiply( angles );
	return *this;
}

auto QAngle::operator /= ( const QAngle& angles ) -> QAngle&
{
	Divide( angles );
	return *this;
}

auto QAngle::operator += ( float value ) -> QAngle&
{
	Add( value );
	return *this;
}

auto QAngle::operator -= ( float value ) -> QAngle&
{
	Subtract( value );
	return *this;
}

auto QAngle::operator *= ( float value ) -> QAngle&
{
	Multiply( value );
	return *this;
}

auto QAngle::operator /= ( float value ) -> QAngle&
{
	Divide( value );
	return *this;
}

auto QAngle::operator + ( const QAngle& angles ) const -> QAngle
{
	auto data = *this;
	data.Add( angles );
	return data;
}

auto QAngle::operator - ( const QAngle& angles ) const -> QAngle
{
	auto data = *this;
	data.Subtract( angles );
	return data;
}

auto QAngle::operator * ( const QAngle& angles ) const -> QAngle
{
	auto data = *this;
	data.Multiply( angles );
	return data;
}

auto QAngle::operator / ( const QAngle& angles ) const -> QAngle
{
	auto data = *this;
	data.Divide( angles );
	return data;
}

auto QAngle::operator + ( float value ) const -> QAngle
{
	auto data = *this;
	data.Add( value );
	return data;
}

auto QAngle::operator - ( float value ) const -> QAngle
{
	auto data = *this;
	data.Subtract( value );
	return data;
}

auto QAngle::operator * ( float value ) const -> QAngle
{
	auto data = *this;
	data.Multiply( value );
	return data;
}

auto QAngle::operator / ( float value ) const -> QAngle
{
	auto data = *this;
	data.Divide( value );
	return data;
}

QAngle QAngle::operator-(void) const
{
	return QAngle(-m_x, -m_y, -m_z);
}

QAngle QAngle::Zero = { 0.f, 0.f, 0.f };


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

  @copyright 2016 NumScale SAS

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

#ifdef BOOST_MSVC
  #pragma warning(disable: 4146) // unary minus applied to unsigned
  #pragma warning(push)
#endif
#include <boost/config.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/unary_minus.hpp>
#
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( unary_minus_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const { return -a0; }
  };

} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    ISAM2-impl.cpp
 * @brief   Incremental update functionality (ISAM2) for BayesTree, with fluid
 * relinearization.
 * @author  Michael Kaess
 * @author  Richard Roberts
 */

#include <gtsam/base/debug.h>
#include <gtsam/config.h>            // for GTSAM_USE_TBB
#include <gtsam/inference/Symbol.h>  // for selective linearization thresholds
#include <gtsam/nonlinear/ISAM2-impl.h>

#include <boost/range/adaptors.hpp>
#include <functional>
#include <limits>
#include <string>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
                                   VectorValues* result) {
  // parents are assumed to already be solved and available in result
  result->update(clique->conditional()->solve(*result));

  // starting from the root, call optimize on each conditional
  for (const ISAM2::sharedClique& child : clique->children)
    optimizeInPlace(child, result);
}
}  // namespace internal

/* ************************************************************************* */
size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
                                           const KeySet& replacedKeys,
                                           double wildfireThreshold,
                                           VectorValues* delta) {
  size_t lastBacksubVariableCount;

  if (wildfireThreshold <= 0.0) {
    // Threshold is zero or less, so do a full recalculation
    for (const ISAM2::sharedClique& root : roots)
      internal::optimizeInPlace(root, delta);
    lastBacksubVariableCount = delta->size();

  } else {
    // Optimize with wildfire
    lastBacksubVariableCount = 0;
    for (const ISAM2::sharedClique& root : roots)
      lastBacksubVariableCount += optimizeWildfireNonRecursive(
          root, wildfireThreshold, replacedKeys, delta);  // modifies delta

#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
    for (VectorValues::const_iterator key_delta = delta->begin();
         key_delta != delta->end(); ++key_delta) {
      assert((*delta)[key_delta->first].allFinite());
    }
#endif
  }

  return lastBacksubVariableCount;
}

/* ************************************************************************* */
namespace internal {
void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
                  const VectorValues& grad, VectorValues* RgProd,
                  size_t* varsUpdated) {
  // Check if any frontal or separator keys were recalculated, if so, we need
  // update deltas and recurse to children, but if not, we do not need to
  // recurse further because of the running separator property.
  bool anyReplaced = false;
  for (Key j : *clique->conditional()) {
    if (replacedKeys.exists(j)) {
      anyReplaced = true;
      break;
    }
  }

  if (anyReplaced) {
    // Update the current variable
    // Get VectorValues slice corresponding to current variables
    Vector gR =
        grad.vector(KeyVector(clique->conditional()->beginFrontals(),
                                    clique->conditional()->endFrontals()));
    Vector gS =
        grad.vector(KeyVector(clique->conditional()->beginParents(),
                                    clique->conditional()->endParents()));

    // Compute R*g and S*g for this clique
    Vector RSgProd = clique->conditional()->R() * gR +
                     clique->conditional()->S() * gS;

    // Write into RgProd vector
    DenseIndex vectorPosition = 0;
    for (Key frontal : clique->conditional()->frontals()) {
      Vector& RgProdValue = (*RgProd)[frontal];
      RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
      vectorPosition += RgProdValue.size();
    }

    // Now solve the part of the Newton's method point for this clique
    // (back-substitution)
    // (*clique)->solveInPlace(deltaNewton);

    *varsUpdated += clique->conditional()->nrFrontals();

    // Recurse to children
    for (const ISAM2::sharedClique& child : clique->children) {
      updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated);
    }
  }
}
}  // namespace internal

/* ************************************************************************* */
size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots,
                                 const KeySet& replacedKeys,
                                 const VectorValues& gradAtZero,
                                 VectorValues* RgProd) {
  // Update variables
  size_t varsUpdated = 0;
  for (const ISAM2::sharedClique& root : roots) {
    internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd,
                           &varsUpdated);
  }

  return varsUpdated;
}

/* ************************************************************************* */
VectorValues DeltaImpl::ComputeGradientSearch(const VectorValues& gradAtZero,
                                                const VectorValues& RgProd) {
  // Compute gradient squared-magnitude
  const double gradientSqNorm = gradAtZero.dot(gradAtZero);

  // Compute minimizing step size
  double RgNormSq = RgProd.vector().squaredNorm();
  double step = -gradientSqNorm / RgNormSq;

  // Compute steepest descent point
  return step * gradAtZero;
}

}  // namespace gtsam


// FaceMorphDlg.cpp : implementation file
//

#include "pch.h"
#include "framework.h"
#include "FaceMorph.h"
#include "FaceMorphDlg.h"
#include "FileHandle.h"
#include "DxDlg.h"
#include "afxdialogex.h"
#include <Eigen/Core>
#include <Eigen/SVD>

#ifdef _DEBUG
#define new DEBUG_NEW
#endif
const int LINE_BUFF_SIZE = 4096;
const float g_fFOV = 12.5936f;// used to be 45.5f;
const float g_fAspect = 1.0f;
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
#ifdef AFX_DESIGN_TIME
	enum { IDD = IDD_ABOUTBOX };
#endif

	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support

// Implementation
protected:
	DECLARE_MESSAGE_MAP()
};
//---------------------------------------------------------------//
CAboutDlg::CAboutDlg() : CDialog(IDD_ABOUTBOX)
{
}
//---------------------------------------------------------------//
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}
//---------------------------------------------------------------//
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
END_MESSAGE_MAP()

// CFaceMorphDlg dialog

//---------------------------------------------------------------//
CFaceMorphDlg::CFaceMorphDlg(CWnd* pParent /*=nullptr*/)
	: CDialog(IDD_FACEMORPH_DIALOG, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	//cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, alpha, A, k, B, n, beta, C, n);
}
//---------------------------------------------------------------//
CFaceMorphDlg::~CFaceMorphDlg()
{

}
//---------------------------------------------------------------//
void CFaceMorphDlg::DoDataExchange(CDataExchange* pDX)
{
	for (int h = 0; h < NUM_SLIDERS; h++)
	{
		DDX_Control(pDX, IDC_SLIDER1 + h, m_Slider[h]);
	}
	CDialog::DoDataExchange(pDX);
}
//---------------------------------------------------------------//
BEGIN_MESSAGE_MAP(CFaceMorphDlg, CDialog)
	ON_WM_TIMER()
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_HSCROLL()
	ON_WM_QUERYDRAGICON()
	ON_WM_CLOSE()
	ON_BN_CLICKED(IDC_RESET, &CFaceMorphDlg::OnBnClickedReset)
	ON_BN_CLICKED(IDC_CHECK1, &CFaceMorphDlg::OnBnClickedCheck1)
	ON_BN_CLICKED(IDC_EXPORT, &CFaceMorphDlg::OnBnClickedExport)
END_MESSAGE_MAP()
//---------------------------------------------------------------//
// CFaceMorphDlg message handlers
BOOL CFaceMorphDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != nullptr)
	{
		BOOL bNameValid;
		CString strAboutMenu;
		bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
		ASSERT(bNameValid);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon

	SetDlgItemText(IDC_EXPORT_PATH, L"C:\\_3DMMExport");

	CRect rtFrame;
	GetDlgItem(IDC_FRAME)->GetWindowRect(rtFrame);
	ScreenToClient(rtFrame);

	for (int h = 0; h < NUM_SLIDERS; h++)
	{
		m_Slider[h].SetRange(0, 2 * SLIDER_CENTER);
		m_Slider[h].SetPos(SLIDER_CENTER);
	}
	m_pRenderWnd = new CDxWnd();

	BOOL b2 = m_pRenderWnd->Create(NULL, _T("DxWnd"), WS_VISIBLE, rtFrame, this, 1);

	//calculate the eigenvalues here and pass them to CDxDlg
	//first we need to find the Meshess folder//
	//This will be at a higher level. Recursively search until we find it
	CString strMeshPath;
	TCHAR szFilePath[MAX_PATH + _ATL_QUOTES_SPACE];
	DWORD dwFLen = ::GetModuleFileName(NULL, szFilePath + 0, MAX_PATH);
	strMeshPath = CString(szFilePath);

	long nRight = strMeshPath.ReverseFind(_T('\\'));//remove exe name
	strMeshPath = strMeshPath.Left(nRight);//this will ne the same folder as the EXe
	bool bFoundMeshFolder = false;
	int nNumAttempts = 0;
	while (!bFoundMeshFolder && nNumAttempts < 3)
	{
		nRight = strMeshPath.ReverseFind(_T('\\'));//move one folder up
		strMeshPath = strMeshPath.Left(nRight);//this will ne the same folder as
		nNumAttempts++;
		if (DoesDirExist(strMeshPath + L"\\Meshes")) {
			bFoundMeshFolder = true;
			strMeshPath.Append(L"\\Meshes\\");
		}

	}
	if (bFoundMeshFolder)
	{
		CalcEigenValues(strMeshPath);
	}
	else
	{
		AfxMessageBox(L"Failed to find Mesh Folder");
	}

	// Draw the background gradient.
	m_pRenderWnd->Initialize(this);
	m_pRenderWnd->MoveWindow(rtFrame);
	SetTimer(1, 50, NULL);

	/*dgesvd("All", "All", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork,
		&info);*/

	return TRUE;  // return TRUE  unless you set the focus to a control
}
//---------------------------------------------------------------//
bool CFaceMorphDlg::CalcEigenValues(CString strMeshPath)
{
	if (!m_pRenderWnd)
	{
		AfxMessageBox(L"m_pRenderWnd not created");
		return false;
	}
	std::vector<std::vector<float> > vMeshes;

	int nNumVertices = -1;
	int nI1 = 0;
	int nI2 = 0;
	int nI3 = 0;

	////////////////////////////////////////////////////////////////////////////////////////
	//STEP ONE: load the files
	////////////////////////////////////////////////////////////////////////////////////////
	CFileFind finder;
	//count the number of files we will have to load
	int nNumFiles = 0;
	BOOL bWorking = finder.FindFile(strMeshPath + _T("*.mesh"));
	while (bWorking)
	{
		bWorking = finder.FindNextFile();
		nNumFiles++;
	}
	finder.Close();

	int nFileCounter = 0;
	bWorking = finder.FindFile(strMeshPath + _T("*.mesh"));
	while (bWorking)
	{
		bWorking = finder.FindNextFile();
		if (!bWorking) {
			long ert = 0;
		}
		CString strPath = finder.GetFilePath();
		int nCounter = 0;
		CFileHandle file(strPath, TEXT("r"));
		if (file.GetFile() != 0)
		{
			//cound the vertices in advance and make sure they fit the vector
			while (!feof(file.GetFile()))
			{
				CHAR buffer[LINE_BUFF_SIZE];
				fgets(buffer, LINE_BUFF_SIZE, file.GetFile());
				if (buffer[0] == 'v')
				{
					nCounter++;
				}
			}
			if (nNumVertices == -1)
			{
				nNumVertices = nCounter;
			}
			else if (nNumVertices != nCounter)
			{
				finder.Close();
				AfxMessageBox(L"wrong size");
				return false;
			}
			if (nFileCounter == 0)
			{
				m_pRenderWnd->m_nNumVertices = nNumVertices;
				//we have enough info now to preallocate memory to avoid performance penalties caused by dynamic memory allocations...
				vMeshes.reserve(nNumFiles);
				vMeshes.resize(nNumFiles);
				int nNumVerticesXYZ = nNumVertices * 3;
				for (int h = 0; h < nNumFiles; h++)
				{
					vMeshes[h].reserve(nNumVerticesXYZ);
					vMeshes[h].resize(nNumVerticesXYZ);
				}

				m_pRenderWnd->m_AveFace.reserve(nNumVerticesXYZ);
				m_pRenderWnd->m_AveFace.resize(nNumVerticesXYZ);
				m_pRenderWnd->m_Mesh.reserve(nNumVerticesXYZ);
				m_pRenderWnd->m_Mesh.resize(nNumVerticesXYZ);
			}
			rewind(file.GetFile());
			int nIndex = 0;
			float x = 0;
			float y = 0;
			float z = 0;
			while (!feof(file.GetFile()))
			{
				CHAR buffer[LINE_BUFF_SIZE];
				fgets(buffer, LINE_BUFF_SIZE, file.GetFile());
				if (0 == strncmp("v ", buffer, 2))
				{
					//load vertices into the vectors
					sscanf_s(buffer + 1, "%f %f %f", &x, &y, &z);
					m_pRenderWnd->m_AveFace[nIndex] += x;
					m_pRenderWnd->m_AveFace[nIndex + nNumVertices] += y;
					m_pRenderWnd->m_AveFace[nIndex + 2 * nNumVertices] += z;
					vMeshes[nFileCounter][nIndex] = x;
					vMeshes[nFileCounter][nIndex + nNumVertices] = y;
					vMeshes[nFileCounter][nIndex + 2 * nNumVertices] = z;
					nIndex++;
				}
				else if (nFileCounter == 0 && 0 == strncmp("f ", buffer, 2))
				{
					//load face information (list of three points forming a triangle). Do it for the first mesh only as the rest should be the same
					sscanf_s(buffer + 1, "%d %d %d", &nI1, &nI2, &nI3);
					m_pRenderWnd->m_vIndices.push_back(nI1 - 1);
					m_pRenderWnd->m_vIndices.push_back(nI2 - 1);
					m_pRenderWnd->m_vIndices.push_back(nI3 - 1);
				}
			}
			nFileCounter++;
		}
	}
	//finder.Close(); //note, CFileFinder calls Close on destructor, so we don't need to call this here

	////////////////////////////////////////////////////////////////////////////////////////
	//STEP TWO: calculate the mean/average face mesh
	////////////////////////////////////////////////////////////////////////////////////////
	for_each(m_pRenderWnd->m_AveFace.begin(), m_pRenderWnd->m_AveFace.end(), [nNumFiles](float& x) {x = x / nNumFiles; });

	////////////////////////////////////////////////////////////////////////////////////////
	//STEP THREE: subtract the mean from each face mesh
	////////////////////////////////////////////////////////////////////////////////////////
	auto t = m_pRenderWnd->m_AveFace;
	for (auto& m : vMeshes)
	{
		int j = 0;
		for_each(m.begin(), m.end(), [&j, &t](float& x) {x = x - t[j]; j++; });
	}
	/*for (int h = 0; h < nNumFiles; h++)//older style for loop - but is it easier to read??
	{
		for (int j = 0; j < nNumVertices * 3; j++){
			vMeshes[h][j] = vMeshes[h][j] - m_pRenderWnd->m_AveFace[j];
		}
	}*/
	////////////////////////////////////////////////////////////////////////////////////////
	//STEP FOUR: calculate the eigenvectors
	////////////////////////////////////////////////////////////////////////////////////////
	int nNumVerticesXYZ = nNumVertices * 3;
	Eigen::MatrixXf X(nNumVerticesXYZ, nNumFiles);
	int h = 0;
	for (auto& m : vMeshes)
	{
		int j = 0;
		for_each(m.begin(), m.end(), [&j, h, &X](float& x) {X(j, h) = x; j++; });
		h++;
	}
	/*for (int h = 0; h < nNumFiles; h++)//older style for loop - is this easier to read??
	{
		for (int j = 0; j < nNumVerticesXYZ; j++) {
			X(j, h) = vMeshes[h][j];
		}
	}*/

	//Use Principle Component Analysis. SVD routine in the Eigen library to do this
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(X, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::MatrixXf U = svd.matrixU();
	Eigen::MatrixXf V = svd.matrixV();
	Eigen::VectorXf s = svd.singularValues();

	if (s.rows() < NUM_EIGENS)
	{
		CString strMsg;
		strMsg.Format(L"Not enough eigenvectors found. We need at least %d. Please increase the number of meshes and restart the application.", NUM_EIGENS);
		AfxMessageBox(strMsg);
		return false;
	}

	//There will be nNumFiles eigenvectors, but only take the first 20 (NUM_EIGENS)
	m_pRenderWnd->m_Eigen.reserve(NUM_EIGENS);
	m_pRenderWnd->m_Eigen.resize(NUM_EIGENS);
	for (int h = 0; h < NUM_EIGENS; h++)
	{
		m_pRenderWnd->m_Eigen[h].reserve(nNumVerticesXYZ);
		m_pRenderWnd->m_Eigen[h].resize(nNumVerticesXYZ);
		int j = 0;
		for_each(m_pRenderWnd->m_Eigen[h].begin(), m_pRenderWnd->m_Eigen[h].end(), [&j, h, &U](float& x) {x = U(j, h); j++; });
		/*for (int j = 0; j < nNumVerticesXYZ; j++) 	{//older style, again, isn't it easier to read?
			m_pRenderWnd->m_Eigen[h][j] = U(j, h);
		}*/
	}
	//We now have the eigenvectors. The mesh that is rendered will be the average/mean mesh plus a linear combination of the
	//the eignevector meshes (m_Eigen). That is each eigenvector is multipled by the scalar corresponding to each slider in the UI.
	return true;
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnTimer(UINT_PTR nIdEvent)
{
	if (nIdEvent == 1)
	{
		if(m_pRenderWnd) m_pRenderWnd->Render();
	}
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}
//---------------------------------------------------------------//
// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.
void CFaceMorphDlg::OnPaint()
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}

	for (int h = 0; h < 20; h++)
	{
		m_Slider[h].SetRange(0, 2 * SLIDER_CENTER);
		m_Slider[h].SetPos(SLIDER_CENTER);
	}
}
//---------------------------------------------------------------//
// The system calls this function to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CFaceMorphDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnSlider()
{
	if (!m_pRenderWnd) return;
	for (int h = 0; h < NUM_SLIDERS; h++)
	{
		m_Scalars.m_Next[h] = (float)m_Slider[h].GetPos() - SLIDER_CENTER;
	}
	m_pRenderWnd->RecalcMesh(m_Scalars);
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar)
{
	CSliderCtrl* pSlider = reinterpret_cast<CSliderCtrl*>(pScrollBar);
	for (int h = 0; h < NUM_SLIDERS; h++)
	{
		if (pSlider == &m_Slider[h])
		{
			OnSlider();
		}
	}
	CDialog::OnHScroll(nSBCode, nPos, pScrollBar);
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnClose()
{
	if (m_pRenderWnd)
	{
		m_pRenderWnd->ShutDown();
		m_pRenderWnd->DestroyWindow();
		delete m_pRenderWnd;
		m_pRenderWnd = 0;
	}

	CDialog::OnClose();
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnBnClickedReset()
{
	for (int h = 0; h < NUM_SLIDERS; h++)
	{
		m_Slider[h].SetPos(SLIDER_CENTER);
	};
	OnSlider();
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnBnClickedCheck1()
{
	if (!m_pRenderWnd) return;
	m_pRenderWnd->m_bWireframe = !m_pRenderWnd->m_bWireframe;
}
//-------------------------------------------------------------------------------------//
bool CFaceMorphDlg::DoesDirExist(CString str)
{
	DWORD dwRetVal = GetFileAttributes(str);
	if (dwRetVal == 0xFFFFFFFF)
	{
		return false;
	}
	else if (dwRetVal & FILE_ATTRIBUTE_DIRECTORY)
	{
		return true;
	}
	return false;
}
//---------------------------------------------------------------//
void CFaceMorphDlg::OnBnClickedExport()
{
	CString strLine;
	CString strExportPath;
	CWaitCursor wait;

	GetDlgItemText(IDC_EXPORT_PATH, strExportPath);
	if (!DoesDirExist(strExportPath))
	{
		AfxMessageBox(L"Folder doesn't exist!");
		return;
	}
	//export eigenvalues
	CStdioFile f;
	int nNumVerticesXYZ = m_pRenderWnd->m_nNumVertices * 3;
	if (f.Open(strExportPath + L"\\Eigen.dat", CFile::modeCreate | CFile::modeWrite))
	{
		for (int j = 0; j < nNumVerticesXYZ; j++)
		{
			for (int h = 0; h < NUM_EIGENS; h++)
			{
				strLine.Format(L"%f", m_pRenderWnd->m_Eigen[h][j]);
				f.WriteString(strLine);
			}
			f.WriteString(L"\n");
		}
		f.Close();
	}
	else
	{
		AfxMessageBox(L"Could not save EigenVector file");
		return;
	}

	if (f.Open(strExportPath + L"\\MeanFace.dat", CFile::modeCreate | CFile::modeWrite))
	{
		for (int j = 0; j < nNumVerticesXYZ; j++)
		{
			strLine.Format(L"%f\n", m_pRenderWnd->m_AveFace[j]);
			f.WriteString(strLine);
		}
		f.Close();
	}
	else
	{
		AfxMessageBox(L"Could not save file");
		return;
	}

	if (f.Open(strExportPath + L"\\MeanFace.obj", CFile::modeCreate | CFile::modeWrite))
	{
		for (int j = 0; j < m_pRenderWnd->m_nNumVertices; j++)
		{
			strLine.Format(L"v %f %f %f\n",
				m_pRenderWnd->m_AveFace[j],
				m_pRenderWnd->m_AveFace[m_pRenderWnd->m_nNumVertices + j],
				m_pRenderWnd->m_AveFace[2 * m_pRenderWnd->m_nNumVertices + j]);
			f.WriteString(strLine);
		}
		size_t nNumTriangles = m_pRenderWnd->m_vIndices.size() / 3;
		for (int j = 0; j < nNumTriangles; j++)
		{
			strLine.Format(L"f %d %d %d\n",
				m_pRenderWnd->m_vIndices[3 * j + 0] + 1,
				m_pRenderWnd->m_vIndices[3 * j + 1] + 1,
				m_pRenderWnd->m_vIndices[3 * j + 2] + 1);
			f.WriteString(strLine);
		}
		f.Close();
	}
	else
	{
		AfxMessageBox(L"Could not save file");
		return;
	}
	//To load this into matlab using a .,m file, something along the following should work
	//fid = fopen('C:\_3DMMExport\MeanFace.dat');
	//avgFace = [];
	//while ~feof(fid)
	//	tline = fgetl(fid);
	//avgFace = [avgFace; sscanf(tline(1:end), '%f')'];
	//	end
	//	fclose(fid);
	//fid = fopen('C:\_3DMMExport\Eigen.dat');
	//egn = [length(avgFace), 20];
	//k = 1;
	//while ~feof(fid)
	//	tline = fgetl(fid);
	//egn(k, 1:20) = sscanf(tline(1:end), '%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f');
	//k = k + 1;
	//end
	//	fclose(fid);
}


//
// Created by qfeng on 2018-12-27.
//

#include "pipeline/pl_tail.h"
#include "model/model_daemon.h"
#include "ctpl.h"
#include <boost/thread/latch.hpp>

namespace algocomp {

    float PLTail::sigmoid(float inp_score) {
        if (std::isfinite(inp_score)) {
            return 1.0 / (1.0 + exp(-inp_score));
        } else return 0.5;
    }

    const std::vector<float> PLTail::work_on_none_mode(AUCImprEncs &inp_encs) {
        // encs->kvs->scores

        std::vector<float> outp;
        try {
            for (int i = 0; i < inp_encs.encs_size(); ++i) {
                auto enc_bunch = inp_encs.encs(i);

                Eigen::SparseVector<float> kv(inp_encs.feat_size());
                kv.reserve(enc_bunch.encs_size());
                for (auto enc: enc_bunch.encs()) {
                    kv.insert(enc.enc()) = enc.val();
                }

                if (_model) {
                    // Model score and sigmoid transform
                    auto score = _model->score(kv);
                    outp.push_back(sigmoid(score));
                } else {
                    _logger->error("Use (separately) empty model daemon.");
                }
            }

        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        return outp;
    }

    const std::vector<float> PLTail::work_on_example_mode(AUCImprEncs &inp_encs) {
        std::vector<float> outp;
        try {
            boost::latch count_down_latch(1);
            auto mth_func = [this, &outp, &inp_encs, &count_down_latch](int id) {
                for (int i = 0; i < inp_encs.encs_size(); ++i) {
                    auto enc_bunch = inp_encs.encs(i);
                    Eigen::SparseVector<float> kv(inp_encs.feat_size());
                    kv.reserve(enc_bunch.encs_size());
                    for (auto enc: enc_bunch.encs()) {
                        kv.insert(enc.enc()) = enc.val();
                    }

                    if (_model) {
                        // Model score and sigmoid transform
                        auto score = _model->score(kv);
                        outp.push_back(sigmoid(score));
                    } else {
                        _logger->error("Use (separately) empty model daemon.");
                    }
                }
                count_down_latch.count_down();
            };

            _thp_rsrc->push(mth_func);
            count_down_latch.wait();
        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        return outp;
    }

    const std::vector<float> PLTail::work_on_ads_mode(AUCImprEncs &inp_encs) {
        std::vector<float> outp;
        try {
            std::vector<std::pair<int, float>> outp_;
            boost::latch count_down_latch(inp_encs.encs_size());
            std::mutex res_mutex;

            auto mth_func = [this, &outp_, &res_mutex, &count_down_latch, &inp_encs](int id, int index) {
                try {
                    auto enc_bunch = inp_encs.encs(index);
                    Eigen::SparseVector<float> kv(inp_encs.feat_size());
                    kv.reserve(enc_bunch.encs_size());
                    for (auto enc: enc_bunch.encs()) {
                        kv.insert(enc.enc()) = enc.val();
                    }

                    if (_model) {
                        auto score = _model->score(kv);
                        {
                            std::lock_guard<std::mutex> lock(res_mutex);
                            outp_.emplace_back(index, sigmoid(score));
                        }
                    } else {
                        _logger->error("Use (separately) empty model daemon.");
                    }

                } catch (const std::exception &ex) {
                    _logger->error("Predict score fail (in thread {}) in pltail: {}", id, ex.what());
                }
                count_down_latch.count_down();
            };

            for (int i = 0; i < inp_encs.encs_size(); ++i) {
                _thp_rsrc->push(mth_func, i);
            }
            count_down_latch.wait();

            if (outp_.size() > 0 && outp_.size() == inp_encs.encs_size()) {
                std::sort(outp_.begin(), outp_.end(),
                          [](const std::pair<int, float> &lhs, const std::pair<int, float> &rhs) {
                              return lhs.first < rhs.first;
                          });
                std::for_each(outp_.begin(), outp_.end(), [&outp](std::pair<int, float> ss) {
                    outp.push_back(ss.second);
                });
            } else {
                _logger->warn("Number of predicted scores is zero or inconsistent with input size {} != {}",
                              outp_.size(), inp_encs.encs_size());
            }
        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        return outp;
    }

    const std::vector<float> PLTail::work_on_none_mode(const std::vector<Eigen::SparseVector<float>> &kv) {
        std::vector<float> outp;
        try {
            if (_model) {
                for (int i = 0; i < kv.size(); ++i) {
                    // Model score and sigmoid transform
                    auto score = _model->score(kv[i]);
                    outp.push_back(sigmoid(score));
                }
            } else {
                _logger->error("Use (separately) empty model daemon.");
            }
        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        if (outp.size() != kv.size()) {
            _logger->error("Inconsistent size of input kvs {} and output socres {} (thread type: {}).", kv.size(),
                           outp.size(), _thread_type_str);
            outp.clear();
        }

        return outp;
    }

    const std::vector<float> PLTail::work_on_example_mode(const std::vector<Eigen::SparseVector<float>> &kv) {
        std::vector<float> outp;
        try {
            if (_model) {
                boost::latch count_down_latch(1);
                auto mth_func = [this, &kv, &outp, &count_down_latch](int id) {
                    for (int i = 0; i < kv.size(); ++i) {
                        // Model score and sigmoid transform
                        auto score = _model->score(kv[i]);
                        outp.push_back(sigmoid(score));
                    }
                    count_down_latch.count_down();
                };
                _thp_rsrc->push(mth_func);
                count_down_latch.wait();
            } else {
                _logger->error("Use (separately) empty model daemon.");
            }

        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        return outp;

    }

    const std::vector<float> PLTail::work_on_ads_mode(const std::vector<Eigen::SparseVector<float>> &kv) {
        std::vector<float> outp;
        try {
            if (_model) {
                std::vector<std::pair<int, float>> outp_;
                boost::latch count_down_latch(kv.size());
                std::mutex res_mutex;
                auto mth_func = [this, &kv, &outp_, &count_down_latch, &res_mutex](int id, int index) {
                    {
                        auto score = _model->score(kv[index]);
                        {
                            std::lock_guard<std::mutex> lock(res_mutex);
                            outp_.emplace_back(index, sigmoid(score));
                        }
                    }
                    count_down_latch.count_down();
                };
                for (int i = 0; i < kv.size(); ++i) {
                    _thp_rsrc->push(mth_func, i);
                }
                count_down_latch.wait();

                if (outp_.size() > 0 && outp_.size() == kv.size()) {
                    std::sort(outp_.begin(), outp_.end(),
                              [](const std::pair<int, float> &lhs, const std::pair<int, float> &rhs) {
                                  return lhs.first < rhs.first;
                              });
                    std::for_each(outp_.begin(), outp_.end(), [&outp](std::pair<int, float> v) {
                        outp.push_back(v.second);
                    });

                } else {
                    _logger->warn("Number of predicted scores is zero or inconsistent with input size {} != {}",
                                  outp_.size(), kv.size());
                }
            } else {
                _logger->error("Use (separately) empty model daemon.");
            }

        } catch (const exception &ex) {
            _logger->error("Predict score (separately used) fail in pltail (thread type: {}) : {}",
                           _thread_type_str,
                           ex.what());
        }

        return outp;
    }

    const std::vector<float> PLTail::work(const char *encs_pb, long len_encs_pb) {
        std::vector<float> outp;
        google::protobuf::Arena arena;

        try {
            AUCImprEncs *encs = google::protobuf::Arena::CreateMessage<AUCImprEncs>(&arena);
            encs->ParseFromArray(encs_pb, len_encs_pb);

            if (_thread_type == ThreadType::NO) {
                outp = work_on_none_mode(*encs);
            } else if (_thread_type == ThreadType::ON_EXAMPLE) {
                outp = work_on_example_mode(*encs);
            } else if (_thread_type == ThreadType::ON_ADS) {
                outp = work_on_ads_mode(*encs);
            } else {
                _logger->error("Unknown thread type.");
            }
        } catch (const exception &ex) {
            _logger->error("Predict scores (separately used) fail in pltail: {}", ex.what());
        }
        if (arena.SpaceAllocated()) {
            arena.Reset();
        }

        return outp;
    }

    const std::vector<float> PLTail::work(const vector<Eigen::SparseVector<float>> &kv) {
        std::vector<float> outp;
        if (_thread_type == ThreadType::NO) {
            outp = work_on_none_mode(kv);
        } else if (_thread_type == ThreadType::ON_EXAMPLE) {
            outp = work_on_example_mode(kv);

        } else if (_thread_type == ThreadType::ON_ADS) {
            outp = work_on_ads_mode(kv);
        } else {
            _logger->error("Unknown thread type.");
        }
        return outp;
    }
}


/*
 * Tests for the Harris/Shi-Tomasi feature detector
 * test_harris.cpp
 *
 * Copyright (c) 2019-2020 Balazs Nagy,
 * Robotics and Perception Group, University of Zurich
 * 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 <iostream>
#include <Eigen/Dense>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <cuda_runtime_api.h>
#include "../include/test_harris.h"
#include "vilib/preprocess/pyramid.h"
#include "vilib/storage/pyramid_pool.h"
#include "vilib/feature_detection/harris/harris_cpu.h"
#include "vilib/feature_detection/harris/harris_gpu.h"
#include "vilib/config.h"
#include "vilib/timer.h"
#include "vilib/statistics.h"

using namespace vilib;

/*
 * Note to future self and others:
 * there's some difference between the CPU and the GPU implementation.
 * the reason is three-fold:
 * - the CPU (OpenCV) implementation does not provide a score for features,
 *   hence we need to generate the score ourselves (currently hardcoded to be 1.0f)
 *   During multi-level detection, if a cell is already occupied by a feature, this
 *   feature is not going to be replaced from lower pyramid levels, because they will have
 *   the same hard-coded score. This is a deficiency of cv::goodFeaturesToTrack()
 * - the CPU (OpenCV) implementation uses a minimum euclidean distance between
 *   features. Once this is satisfied, then we apply the grid cell. (if set to 0,
 *   this is not an issue)
 * - the GPU version skips at least 1 row and 1 column of pixels at the borders which
 *   might contain the maximum response, and since we use relative thresholding,
 *   this might introduce a (noticable) difference between the CPU and GPU version outputs
 */

// Frame preprocessing
#define PYRAMID_LEVELS                       1
#define PYRAMID_MIN_LEVEL                    0
#define PYRAMID_MAX_LEVEL                    PYRAMID_LEVELS

// Harris/Shi-Tomasi detector parameters
#define USE_HARRIS                           true
#define HARRIS_K                             0.04f
// CPU Detector parameters
#define HARRIS_QUALITY_LEVEL                 0.1f
#define SHI_TOMASI_QUALITY_LEVEL             0.1f
#define MIN_EUCLIDEAN_DISTANCE               0.0f
#define CONV_FILTER_BORDER_TYPE              conv_filter_border_type::BORDER_SKIP

// NMS parameters
#define HORIZONTAL_BORDER                    0
#define VERTICAL_BORDER                      0
#define CELL_SIZE_WIDTH                      32
#define CELL_SIZE_HEIGHT                     32

// Test framework options
#define DISPLAY_PYRAMID_CPU                  0
#define DISPLAY_DETECTED_FEATURES_CPU        1
#define DISPLAY_DETECTED_FEATURES_GPU        1
#define ENABLE_CPU_VERSION                   1
#define ENABLE_GPU_VERSION                   1

// Test framework statistics
#define STAT_ID_DETECTOR_TIMER               0
#define STAT_ID_FEATURE_COUNT                1

#if (USE_HARRIS == true)
#define DETECTOR_NAME                        "Harris"
#else
#define DETECTOR_NAME                        "Shi-Tomasi"
#endif /* USE_HARRIS */
#define QUALITY_LEVEL                        (USE_HARRIS?HARRIS_QUALITY_LEVEL:SHI_TOMASI_QUALITY_LEVEL)

TestHarris::TestHarris(const char * file_path, const int max_image_num) :
  TestBase(DETECTOR_NAME " detector",file_path,max_image_num) {
}

TestHarris::~TestHarris(void) {
}

bool TestHarris::run(void) {
  // Create the detector statistics
  std::vector<Statistics> stat_cpu, stat_gpu;
  stat_cpu.emplace_back("[usec]",DETECTOR_NAME);
  stat_cpu.emplace_back("[1]",DETECTOR_NAME " feature count");
  stat_gpu.emplace_back("[usec]",DETECTOR_NAME);
  stat_gpu.emplace_back("[1]",DETECTOR_NAME " feature count");

  // Create detectors
  if(!load_image_dimensions()) {
    // Could not acquire the initialization parameters
    return false;
  }
  // CPU
#if ENABLE_CPU_VERSION
  detector_cpu_.reset(new opencv::HarrisCPU<true>(image_width_,
                                                 image_height_,
                                           CELL_SIZE_WIDTH,
                                           CELL_SIZE_HEIGHT,
                                           PYRAMID_MIN_LEVEL,
                                           PYRAMID_MAX_LEVEL,
                                           HORIZONTAL_BORDER,
                                           VERTICAL_BORDER,
                                           USE_HARRIS,
                                           HARRIS_K,
                                           QUALITY_LEVEL,
                                           MIN_EUCLIDEAN_DISTANCE));
#endif /* ENABLE_CPU_VERSION */
  // GPU
#if ENABLE_GPU_VERSION
  detector_gpu_.reset(new HarrisGPU(image_width_,
                                    image_height_,
                                    CELL_SIZE_WIDTH,
                                    CELL_SIZE_HEIGHT,
                                    PYRAMID_MIN_LEVEL,
                                    PYRAMID_MAX_LEVEL,
                                    HORIZONTAL_BORDER,
                                    VERTICAL_BORDER,
                                    CONV_FILTER_BORDER_TYPE,
                                    USE_HARRIS,
                                    HARRIS_K,
                                    QUALITY_LEVEL));
#endif /* ENABLE_GPU_VERSION */

  // Initialize the pyramid pool
  PyramidPool::init(1,
                    image_width_,
                    image_height_,
                    1,  // grayscale
                    PYRAMID_LEVELS,
                    IMAGE_PYRAMID_MEMORY_TYPE);

  // Run benchmark suite (it will call run_benchmark()) for us
  bool success = run_benchmark_suite(stat_cpu,stat_gpu);

  // Deinitialize the pyramid pool (for consecutive tests)
  PyramidPool::deinit();
  return success;
}

bool TestHarris::run_benchmark(std::vector<vilib::Statistics> & stat_cpu,
                               std::vector<vilib::Statistics> & stat_gpu) {
  Timer timer;
  std::vector<cv::Mat> image_pyramid;
  // CPU -----------------------------------------------------------------------
#if ENABLE_CPU_VERSION
  timer.start();
  pyramid_create_cpu(image_,image_pyramid,PYRAMID_LEVELS,false);
  // Reset detector's grid
  detector_cpu_->reset();
  // Do the detection
  detector_cpu_->detect(image_pyramid);
  timer.stop();
  // Add statistics
  stat_cpu[STAT_ID_DETECTOR_TIMER].add(timer.elapsed_usec());
  stat_cpu[STAT_ID_FEATURE_COUNT].add(detector_cpu_->count());
  // Display results
#if DISPLAY_PYRAMID_CPU
  pyramid_display(image_pyramid);
#endif /* DISPLAY_PYRAMID_CPU */
#if DISPLAY_DETECTED_FEATURES_CPU
  // detector_cpu_->displayFeatures(DETECTOR_NAME " detector (CPU)",image_pyramid,true);
  const std::vector<vilib::DetectorBase<true>::FeaturePoint> result_cpu = detector_cpu_->getPoints();
  int count_cpu = 0;
  for (int i = 0; i < result_cpu.size(); i++){
      if (result_cpu.at(i).x_ != 0 || result_cpu.at(i).y_){
          std::cout << "CPU: " << result_cpu.at(i).x_ << " " << result_cpu.at(i).y_ << std::endl;
          count_cpu ++;
      }
  }
  std::cout << "Number of concer: " << count_cpu << std::endl;
#endif /* DISPLAY_DETECTED_FEATURES_CPU */
#else
  (void)stat_cpu;
  pyramid_create_cpu(image_,image_pyramid,PYRAMID_LEVELS,false);
#endif /* ENABLE_CPU_VERSION */

  // GPU -----------------------------------------------------------------------
#if ENABLE_GPU_VERSION
  timer.start();
  // Create a Frame (image upload, pyramid)
  std::shared_ptr<Frame> frame0(new Frame(image_,0,PYRAMID_LEVELS));
  // Reset detector's grid
  // Note: this step could be actually avoided with custom processing
  detector_gpu_->reset();
  // Do the detection
  detector_gpu_->detect(frame0->pyramid_);
  timer.stop();
  // Add statistics
  stat_gpu[STAT_ID_DETECTOR_TIMER].add(timer.elapsed_usec());
  stat_gpu[STAT_ID_FEATURE_COUNT].add(detector_gpu_->count());
  // Display results
#if DISPLAY_DETECTED_FEATURES_GPU
  // frame0->pyramid_.at(0)->display();

  // detector_gpu_->displayFeatures(DETECTOR_NAME " detector (GPU)",image_pyramid,true,true);
  const std::vector<vilib::DetectorBase<true>::FeaturePoint> result_gpu = detector_gpu_->getPoints();
  int count_gpu = 0;
  for (int i = 0; i < result_gpu.size(); i++){
      if (result_gpu.at(i).x_ != 0 || result_gpu.at(i).y_){
          std::cout << "GPU: " <<result_gpu.at(i).x_ << " " << result_gpu.at(i).y_ << std::endl;
          count_gpu ++;
      }
  }
  std::cout << "Number of concer: " << count_gpu << std::endl;

#endif /* DISPLAY_DETECTED_FEATURES_GPU */
#else
  (void)stat_gpu;
#endif /* ENABLE_GPU_VERSION */

  return true;
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "exs.hpp"

#include <boost/format.hpp>

#include <alice/rules.hpp>

#include <core/utils/program_options.hpp>
#include <reversible/circuit.hpp>
#include <reversible/truth_table.hpp>
#include <cli/reversible_stores.hpp>
#include <reversible/synthesis/exact_synthesis.hpp>
#include <reversible/synthesis/exact_toffoli_synthesis.hpp>
#include <reversible/synthesis/quantified_exact_synthesis.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

exs_command::exs_command( const environment::ptr& env )
  : cirkit_command( env, "Exact synthesis" )
{
  opts.add_options()
    ( "mode,m",              value_with_default( &mode ),        "mode (0: BDD, 1: SAT, 2: SAT (Toffoli))" )
    ( "start_depth,s",       value_with_default( &start_depth ), "initial search depth" )
    ( "max_depth",           value_with_default( &max_depth ),   "maximum search depth" )
    ( "negative,n",                                              "allow negative control lines" )
    ( "multiple,m",                                              "allow multiple target lines (only with SAT)" )
    ( "all_solutions,a",                                         "extract all solutions (only with BDD)" )
    ;
  add_new_option( false );
  be_verbose();
}

command::rules_t exs_command::validity_rules() const
{
  return {
    { [this]() { return this->mode <= 2u; }, "mode must be either 0 or 1" },
    has_store_element<binary_truth_table>( env )
  };
}

bool exs_command::execute()
{
  auto& circuits = env->store<circuit>();
  auto& specs    = env->store<binary_truth_table>();

  extend_if_new( circuits );

  auto settings = make_settings();
  settings->set( "start_depth",   start_depth );
  settings->set( "max_depth",     max_depth );
  settings->set( "negative",      is_set( "negative" ) );
  settings->set( "multiple",      is_set( "multiple" ) );
  settings->set( "all_solutions", is_set( "all_solutions" ) );

  circuit circ;
  auto result = false;

  if ( mode == 0u )
  {
    result = quantified_exact_synthesis( circ, specs.current(), settings, statistics );
  }
  else if ( mode == 1u )
  {
    result = exact_synthesis( circ, specs.current(), settings, statistics );
  }
  else if ( mode == 2u )
  {
    result = exact_toffoli_synthesis( circ, specs.current(), settings, statistics );
  }

  circuits.current() = circ;

  if ( mode == 0u && result )
  {
    //std::cout << "[i] number of solutions: " << statistics->get<unsigned>( "num_circuits" ) << std::endl;
  }

  std::cout << boost::format( "[i] run-time: %.2f secs" ) % statistics->get<double>( "runtime" ) << std::endl;

  if ( mode == 0u && result && is_set( "all_solutions" ) )
  {
    auto current_index = circuits.current_index();

    for ( const auto& sol : statistics->get<std::vector<circuit>>( "solutions" ) )
    {
      circuits.extend();
      circuits.current() = sol;
    }

    circuits.set_current_index( current_index );
  }

  return true;
}

command::log_opt_t exs_command::log() const
{
  return log_opt_t({
      { "runtime", statistics->get<double>( "runtime" ) }//,
      //{ "num_circuits", static_cast<int>( statistics->get<unsigned>( "num_circuits" ) ) }
    });
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

//  As a special exception, you have permission to link this program
//  with the CGAL library and distribute executables, as long as you
//  follow the requirements of the GNU GPL in regard to all of the
//  software in the executable aside from CGAL.

#ifndef GRAPH_TRIANGULATION_HH
#define GRAPH_TRIANGULATION_HH

#include <unordered_set>
#include <tuple>

#include <boost/functional/hash.hpp>

#include "graph_util.hh"

namespace graph_tool
{
using namespace std;
using namespace boost;

template <class Graph>
bool is_adjacent(typename graph_traits<Graph>::vertex_descriptor v1,
                 typename graph_traits<Graph>::vertex_descriptor v2,
                 Graph& g)
{
    typename graph_traits<Graph>::out_edge_iterator e, e_end;
    for (tie(e, e_end) = out_edges(v1, g); e != e_end; ++e)
        if (target(*e, g) == v2)
            return true;
    return false;
}

struct hash_point
{
    template <class Vertex>
    std::size_t operator()(const Vertex& v) const
    {
        size_t seed = 42;
        hash_combine(seed, v.point().x());
        hash_combine(seed, v.point().y());
        hash_combine(seed, v.point().z());
        return seed;
    }
};

template <class Triang>
struct get_triangulation
{
    // this will insert edges in the graph
    template <class Graph, class VertexMap>
    class edge_inserter
    {
    public:
        typedef output_iterator_tag iterator_category;
        typedef typename graph_traits<Graph>::vertex_descriptor value_type;
        typedef size_t difference_type;
        typedef typename graph_traits<Graph>::vertex_descriptor* pointer;
        typedef typename graph_traits<Graph>::vertex_descriptor& reference;

        edge_inserter(Graph& g, const typename VertexMap::key_type& v,
                      VertexMap& vertex_map): _g(g), _vertex_map(vertex_map),
                                              _source(vertex_map[v]) {}

        edge_inserter& operator++() { return *this; }
        edge_inserter& operator++(int) { return *this; }
        edge_inserter& operator*() { return *this; }

        template <class Vertex>
        edge_inserter& operator=(const Vertex& v)
        {
            typename VertexMap::iterator iter = _vertex_map.find(*v);
            if (iter != _vertex_map.end())
            {
                typename graph_traits<Graph>::vertex_descriptor target
                    = iter->second;
                if (!is_adjacent(_source, target, _g) && _source != target)
                    add_edge(_source, target, _g);
            }
            return *this;
        }

    private:
        Graph& _g;
        VertexMap _vertex_map;
        typename graph_traits<Graph>::vertex_descriptor _source;
    };

    template <class Graph, class Points, class PosMap>
    void operator()(Graph& g, Points& points, PosMap pos) const
    {
        typedef std::unordered_map
            <typename Triang::Vertex,
             typename graph_traits<Graph>::vertex_descriptor,
             hash_point> vertex_map_t;
        vertex_map_t vertex_map;

        Triang T;
        for (size_t i = 0; i < points.shape()[0]; ++i)
        {
            typename Triang::Point p(points[i][0], points[i][1], points[i][2]);
            typename graph_traits<Graph>::vertex_descriptor v = add_vertex(g);
            vertex_map[*T.insert(p)] = v;
            pos[v].resize(3);
            for (size_t j = 0; j < 3; ++j)
                pos[v][j] = points[i][j];
        }

        typename Triang::Vertex_iterator v_begin, v_end;
        v_begin = T.vertices_begin();
        v_end = T.vertices_end();
        while (v_begin != v_end)
        {
            typename Triang::Vertex_handle v = v_begin++;
            if (vertex_map.find(*v) == vertex_map.end())
                continue;
            edge_inserter<Graph, vertex_map_t> insert(g, *v, vertex_map);
            T.incident_vertices(v, insert);
        }
    }

};

} // namespace graph_tool

#endif // GRAPH_TRIANGULATION_HH