#include "DepthDetector.h"

#include <boost/math/special_functions/fpclassify.hpp>

namespace hpe
{

    DepthDetector::DepthDetector(std::string dataFolder)
    {
        Init(dataFolder);
    }

    DepthDetector::~DepthDetector(void)
    {
    }

    hpe::headPoseInfo DepthDetector::Detect(cv::Mat depthFrame)
    {
        cv::Mat converted(depthFrame.rows, depthFrame.cols, depthFrame.type());

        for (int y = 0; y < depthFrame.rows; ++y)
        {
            for (int x = 0; x < depthFrame.cols; ++x)
            {
                float depthValue = depthFrame.at<float>(y, x);

                if (depthValue > 1.5 || depthValue == 0.0f || boost::math::isnan(depthValue))
                {
                    depthValue = 0;
                }
                else
                {
                    depthValue = depthValue * 1000;
                }

                converted.at<float>(y, x) = depthValue;
            }
        }

        headPoseInfo info = estimateHeadAngles(converted, m_cascade, m_paramList);
        return info;
    }

    void DepthDetector::Init(std::string dataFolder)
    {
        std::string configFileName = dataFolder + "/config.txt";
        loadConfig(configFileName, &m_paramList);

        std::string modelsDir = dataFolder + "/models/";
        for (int i = 0; i < 5; i++)
        {
            m_cascade.push_back(readTree(modelsDir, i + 1));
        }
    }

}


<% render(instance) %>
<%= includes((0..x).size) %>

#include <boost/hana/foldable/foldable.hpp>
#include <boost/hana/integral.hpp>

// We must make sure we always have the same seed to have reproducible results.
<% xs = (0..x).map { |i| "boost::hana::int_<#{i}>" }.shuffle(random: Random.new(0)) %>

int main() {
    auto go = boost::hana::minimum(
        <%= list(xs.map { |x| "decltype(#{x})" }, xs) %>
    );
}


/*****************************************************************************/
/*  Copyright (c) 2016, Karl Pauwels, 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 <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>
#include <stdexcept>
#include <limits>
#include <fstream>
#include <thread>

// TOFIX: find why qtcreator cannot pickup header files
//#include <multiple_rigid_models_ogre.h>
//#include <windowless_gl_context.h>
//#include <hdf5_file.h>
//#include <utilities.h>
//#include <device_1d.h>

#include <utility_kernels.h>
#include <utility_kernels_pose.h>
#include <hdf5_file.h>
#include <multiple_rigid_models_ogre.h>

#include "../../fato_rendering/include/multiple_rigid_models_ogre.h"
#include "../../fato_rendering/include/windowless_gl_context.h"
#include "../../tracker/include/feature_matcher.hpp"
#include "../../fato_rendering/include/device_1d.h"
#include "../../utilities/include/hdf5_file.h"
#include "../../utilities/include/utilities.h"
#include "../../io/include/VideoWriter.h"

// default parameters
int width = 640;
int height = 480;
double fx = 500.0;
double fy = 500.0;
double cx = width / 2.0;
double cy = height / 2.0;
double near_plane = 0.01;  // for init only
double far_plane = 10.0;   // for init only
Eigen::Matrix<double, 3, 8> bounding_box;

void getPose(float rot_x, float rot_y,
             Eigen::Translation<double, 3> &tra_center,
             Eigen::Transform<double, 3, Eigen::Affine> &t_render) {
  Eigen::Matrix3d rot_view;
  rot_view = Eigen::AngleAxisd(rot_y, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(rot_x, Eigen::Vector3d::UnitX());

  // rotate around x to 'render' just as Ogre
  Eigen::Matrix3d Rx_180 = Eigen::Matrix<double, 3, 3>::Identity();
  Rx_180(1, 1) = -1.0;
  Rx_180(2, 2) = -1.0;
  rot_view *= Rx_180;

  // apply tra_center -> rot_view to bounding box
  Eigen::Transform<double, 3, Eigen::Affine> t = rot_view * tra_center;
  Eigen::Matrix<double, 3, 8> bb;
  bb = t * bounding_box;

  // compute minimal z-shift required to ensure visibility
  // assuming cx,cy in image center
  Eigen::Matrix<double, 1, 8> shift_x =
      (2.0 * fx / (double)width) * bb.row(0).array().abs();
  shift_x -= bb.row(2);
  Eigen::Matrix<double, 1, 8> shift_y =
      (2.0 * fy / (double)height) * bb.row(1).array().abs();
  shift_y -= bb.row(2);
  Eigen::Matrix<double, 1, 16> shift;
  shift << shift_x, shift_y;
  double z_shift = shift.maxCoeff();
  Eigen::Translation<double, 3> tra_z_shift(0, 0, z_shift);

  // compute bounding box limits after z-shift
  near_plane = (bb.row(2).array() + z_shift).minCoeff();
  far_plane = (bb.row(2).array() + z_shift).maxCoeff();

  // compose render transform (tra_center -> rot_view -> tra_z_shift)
  t_render = tra_z_shift * rot_view * tra_center;
}

void renderObject(Eigen::Transform<double, 3, Eigen::Affine> &pose,
                  pose::MultipleRigidModelsOgre &model_ogre) {
  double tra_render[3];
  double rot_render[9];
  Eigen::Map<Eigen::Vector3d> tra_render_eig(tra_render);
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> rot_render_eig(
      rot_render);
  tra_render_eig = pose.translation();
  rot_render_eig = pose.rotation();

  std::vector<pose::TranslationRotation3D> TR(1);
  TR.at(0).setT(tra_render);
  TR.at(0).setR_mat(rot_render);

  model_ogre.updateProjectionMatrix(fx, fy, cx, cy, near_plane, far_plane);
  model_ogre.render(TR);
}

void extractFeature();

std::vector<float> projectPoints(
    Eigen::Transform<double, 3, Eigen::Affine> &pose,
    pose::MultipleRigidModelsOgre &model_ogre,
    std::vector<cv::KeyPoint> &points) {
  int num_features = points.size();

  // 3D keypoints (project on z-buffer)
  util::Device1D<float> d_z(height * width);
  pose::convertZbufferToZ(d_z.data(), model_ogre.getZBuffer(), width, height,
                          cx, cy, near_plane, far_plane);
  std::vector<float> h_z(height * width);
  d_z.copyTo(h_z);

  std::vector<float> key_XYZ(3 * num_features);
  for (int k = 0; k < num_features; ++k) {
    float pt_x = points.at(k).pt.x;
    float pt_y = points.at(k).pt.y;

    int x = floor(pt_x);
    x = (x < 0) ? 0 : x;
    x = (x >= width) ? (width - 1) : x;
    int y = floor(pt_y);
    y = (y < 0) ? 0 : y;
    y = (y >= height) ? (height - 1) : y;

    key_XYZ.at(3 * k + 2) = h_z.at(y * width + x);
    key_XYZ.at(3 * k + 0) = (pt_x - cx) * key_XYZ.at(3 * k + 2) / fx;
    key_XYZ.at(3 * k + 1) = (pt_y - cy) * key_XYZ.at(3 * k + 2) / fy;
  }

  // transform 3D keypoints to original 3D model coordinates
  Eigen::Map<Eigen::MatrixXf> key_XYZ_eig(key_XYZ.data(), 3, num_features);
  Eigen::MatrixXd tmp(3, num_features);
  tmp = pose.inverse() * key_XYZ_eig.cast<double>().colwise().homogeneous();
  key_XYZ_eig = tmp.cast<float>();

  // TOFIX: in the original version key_XYZ is used instead, quite strange!
  return key_XYZ;
}

void downloadRenderedImg(pose::MultipleRigidModelsOgre &model_ogre,
                         std::vector<uchar4> &h_texture) {
  util::Device1D<uchar4> d_texture(height * width);
  vision::convertFloatArrayToGrayRGBA(d_texture.data(), model_ogre.getTexture(),
                                      width, height, 1.0, 2.0);
  h_texture.resize(height * width);
  d_texture.copyTo(h_texture);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "fato_generate_model");

  // Create dummy GL context before cudaGL init
  render::WindowLessGLContext dummy(10, 10);

  // CUDA Init
  int device_id = 0;
  fato::initializeCUDARuntime(device_id);

  // viewpoint rotations to apply (Euler angles)
  // more or less uniformly distributed
  std::vector<double> rot_x{
      0.0000, 0.2618, 1.3090, 2.3562, 3.4034, 4.4506, 5.4978, 0.0000,
      0.6046, 1.2092, 1.8138, 2.4184, 3.0230, 3.6276, 4.2322, 4.8368,
      5.4414, 0.2618, 0.7854, 1.3090, 1.8326, 2.3562, 2.8798, 3.4034,
      3.9270, 4.4506, 4.9742, 5.4978, 6.0214, 0.0000, 0.6046, 1.2092,
      1.8138, 2.4184, 3.0230, 3.6276, 4.2322, 4.8368, 5.4414, 0.2618,
      1.3090, 2.3562, 3.4034, 4.4506, 5.4978, 0.0000};

  std::vector<double> rot_y{
      -1.5708, -1.0472, -1.0472, -1.0472, -1.0472, -1.0472, -1.0472, -0.5236,
      -0.5236, -0.5236, -0.5236, -0.5236, -0.5236, -0.5236, -0.5236, -0.5236,
      -0.5236, 0.0000,  0.0000,  0.0000,  0.0000,  0.0000,  0.0000,  0.0000,
      0.0000,  0.0000,  0.0000,  0.0000,  0.0000,  0.5236,  0.5236,  0.5236,
      0.5236,  0.5236,  0.5236,  0.5236,  0.5236,  0.5236,  0.5236,  1.0472,
      1.0472,  1.0472,  1.0472,  1.0472,  1.0472,  1.5708};

  std::string obj_file_name;
  float response_thresh;
  if (ros::param::get("fato/model/object_path", obj_file_name)) {
    ros::param::get("fato/model/response", response_thresh);
  } else {
    throw std::runtime_error("Cannot read the parameters required");
  }
  int descriptor_type = 0;
  if (!ros::param::get("fato/descriptor_type", descriptor_type)) {
    ROS_INFO("No descriptor type defined");
  }

  // storage
  std::vector<float> all_keypoints;
  std::vector<uchar> all_descriptors;
  int all_num_features = 0;

  // setup the engines
  pose::MultipleRigidModelsOgre model_ogre(width, height, fx, fy, cx, cy,
                                           near_plane, far_plane);
  model_ogre.addModel(obj_file_name);

  // check existence output file
  size_t ext_pos = obj_file_name.find_last_of(".");
  std::string h5_file_name = obj_file_name;
  h5_file_name.replace(ext_pos, 4, "_features.h5");

  if (boost::filesystem::exists(boost::filesystem::path(h5_file_name))) {
    char user_input = ' ';
    while ((user_input != 'y') && (user_input != 'n')) {
      std::cout << h5_file_name + " exists. Remove and overwrite? [y/n]: ";
      std::cin >> user_input;
    }
    if (user_input == 'n')
      return EXIT_SUCCESS;
    else  // remove the file
      boost::filesystem::remove(boost::filesystem::path(h5_file_name));
  }

  std::unique_ptr<fato::FeatureMatcher> matcher;
  switch (descriptor_type) {
    case 0:
      matcher.reset(new fato::BriskMatcher);
      break;
    case 1:
    {
      matcher.reset(new fato::AkazeMatcher);
      fato::AkazeMatcher* tmp_p = dynamic_cast<fato::AkazeMatcher*>(matcher.get());
      tmp_p->init(width,height);
  }
      break;
    default:
      break;
  }

  // object vertices
  const render::RigidObject &obj_model = model_ogre.getRigidObject(0);
  Eigen::Map<const Eigen::MatrixXf> vertices_f(obj_model.getPositions().data(),
                                               3, obj_model.getNPositions());
  Eigen::MatrixXd vertices;
  vertices = vertices_f.cast<double>();

  // bounding box
  Eigen::Map<const Eigen::MatrixXf> bounding_box_f(
      obj_model.getBoundingBox().data(), 3, 8);

  bounding_box = bounding_box_f.cast<double>();

  // centralizing translation
  auto mn = vertices.rowwise().minCoeff();
  auto mx = vertices.rowwise().maxCoeff();
  Eigen::Translation<double, 3> tra_center(-(mn + mx) / 2.0f);

  std::string window_name = "Model Construction";
  cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);

  int dsc_size = 0;

  // fato::VideoWriter
  // video_writer("/home/alessandro/Downloads/","model_generation.avi", 640,480,
  // 1, 30);

  cv::VideoWriter writer("/home/alessandro/Downloads/model_generation.avi",
                         CV_FOURCC('M', 'J', 'P', 'G'), 10,
                         cv::Size(width, height), 1);

  // computing min and max response for feature descriptors
  float min_response = std::numeric_limits<float>::max();
  float max_response = 0;

  bool is_init = false;
  cv::Mat all_mat;
  // process all views

  std::vector<cv::Mat> rendered_imgs;
  std::vector<std::vector<cv::KeyPoint>> img_keypoints;
  std::vector<std::vector<float>> projected_points;
  std::vector<cv::Mat> img_descriptors;

  auto elapsed = 0;

  for (int view_ind = 0; view_ind < rot_x.size(); ++view_ind) {
    // compose render transform (tra_center -> rot_view -> tra_z_shift)
    Eigen::Transform<double, 3, Eigen::Affine> t_render;
    getPose(rot_x.at(view_ind), rot_y.at(view_ind), tra_center, t_render);
    // tra_z_shift * rot_view * tra_center;

    renderObject(t_render, model_ogre);

    std::this_thread::sleep_for(std::chrono::milliseconds(10));

    std::vector<uchar4> h_texture(height * width);
    downloadRenderedImg(model_ogre, h_texture);

    cv::Mat img_rgba(height, width, CV_8UC4, h_texture.data());
    cv::Mat img_gray;
    cv::cvtColor(img_rgba, img_gray, CV_RGBA2GRAY);

    matcher->extractTarget(img_gray);
    std::vector<cv::KeyPoint> &points = matcher->getTargetPoints();
    cv::Mat &dscs = matcher->getTargetDescriptors();

    img_descriptors.push_back(dscs);

    int num_features = points.size();

    // calculating min and max response
    for (auto kp : points) {
      if (kp.response < min_response) min_response = kp.response;
      if (kp.response > max_response) max_response = kp.response;
    }

    if (!is_init && num_features > 0) {
      all_mat = dscs.clone();
      is_init = true;
    } else if (num_features > 0) {
      cv::vconcat(all_mat, dscs, all_mat);
    }

    std::vector<uchar> descriptors;

    if (num_features > 0) {
      dsc_size = dscs.cols;

      for (int i = 0; i < dscs.rows; ++i) {
        descriptors.insert(descriptors.end(), dscs.ptr<uchar>(i),
                           dscs.ptr<uchar>(i) + dscs.cols);
      }
    }

    std::vector<float> key_XYZ = projectPoints(t_render, model_ogre, points);
    projected_points.push_back(key_XYZ);

    // store
    all_keypoints.insert(all_keypoints.end(), key_XYZ.begin(), key_XYZ.end());
    all_descriptors.insert(all_descriptors.end(), descriptors.begin(),
                           descriptors.end());
    all_num_features += num_features;

    // draw keypoints
    cv::Mat img_keys;
    cv::cvtColor(img_gray, img_keys, CV_GRAY2BGR);

    rendered_imgs.push_back(img_keys);
    img_keypoints.push_back(points);
  }

  std::cout << "min response " << min_response << " max reponse "
            << max_response << std::endl;

  int valid_point_count = 0;
  int invalid_points = 0;

  std::vector<float> all_filtered_keypoints;
  std::vector<uchar> all_filtered_descriptors;

  for (auto i = 0; i < rendered_imgs.size(); ++i) {
    cv::Mat img = rendered_imgs.at(i);
    std::vector<cv::KeyPoint> &key_points = img_keypoints.at(i);
    cv::Mat &descriptors = img_descriptors.at(i);
    std::vector<float> &points = projected_points.at(i);

    for (auto j = 0; j < key_points.size(); ++j) {
      cv::KeyPoint &kp = key_points.at(j);

      bool is_valid = true;

      if (kp.response < response_thresh)
        is_valid = false;
      else
        std::cout << "response: " << kp.response << "(" << response_thresh
                  << ")" << std::endl;
      int id = 3 * j;

      if (fato::is_nan(points.at(id)) || fato::is_nan(points.at(id + 1)) ||
          fato::is_nan(points.at(id + 2))) {
        invalid_points++;
        is_valid = false;
      }

      if (is_valid) {
        all_filtered_descriptors.insert(
            all_filtered_descriptors.end(), descriptors.ptr<uchar>(j),
            descriptors.ptr<uchar>(j) + descriptors.cols);
        all_filtered_keypoints.insert(all_filtered_keypoints.end(),
                                      points.begin() + id,
                                      points.begin() + id + 3);
        valid_point_count++;
      }

      // drawing stuff kept separated from saving stuff
      if (is_valid) {
        float scaled_response = (kp.response - min_response) /
                                static_cast<float>(max_response - min_response);
        uchar b = 255 * (scaled_response);
        uchar r = 255 * (1 - scaled_response);
        cv::Scalar color(b, 0, r);
        cv::circle(img, cv::Point2d(kp.pt.x, kp.pt.y), kp.size, color, 1, 8);
        cv::circle(img, cv::Point2d(kp.pt.x, kp.pt.y), 1, color, -1, 8);
      } else {
        // cv::circle(img, cv::Point2d(kp.pt.x, kp.pt.y), 3, cv::Scalar(0, 255,
        // 0),
        //       -1, 8);
      }
    }

    cv::imshow(window_name, img);

    std::string img_name = "/home/alessandro/debug/images/model" + std::to_string(i) + ".png";
    cv::imwrite(img_name, img);

    std::ofstream file("/home/alessandro/Downloads/img.txt");

    for (int i = 0; i < img.rows; ++i) {
      for (int j = 0; j < img.cols; ++j) {
        cv::Vec3b s = img.at<cv::Vec3b>(i, j);
        file << "[" << (int)s[0] << "," << (int)s[1] << "," << (int)s[2]
             << "] ";
      }
      file << "\n";
    }

    writer.write(img);
    cv::waitKey(10);
  }

  std::cout << "Keypoints after response filtering: " << valid_point_count
            << std::endl;
  std::cout << "Invalid keypoints after response filtering: " << invalid_points
            << std::endl;
  std::cout << "Size of filtered descriptors "
            << all_filtered_descriptors.size() << std::endl;
  std::cout << "\t size: " << valid_point_count << " " << dsc_size << std::endl;
  std::cout << "Size of filtered points " << all_filtered_keypoints.size()
            << std::endl;

  // save
  util::HDF5File out_file(h5_file_name);
  std::vector<int> descriptors_size{valid_point_count, dsc_size};
  std::vector<int> positions_size{valid_point_count, 3};
  out_file.writeArray("descriptors", all_filtered_descriptors, descriptors_size,
                      true);
  out_file.writeArray("positions", all_filtered_keypoints, positions_size,
                      true);

  return EXIT_SUCCESS;
}


// All content Copyright (C) 2018 Genomics plc
#include "utils/NeedlemanWunsch.hpp"
//#include <boost/numeric/ublas/io.hpp>
#include <stack>
#include "utils/interval.hpp"

namespace wecall
{
namespace utils
{
    std::vector< NWVariant > NeedlemanWunsch::traceBack() const
    {
        std::stack< Backtrace > currentItems;

        const auto lastRefIndex = m_scoreMatrix.size1() - 1;
        const auto lastAltIndex = m_scoreMatrix.size2() - 1;
        currentItems.push(
            Backtrace{{}, lastRefIndex, lastAltIndex, m_traceMatrix( lastRefIndex, lastAltIndex ), false} );

        std::vector< std::vector< NWVariant > > allVariantCombinations;

        while ( not currentItems.empty() )
        {
            bool deadEnd = false;

            auto current = currentItems.top();
            currentItems.pop();

            while ( ( current.refIndex > 0 or current.altIndex > 0 ) and not deadEnd )
            {
                auto cache = current;
                if ( current.trace == 0 )
                {
                    // Reached the left side or the top
                    if ( current.refIndex and current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        if ( current.altIndex != 0 )
                        {
                            // insertion of the rest of the alt sequence
                            const auto alt = m_altString.substr( 0, current.altIndex );
                            current.variants.push_back( NWVariant( 0, 0, alt ) );
                        }
                        if ( current.refIndex != 0 )
                        {
                            // deletion of the rest of ref sequence
                            current.variants.push_back( NWVariant( 0, current.refIndex, "" ) );
                        }
                    }
                    break;
                }
                else if ( current.trace & m_matchBit )
                {
                    // No indel
                    current.trace ^= m_matchBit;
                    current.previousIsDeletion = false;
                    current.altIndex -= 1;
                    current.refIndex -= 1;
                }
                else if ( current.trace & m_snpBit )
                {
                    // SNP
                    current.trace ^= m_snpBit;
                    current.previousIsDeletion = false;
                    current.altIndex -= 1;
                    current.refIndex -= 1;
                    const auto alt = m_altString.substr( current.altIndex, 1 );
                    current.variants.push_back( NWVariant( current.refIndex, current.refIndex + 1, alt ) );
                }
                else if ( current.trace & m_insertSingleBit )
                {
                    // insertion of size 1
                    current.trace ^= m_insertSingleBit;
                    if ( current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        current.previousIsDeletion = false;
                        current.altIndex -= 1;
                        const auto alt = m_altString.substr( current.altIndex, 1 );
                        current.variants.push_back( NWVariant( current.refIndex, current.refIndex, alt ) );
                    }
                }
                else if ( current.trace & m_deleteSingleBit )
                {
                    // deletion of size 1
                    current.trace ^= m_deleteSingleBit;
                    current.previousIsDeletion = true;
                    current.refIndex -= 1;
                    current.variants.push_back( NWVariant( current.refIndex, current.refIndex + 1, "" ) );
                }
                else if ( current.trace & m_insertMultiBit )
                {
                    // insertion of size > 1
                    current.trace ^= m_insertMultiBit;
                    if ( current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        current.previousIsDeletion = false;
                        auto targetScore = m_scoreMatrix( current.refIndex, current.altIndex );
                        auto startingAltIndex = current.altIndex;

                        for ( std::size_t len = 2; len <= startingAltIndex; ++len )
                        {
                            current.altIndex = startingAltIndex - len;
                            auto actualScore = m_scoreMatrix( current.refIndex, current.altIndex ) +
                                               m_nWPenalties.insertionFunction( current.altIndex, len );

                            if ( targetScore == actualScore )
                            {
                                const auto alt = m_altString.substr( current.altIndex, len );
                                const auto variant = NWVariant( current.refIndex, current.refIndex, alt );
                                const auto newTrace = m_traceMatrix( current.refIndex, current.altIndex );

                                if ( newTrace == 0 )
                                {
                                    // We are at the beginning of the alt sequence, so there can't be any more starting
                                    // points that achieve the required score after this. Add the new variant to the
                                    // current variants and continue.
                                    current.variants.push_back( variant );
                                    break;
                                }
                                else
                                {
                                    // There may be more starting points earlier in the alt sequence, so we save the
                                    // information about starting at this point on the stack rather than continuing with
                                    // it straight away.
                                    auto newVariants = current.variants;
                                    newVariants.push_back( variant );
                                    currentItems.push( Backtrace{newVariants,
                                                                 current.refIndex,
                                                                 current.altIndex,
                                                                 newTrace,
                                                                 current.previousIsDeletion} );
                                }
                            }
                            if ( m_traceMatrix( current.refIndex, current.altIndex ) == 0 )
                            {
                                // If we get here then we got to the beginning of the alt sequence, and it wasn't a
                                // possible starting point for the insertion (if it was then we would have broken out of
                                // the for loop), so we shouldn't continue from the current position. We make this
                                // happen by setting deadEnd to true.
                                deadEnd = true;
                            }
                        }
                    }
                }
                else if ( current.trace & m_deleteMultiBit )
                {
                    // deletion of size > 1
                    current.trace ^= m_deleteMultiBit;
                    current.previousIsDeletion = true;

                    auto targetScore = m_scoreMatrix( current.refIndex, current.altIndex );
                    auto startingRefIndex = current.refIndex;

                    for ( std::size_t len = 2; len <= startingRefIndex; ++len )
                    {
                        current.refIndex = startingRefIndex - len;
                        auto actualScore = m_scoreMatrix( current.refIndex, current.altIndex ) +
                                           m_nWPenalties.deletionFunction( current.refIndex, len );

                        if ( targetScore == actualScore )
                        {
                            const auto variant = NWVariant( current.refIndex, startingRefIndex, "" );
                            const auto newTrace = m_traceMatrix( current.refIndex, current.altIndex );

                            if ( newTrace == 0 )
                            {
                                // We are at the beginning of the ref sequence, so there can't be any more starting
                                // points that achieve the required score after this. Add the new variant to the
                                // current variants and continue.
                                current.variants.push_back( variant );
                                break;
                            }
                            else
                            {
                                // There may be more starting points earlier in the ref sequence, so we save the
                                // information about starting at this point on the stack rather than continuing with
                                // it straight away.
                                auto newVariants = current.variants;
                                newVariants.push_back( variant );
                                currentItems.push( Backtrace{newVariants,
                                                             current.refIndex,
                                                             current.altIndex,
                                                             newTrace,
                                                             current.previousIsDeletion} );
                            }
                        }
                        if ( m_traceMatrix( current.refIndex, current.altIndex ) == 0 )
                        {
                            // If we get here then we got to the beginning of the ref sequence, and it wasn't a
                            // possible starting point for the insertion (if it was then we would have broken out of
                            // the for loop), so we shouldn't continue from the current position. We make this
                            // happen by setting deadEnd to true.
                            deadEnd = true;
                        }
                    }
                }
                if ( current.trace != 0 )
                {
                    cache.trace = current.trace;
                    currentItems.push( cache );
                }
                current.trace = m_traceMatrix( current.refIndex, current.altIndex );
            }
            if ( not deadEnd )
            {
                allVariantCombinations.push_back( current.variants );
            }
        }
        if ( allVariantCombinations.size() > 1 )
        {
            WECALL_LOG( DEBUG, "Warning: while normalizing variants we found "
                                    << allVariantCombinations.size() << " possible normalizations" << std::endl );
        }

        return allVariantCombinations.back();
    }

    int32_t NeedlemanWunsch::getScoreMatrix()
    {
        const std::size_t refDim = m_referenceString.size() + 1;
        const std::size_t altDim = m_altString.size() + 1;

        //        scoreMatrix_t m_scoreMatrix( refDim, altDim );
        //        traceMatrix_t m_traceMatrix( refDim, altDim );
        m_scoreMatrix = scoreMatrix_t( refDim, altDim );
        m_traceMatrix = traceMatrix_t( refDim, altDim );

        m_scoreMatrix( 0, 0 ) = 0;

        // Initialise the first column of the matrix
        //        for ( std::size_t rowIndex = 1; rowIndex < refDim; ++rowIndex )
        //        {
        //            m_scoreMatrix( rowIndex, 0 ) = m_nWPenalties.insertionFunction(0, rowIndex);
        //            m_traceMatrix( rowIndex, 0 ) = 0;
        //        }

        if ( refDim > 1 )
        {
            m_scoreMatrix( 1, 0 ) = m_nWPenalties.insertionOpen();
            m_traceMatrix( 1, 0 ) = 0;
        }

        for ( std::size_t rowIndex = 2; rowIndex < refDim; ++rowIndex )
        {
            m_scoreMatrix( rowIndex, 0 ) = m_scoreMatrix( rowIndex - 1, 0 ) + m_nWPenalties.extendInsertion();
            m_traceMatrix( rowIndex, 0 ) = 0;
        }

        // Initialise the first row of the matrix
        //        for ( std::size_t colIndex = 1; colIndex < refDim; ++colIndex )
        //        {
        //            m_scoreMatrix( 0, colIndex ) = m_nWPenalties.deletionFunction(0, colIndex);
        //            m_traceMatrix( 0, colIndex ) = 0;
        //        }

        if ( altDim > 1 )
        {
            m_scoreMatrix( 0, 1 ) = m_nWPenalties.deletionOpen();
            m_traceMatrix( 0, 1 ) = 0;
        }

        for ( std::size_t colIndex = 2; colIndex < altDim; ++colIndex )
        {
            m_scoreMatrix( 0, colIndex ) = m_scoreMatrix( 0, colIndex - 1 ) + m_nWPenalties.extendDeletion();
            m_traceMatrix( 0, colIndex ) = 0;
        }

        // Create the colScore matrix we will use when filling in the matrix
        // It took me a while to work out why we use these values. When we add
        std::vector< int32_t > colScore( altDim );
        // Note that colScore[0] is never used, so there's no need to initialise it
        //        for ( std::size_t colIndex = 1; colIndex < altDim; ++colIndex )
        //        {
        //            // When we add n*m_nWPenalties.gapLinear to colScore[colIndex] we want to get the score for having
        //            // an insertion of size colIndex followed by a deletion of size n
        //            colScore[colIndex] = m_nWPenalties.insertionFunction(0, colIndex) +
        //                                 m_nWPenalties.deletionFunction(0, 1) - m_nWPenalties.gapLinear;
        //        }
        if ( altDim > 1 )
        {
            colScore[1] = m_nWPenalties.insertionOpen() + m_nWPenalties.deletionOpen() - m_nWPenalties.extendDeletion();
        }

        for ( std::size_t colIndex = 2; colIndex < altDim; ++colIndex )
        {
            colScore[colIndex] = colScore[colIndex - 1] + m_nWPenalties.extendDeletion();
        }

        // Now go through the rest of the matrix, row by row, calculating the correct values
        for ( int32_t rowIndex = 1; rowIndex < static_cast< int32_t >( refDim ); ++rowIndex )
        {
            //            // When we add m_nWPenalties.gapLinear to rowScore we want to get the score for having a
            //            // deletion of size rowIndex followed by an insertion of size 1
            auto rowScore = m_nWPenalties.deletionFunction( 0, rowIndex ) + m_nWPenalties.insertionFunction( 0, 1 ) -
                            m_nWPenalties.extendInsertion();
            //            auto rowScore = 2 * m_nWPenalties.gapConstant + rowIndex * m_nWPenalties.extendDeletion();

            for ( int32_t colIndex = 1; colIndex < static_cast< int32_t >( altDim ); ++colIndex )
            {
                // Calculate score for not having a gap, i.e. sequences match or there is a SNP
                const auto refBase = m_referenceString.at( rowIndex - 1 );
                const auto altBase = m_altString.at( colIndex - 1 );
                const int32_t noGapScore =
                    m_scoreMatrix( rowIndex - 1, colIndex - 1 ) + m_nWPenalties.matchFunction( refBase, altBase );

                const auto insertSingleScore =
                    m_scoreMatrix( rowIndex, colIndex - 1 ) + m_nWPenalties.insertionFunction( colIndex - 1, 1 );
                const auto insertMultiScore = rowScore + m_nWPenalties.extendInsertion();
                rowScore = std::max( insertSingleScore, insertMultiScore );

                const auto deleteSingleScore =
                    m_scoreMatrix( rowIndex - 1, colIndex ) + m_nWPenalties.deletionFunction( rowIndex - 1, 1 );
                const auto deleteMultiScore = colScore[colIndex] + m_nWPenalties.extendDeletion();
                colScore[colIndex] = std::max( deleteSingleScore, deleteMultiScore );

                const auto bestScore =
                    std::max( {noGapScore, insertSingleScore, insertMultiScore, deleteSingleScore, deleteMultiScore} );

                m_scoreMatrix( rowIndex, colIndex ) = bestScore;

                bitType_t trace = 0;

                if ( bestScore == noGapScore )
                {
                    if ( refBase == altBase )
                    {
                        trace |= m_matchBit;
                    }
                    else
                    {
                        trace |= m_snpBit;
                    }
                }
                if ( bestScore == insertSingleScore )
                {
                    trace |= m_insertSingleBit;
                }
                if ( bestScore == deleteSingleScore )
                {
                    trace |= m_deleteSingleBit;
                }
                if ( bestScore == insertMultiScore )
                {
                    trace |= m_insertMultiBit;
                }
                if ( bestScore == deleteMultiScore )
                {
                    trace |= m_deleteMultiBit;
                }

                m_traceMatrix( rowIndex, colIndex ) = trace;
            }
        }

        return m_scoreMatrix( refDim - 1, altDim - 1 );
    }

    std::ostream & operator<<( std::ostream & out, const NWPenalties & nWPenalties )
    {
        out << "NWPenalties(" << nWPenalties.m_baseMatch << ", " << nWPenalties.m_baseMismatch << ", "
            << nWPenalties.m_insertionConstant << ", " << nWPenalties.m_insertionLinear << ", "
            << nWPenalties.m_deletionConstant << ", " << nWPenalties.m_deletionLinear << ", "
            << nWPenalties.m_indelPosition << ")";
        return out;
    }

    std::ostream & operator<<( std::ostream & out, const NWVariant & nWVariant )
    {
        out << "NWVariant(" << nWVariant.m_start << ", " << nWVariant.m_end << ", " << nWVariant.m_alt << ")";
        return out;
    }
}
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEEV_WVRVL_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEEV_WVRVL_HPP_INCLUDED

#include <nt2/linalg/functions/geev_wvrvl.hpp>
#include <boost/assert.hpp>
#include <nt2/linalg/details/lapack/declare/geev.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/scalar/max.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/linalg/details/utility/envblocksize.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/workspace.hpp>
#include <nt2/linalg/functions/details/combine.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{

  // the routines here compute only the eigenvalues and always in a complex colon vector
  //---------------------------------------------Real-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                             )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A2&w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = n;
      nt2_la_int ldvl = n;
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
    }
  };

  //---------------------------------------------Real-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = n;
      nt2_la_int ldvl = n;
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

//---------------------------------------------Complex-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = n;
      nt2_la_int ldvr = n;
      NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
     info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

  //---------------------------------------------Complex-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                           )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = n;
      nt2_la_int ldvr = n;
      NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

  //---------------------------------------------Real-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<double> wr(of_size(n, 1)),  wi(of_size(n, 1))
                                  , rvr(of_size(n, n)), rvl(of_size(n, n));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = rvl.leading_size();
      nt2_la_int ldvr = rvr.leading_size();
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , rvl.data(), &ldvl
                         , rvr.data(), &ldvr
                         , wk.main(), &wn
                         , &info);
      details::combine_eigens(wr, wi, w);
      details::combine_vects(rvr, wi, vr);
      details::combine_vects(rvl, wi, vl);
      return info;
    }
  };

  //---------------------------------------------Real-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                               (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<float> wr(of_size(n, 1)),  wi(of_size(n, 1))
                                 , rvr(of_size(n, n)), rvl(of_size(n, n));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = rvl.leading_size();
      nt2_la_int ldvr = rvr.leading_size();
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , rvl.data(), &ldvl
                         , rvr.data(), &ldvr
                         , wk.main(), &wn
                         , &info);
      details::combine_eigens(wr, wi, w);
      details::combine_vects(rvr, wi, vr);
      details::combine_vects(rvl, wi, vl);
      return info;
    }
  };

  //---------------------------------------------Complex-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
     {
       char jobvl = 'V';
       char jobvr = 'V';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       vl.resize(of_size(n, n));
       vr.resize(of_size(n, n));
       nt2_la_int ldvl = vl.leading_size();
       nt2_la_int ldvr = vr.leading_size();
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , vl.data(), &ldvl
                          , vr.data(), &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };

  //---------------------------------------------Complex-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
     {
       char jobvl = 'V';
       char jobvr = 'V';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       vl.resize(of_size(n, n));
       vr.resize(of_size(n, n));
       nt2_la_int ldvl = vl.leading_size();
       nt2_la_int ldvr = vr.leading_size();
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , vl.data(), &ldvl
                          , vr.data(), &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };
} }

#endif


//==================================================================================================
/**
  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_FUNCTION_DEFINITION_CUMMAX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_CUMMAX_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/detail/constant/maxinit.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG ( ext, cummax_
                            , (boost::dispatch::cumulative_<cummax_,max_,maxinit_>)
                            );
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, cummax_)
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::cummax_,cummax);
} }

#endif


#include "dynet/rnn.h"
#include "dynet/io-macros.h"

#include <string>
#include <cassert>
#include <vector>
#include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/vector.hpp>

#include "dynet/nodes.h"
#include "dynet/expr.h"

using namespace std;
using namespace dynet::expr;
using namespace dynet;

namespace dynet {

enum { X2H=0, H2H, HB, L2H };

RNNBuilder::~RNNBuilder() {}

void RNNBuilder::save_parameters_pretraining(const string& fname) const {
  cerr << "RNNBuilder::save_parameters_pretraining not overridden.\n";
  abort();
}

void RNNBuilder::load_parameters_pretraining(const string& fname) {
  cerr << "RNNBuilder::load_parameters_pretraining not overridden.\n";
  abort();
}

template<class Archive>
void RNNBuilder::serialize(Archive& ar, const unsigned int) {
  ar & cur;
  ar & head;
  ar & sm;
}

DYNET_SERIALIZE_IMPL(RNNBuilder)

SimpleRNNBuilder::SimpleRNNBuilder(unsigned layers,
                       unsigned input_dim,
                       unsigned hidden_dim,
                       Model& model,
                       bool support_lags) : layers(layers), lagging(support_lags) {
  unsigned layer_input_dim = input_dim;
  for (unsigned i = 0; i < layers; ++i) {
    Parameter p_x2h = model.add_parameters({hidden_dim, layer_input_dim});
    Parameter p_h2h = model.add_parameters({hidden_dim, hidden_dim});
    Parameter p_hb = model.add_parameters({hidden_dim});
    vector<Parameter> ps = {p_x2h, p_h2h, p_hb};
    if (lagging)
        ps.push_back(model.add_parameters({hidden_dim, hidden_dim}));
    params.push_back(ps);
    layer_input_dim = hidden_dim;
  }
  dropout_rate = 0.f;
}

void SimpleRNNBuilder::new_graph_impl(ComputationGraph& cg) {
  param_vars.clear();
  for (unsigned i = 0; i < layers; ++i) {
    Parameter p_x2h = params[i][X2H];
    Parameter p_h2h = params[i][H2H];
    Parameter p_hb = params[i][HB];
    Expression i_x2h =  parameter(cg,p_x2h);
    Expression i_h2h =  parameter(cg,p_h2h);
    Expression i_hb =  parameter(cg,p_hb);
    vector<Expression> vars = {i_x2h, i_h2h, i_hb};

    if (lagging) {
        Parameter p_l2h = params[i][L2H];
        Expression i_l2h =  parameter(cg,p_l2h);
        vars.push_back(i_l2h);
    }

    param_vars.push_back(vars);
  }
}

void SimpleRNNBuilder::start_new_sequence_impl(const vector<Expression>& h_0) {
  h.clear();
  h0 = h_0;
  if (h0.size()) { assert(h0.size() == layers); }
}

Expression SimpleRNNBuilder::set_h_impl(int prev, const vector<Expression>& h_new) {
  if (h_new.size()) { assert(h_new.size() == layers); }
  const unsigned t = h.size();
  h.push_back(vector<Expression>(layers));
  for (unsigned i = 0; i < layers; ++i) {
    Expression y = h_new[i];
    h[t][i] = y;
  }
  return h[t].back();
}

Expression SimpleRNNBuilder::add_input_impl(int prev, const Expression &in) {
  if(dropout_rate != 0.f)
    throw std::runtime_error("SimpleRNNBuilder doesn't support dropout yet");
  const unsigned t = h.size();
  h.push_back(vector<Expression>(layers));

  Expression x = in;

  for (unsigned i = 0; i < layers; ++i) {
    const vector<Expression>& vars = param_vars[i];

    // y <--- f(x)
    Expression y = affine_transform({vars[2], vars[0], x});

    // y <--- g(y_prev)
    if (prev == -1 && h0.size() > 0)
      y = affine_transform({y, vars[1], h0[i]});
    else if (prev >= 0)
      y = affine_transform({y, vars[1], h[prev][i]});

    // x <--- tanh(y)
    x = h[t][i] = tanh(y);
  }
  return h[t].back();
}

Expression SimpleRNNBuilder::add_auxiliary_input(const Expression &in, const Expression &aux) {
  const unsigned t = h.size();
  h.push_back(vector<Expression>(layers));

  Expression x = in;

  for (unsigned i = 0; i < layers; ++i) {
    const vector<Expression>& vars = param_vars[i];
    assert(vars.size() >= L2H + 1);

    Expression y = affine_transform({vars[HB], vars[X2H], x, vars[L2H], aux});

    if (t == 0 && h0.size() > 0)
      y = affine_transform({y, vars[H2H], h0[i]});
    else if (t >= 1)
      y = affine_transform({y, vars[H2H], h[t-1][i]});

    x = h[t][i] = tanh(y);
  }
  return h[t].back();
}

void SimpleRNNBuilder::copy(const RNNBuilder & rnn) {
  const SimpleRNNBuilder & rnn_simple = (const SimpleRNNBuilder&)rnn;
  assert(params.size() == rnn_simple.params.size());
  for(size_t i = 0; i < rnn_simple.params.size(); ++i) {
      params[i][0] = rnn_simple.params[i][0];
      params[i][1] = rnn_simple.params[i][1];
      params[i][2] = rnn_simple.params[i][2];
  }
}

void SimpleRNNBuilder::save_parameters_pretraining(const string& fname) const {
  cerr << "Writing parameters to " << fname << endl;
  ofstream of(fname);
  assert(of);
  boost::archive::binary_oarchive oa(of);
  std::string id = "SimpleRNNBuilder:params";
  oa << id;
  oa << layers;
  for (unsigned i = 0; i < layers; ++i) {
    for (auto p : params[i]) {
      oa << p.get()->values;
    }
  }
}

void SimpleRNNBuilder::load_parameters_pretraining(const string& fname) {
  cerr << "Loading parameters from " << fname << endl;
  ifstream of(fname);
  assert(of);
  boost::archive::binary_iarchive ia(of);
  std::string id;
  ia >> id;
  if (id != "SimpleRNNBuilder:params") {
    cerr << "Bad id read\n";
    abort();
  }
  unsigned l = 0;
  ia >> l;
  if (l != layers) {
    cerr << "Bad number of layers\n";
    abort();
  }
  // TODO check other dimensions
  for (unsigned i = 0; i < layers; ++i) {
    for (auto p : params[i]) {
      ia >> p.get()->values;
    }
  }
}

template<class Archive>
void SimpleRNNBuilder::serialize(Archive& ar, const unsigned int) {
  ar & boost::serialization::base_object<RNNBuilder>(*this);
  ar & params;
  ar & layers;
  ar & lagging;
}
DYNET_SERIALIZE_IMPL(SimpleRNNBuilder)

} // namespace dynet

BOOST_CLASS_EXPORT_IMPLEMENT(dynet::RNNBuilder)
BOOST_CLASS_EXPORT_IMPLEMENT(dynet::SimpleRNNBuilder)


#include <getopt.h>
#include <stdio.h>
#include <stdlib.h>

#include <boost/algorithm/string.hpp>
#include <fstream>
#include <iostream>
#include <list>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sstream>
#include <string>
#include <vector>

#include "Object.cpp"
#include "Object.h"

using namespace std;
using namespace cv;

const int FRAME_WIDTH = 640;
const int FRAME_HEIGHT = 480;
const int MAX_NUM_OBJECTS = 20;
const int MIN_OBJECT_AREA = 28 * 28;
const int MAX_OBJECT_AREA = FRAME_HEIGHT * FRAME_WIDTH / 1.5;
double PIXEL_SIZE_WIDTH = 0.0;
double PIXEL_SIZE_HEIGHT = 0.0;
double W = 2.0;
double H = 2.0 ;
bool IS_CAMERA_CALIBRATED = false;

void setPixelSize(Rect c) {
    // saves the pixel size
    if (W > H && c.width > c.height) {
        PIXEL_SIZE_WIDTH = round(c.width / W);
        PIXEL_SIZE_HEIGHT = round(c.height / H);
    } else {
        PIXEL_SIZE_WIDTH = round(c.width / H);
        PIXEL_SIZE_HEIGHT = round(c.height / W);
    }
}

tuple<int, int> get_size_to_detection_fuction(Rect c) {
    int w, h;
    if (PIXEL_SIZE_HEIGHT == 0.0 && PIXEL_SIZE_WIDTH == 0.0) {
        setPixelSize(c);
    }
    if (PIXEL_SIZE_WIDTH > PIXEL_SIZE_HEIGHT) {
        w = c.width / PIXEL_SIZE_WIDTH;
        h = c.height / PIXEL_SIZE_HEIGHT;
    } else {
        w = c.width / PIXEL_SIZE_HEIGHT;
        h = c.height / PIXEL_SIZE_WIDTH;
    }

    return {w, h};
}

bool calibrate(Rect c, Mat &cameraFeed){
    auto [value1, value2] = get_size_to_detection_fuction(c);
    if (value1 != W && value2 != H) {
        cout << "\rCalibration failed\r" << std::flush;
        //putText(cameraFeed, "Calibration is not correct", Point(10, 30), 1,2, Scalar(0, 0, 250), 2);
        IS_CAMERA_CALIBRATED = false;
        usleep(600);
    } else if  (value1 == W && value2 == H) {
        IS_CAMERA_CALIBRATED = true;
        cout << "Camera Calibrated" << endl;

    }
    return IS_CAMERA_CALIBRATED;
}

string get_size(Rect c) {
    int w, h;
    if (PIXEL_SIZE_HEIGHT == 0.0 && PIXEL_SIZE_WIDTH == 0.0) {
        setPixelSize(c);
    }
    if (PIXEL_SIZE_WIDTH > PIXEL_SIZE_HEIGHT) {
        w = c.width / PIXEL_SIZE_WIDTH;
        h = c.height / PIXEL_SIZE_HEIGHT;
    } else {
        w = c.width / PIXEL_SIZE_HEIGHT;
        h = c.height / PIXEL_SIZE_WIDTH;
    }

    string shape = "";
    if (w == h) {
        shape = "square";
    } else {
        shape = "rectangle";
    }

    return to_string(w) + "x" + to_string(h) + " - " + shape;
}

/**
 * @brief This function is used to mark on the window the detected objects
 * with a contours with color's object and a text with the color's name
 *
 * @param theObjects
 * @param frame
 * @param temp
 * @param contours
 * @param hierarchy
 */
void drawObject(vector<Object> theObjects, Mat &frame, Mat &temp, vector<vector<Point>> contours, vector<Vec4i> hierarchy, Rect c) {
    for (int i = 0; i < theObjects.size(); i++) {
        cv::drawContours(frame, contours, i, theObjects.at(i).getColor(), 3, 8, hierarchy);
        cv::putText(frame, theObjects.at(i).getType() + " > " + get_size(c), cv::Point(theObjects.at(i).getXPos(), theObjects.at(i).getYPos() - 20), 1, 2, theObjects.at(i).getColor());
    }
}

/**
 *
 * @deprecated - Instead of this, it's better calculating the shape on fucntion get_size each time that a object is drawned (this word exists? idk).
 * @brief This function is used to return the object's shape
 *
 * @param c
 * @return string
 */
/*string returnShapeName(Rect c) {
    cout << c.width << " " << c.height << endl;
    cout << endl;
    if (lround(c.width) > lround(c.height)) {
        return "Rectangle";
    } else if (lround(c.width) == lround(c.height)) {
        return "Square";
    } else {
        return "Circle";
    }
}
*/

/**
 * @brief This function is used to erode and dilate original image
 * The objetive with this two functions is to remove noise mainly, and isolate the object
 * https://docs.opencv.org/4.x/db/df6/tutorial_erosion_dilatation.html
 *
 * @param thresh
 */
void morphologicalOperations(Mat &thresh) {
    Mat erodeElement = getStructuringElement(MORPH_RECT, Size(3, 3));

    // Apply the erosion operation
    erode(thresh, thresh, erodeElement);
    erode(thresh, thresh, erodeElement);

    Mat dilateElement = getStructuringElement(MORPH_RECT, Size(9, 9));
    dilate(thresh, thresh, dilateElement);
    dilate(thresh, thresh, dilateElement);
    dilate(thresh, thresh, dilateElement);
}

/**
 * @brief This function is used to track the object throw the image (video capture)
 * ! ---------------------------------------------------------------------------
 * ! Some parts of it are not well implemented, we need to take a loot after...
 * ! ----------------------------------------------------------------------------
 * @param theObject
 * @param threshold
 * @param HSV
 * @param cameraFeed
 */
void trackFilteredObject(Object theObject, Mat threshold, Mat HSV, Mat &cameraFeed) {
    // TODO: see if the contours problem have something related with this function
    vector<Object> objects;
    Mat temp;
    threshold.copyTo(temp);
    // declare the following vectors to use in findContours as output
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(temp, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);
    double refArea = 0;
    bool objectFound = false;
    double area = 0;
    cv::Rect c;
    vector<vector<Point>> hull(contours.size());
    if (hierarchy.size() > 0) {
        int numObjects = hierarchy.size();
        //! if numObjects > MAX_NUM_OBJECTS probably there are much noise in the image
        //! find a way to resolve that situation
        //* https://livecodestream.dev/post/object-tracking-with-opencv/
        //* https://www.pyimagesearch.com/2018/07/30/opencv-object-tracking/
        if (numObjects < MAX_NUM_OBJECTS) {
            for (int index = 0; index >= 0; index = hierarchy[index][0]) {
                // use moments method to find our filtered object
                cv::convexHull(cv::Mat(contours[index]), hull[index], false);
                Moments moment = moments((cv::Mat)contours[index]);
                c = cv::boundingRect((cv::Mat)contours[index]);
                area = moment.m00;
                if (area > MIN_OBJECT_AREA) {
                    Object object;
                    object.setXPos(moment.m10 / area);
                    object.setYPos(moment.m01 / area);
                    object.setType(theObject.getType());
                    object.setColor(theObject.getColor());
                    objects.push_back(object);
                    objectFound = true;
                } else
                    objectFound = false;
            }
            // if object found, draw the contours around it
            if (objectFound == true) {
                // draw object location on screen
                drawObject(objects, cameraFeed, temp, contours, hierarchy, c);
                if (IS_CAMERA_CALIBRATED == false) {
                    calibrate(c, cameraFeed);
                }
                if (IS_CAMERA_CALIBRATED) {
                    putText(cameraFeed, "......", Point(60, 50), 1, 2, Scalar(0, 128, 0), 6);

                } else {
                    putText(cameraFeed, "......" , Point(60, 50), 1, 2, Scalar(0, 0, 255), 6);
                }
            }
        } else
            putText(cameraFeed, "A LOT OF OBJECTS ON IMAGE", Point(0, 50), 1, 2, Scalar(0, 0, 255), 2);
    }
}

void updateCalibrationVariables() {
    string filename = "../../output/piece_size.txt";
    ifstream read(filename.c_str());
    read >> W >> H;
    read.close();
}

void detectInRealTime(vector<Object> objects) {
    vector<Object> new_objects = objects;
    Mat cameraFeed, threshold, HSV, blur;
    VideoCapture capture;

    updateCalibrationVariables(); // update the calibration variables

    capture.open(0);
    // infinite loop, searching for the objects and displaying the result in the window named "Image"
    while (1) {
        // store image on cameraFeed
        capture.read(cameraFeed);

        GaussianBlur(cameraFeed, blur, Size(5, 5), 0, 0);

        // convert BGR to HSV colorspace
        cvtColor(blur, HSV, COLOR_BGR2HSV);

        for (Object obj : new_objects) {
            inRange(HSV, obj.getHSVmin(), obj.getHSVmax(), threshold);
            morphologicalOperations(threshold);
            trackFilteredObject(obj, threshold, HSV, cameraFeed);
        }
        imshow("Image", cameraFeed);

        waitKey(80);  // taxa de refresh da camara, > menor taxa, < maior taxa
    }
}

void detectInRealTime() {
    Object blue("blue"), yellow("yellow"), red("red"), green("green");
    Object new_objects[] = {Object("blue"), Object("yellow"), Object("red"), Object("green")};
    Mat cameraFeed, threshold, HSV, blur;
    VideoCapture capture;

    updateCalibrationVariables(); // update the calibration variables

    capture.open(0);
    // infinite loop, searching for the objects and displaying the result in the window named "Image"
    while (1) {
        // store image on cameraFeed
        capture.read(cameraFeed);

        GaussianBlur(cameraFeed, blur, Size(5, 5), 0, 0);

        // convert BGR to HSV colorspace
        cvtColor(blur, HSV, COLOR_BGR2HSV);

        for (Object obj : new_objects) {
            inRange(HSV, obj.getHSVmin(), obj.getHSVmax(), threshold);
            morphologicalOperations(threshold);
            trackFilteredObject(obj, threshold, HSV, cameraFeed);
        }
        imshow("Image", cameraFeed);
        waitKey(80);  // taxa de refresh da camara, > menor taxa, < maior taxa
    }
}

void on_mouse_click(int event, int x, int y, int flags, void *ptr) {
    if (event == cv::EVENT_LBUTTONDOWN) {
        cv::Mat *snapshot = (cv::Mat *)ptr;
        cv::Mat &img = *((cv::Mat *)(ptr));
        cv::Vec3b pixel = snapshot->at<cv::Vec3b>(x, y);
        cv::Vec3b bgrPixel(snapshot->at<cv::Vec3b>(y, x));
        circle(*snapshot, cv::Point(x, y), 4, cv::Scalar(0, 0, 255), cv::FILLED, 8, 0);
        cv::imshow("Capture", *snapshot);
        int b, g, r;
        b = pixel[0];
        g = pixel[1];
        r = pixel[2];

        cv::Mat3b bgr(bgrPixel);
        cv::Mat3b hsv;
        cv::Mat3b rgb;
        cv::cvtColor(bgr, hsv, cv::COLOR_BGR2HSV);
        cv::cvtColor(bgr, rgb, cv::COLOR_BGR2RGB);

        cv::Vec3b hsvPixel(hsv.at<cv::Vec3b>(0, 0));
        cv::Vec3b rgbPixel(rgb.at<cv::Vec3b>(0, 0));

        std::string rgbText = "[" + std::to_string(r) + ", " + std::to_string(g) + ", " + std::to_string(b) + "]";
        cout << "rgb " << rgbPixel << std::endl;
        cout << "hsv " << hsvPixel << endl;

        int thresh = 20;
        cv::Scalar minHSV = cv::Scalar(hsvPixel.val[0] - thresh, hsvPixel.val[1] - thresh, hsvPixel.val[2] - thresh);
        cv::Scalar maxHSV = cv::Scalar(hsvPixel.val[0] + thresh, hsvPixel.val[1] + thresh, hsvPixel.val[2] + thresh);

        cv::Scalar rgbColor = cv::Scalar(rgbPixel.val[0], rgbPixel.val[1], rgbPixel.val[2]);

        string colour_name;
        cout << "Colour Name? ";
        cin >> colour_name;
        string filename("../../output/color_file.txt");
        ofstream out(filename.c_str(), fstream::app);  // append mode on
        Object ob1(colour_name, minHSV, maxHSV, rgbColor);
        out << ob1;
    }
}

vector<string> split(const string &s, char delim) {
    vector<string> result;
    stringstream ss(s);
    string item;
    while (getline(ss, item, delim)) {
        result.push_back(item);
    }
    return result;
}

vector<Object> readFileCreateColous() {
    vector<Object> objects;
    ifstream file;
    file.open("../../output/color_file.txt");
    string line;
    vector<string> result;
    cout << "Reading file..." << endl;
    string colour_name = "";
    while (getline(file, line)) {
        boost::split(result, line, boost::is_any_of(" - "));

        cv::Scalar hsvMin;
        cv::Scalar hsvMax;
        cv::Scalar rgbColor;
        int r, g, b;

        for (int i = 0; i < result.size(); i++) {
            boost::replace_all(result[i], "[", "");
            boost::replace_all(result[i], "]", "");
            boost::replace_all(result[i], ",", "");

            colour_name = result[0];
            stringstream ss;
            int hmin, smin, vmin;
            int hmax, smax, vmax;

            ss << result[3];
            //cout << "result 4: hmin -- " << result[3] << endl;
            ss >> hmin;
            ss.clear();
            ss << result[4];
            //cout << "result 5: smin -- " << result[4] << endl;
            ss >> smin;
            ss.clear();
            ss << result[5];
            //cout << "result 6: vmin -- " << result[5] << endl;
            ss >> vmin;
            ss.clear();
            ss << result[9];
            ss >> hmax;
            ss.clear();
            ss << result[10];
            ss >> smax;
            ss.clear();
            ss << result[11];
            ss >> vmax;
            ss.clear();
            boost::replace_all(result[15], "[", "");
            boost::replace_all(result[15], "]", "");
            boost::replace_all(result[15], ",", "");
            ss << result[15];
            ss >> r;
            ss.clear();
            boost::replace_all(result[16], "[", "");
            boost::replace_all(result[16], "]", "");
            boost::replace_all(result[16], ",", "");
            ss << result[16];
            ss >> g;
            ss.clear();
            boost::replace_all(result[17], "[", "");
            boost::replace_all(result[17], "]", "");
            boost::replace_all(result[17], ",", "");
            ss << result[17];
            ss >> b;
            ss.clear();

            hsvMin = cv::Scalar(hmin, smin, vmin);
            hsvMax = cv::Scalar(hmax, smax, vmax);
            rgbColor = cv::Scalar(b, g, r);
        }
        Object ob(colour_name, hsvMin, hsvMax, rgbColor);
        objects.push_back(ob);
    }
    file.close();
    return objects;
}

void selectColoursWithMouse() {
    cv::VideoCapture capture(0);

    if (!capture.isOpened()) {
        std::cout << "Error opening VideoCapture." << std::endl;
    }

    cv::Mat frame, snapshot;
    capture.read(frame);

    snapshot = cv::Mat(frame.size(), CV_8UC3, cv::Scalar(23, 32, 32));
    cv::imshow("Capture", snapshot);

    cv::setMouseCallback("Capture", on_mouse_click, &snapshot);

    int keyVal;
    while (1) {
        if (!capture.read(frame)) {
            break;
        }
        cv::imshow("Video", frame);

        keyVal = cv::waitKey(1) & 0xFF;
        if (keyVal == 113 || keyVal == 81) {  // q
            destroyAllWindows();
            capture.release();
            vector<Object> new_objects = readFileCreateColous();
            detectInRealTime(new_objects);
            break;
        } else if (keyVal == 115 || keyVal == 83) {  // S ou s
            snapshot = frame.clone();
            cv::imshow("Capture", snapshot);
        }
    }
}

void saveCalibrationOnFile() {
    cout << "Calibrating Camera..." << endl;
    cout << "Please, place a piece bellow the camera!" << endl;
    cout << "Type piece's width: ";
    cin >> W;
    cout << "Type piece's height: ";
    cin >> H;
    string filename = "../../output/piece_size.txt";
    ofstream out(filename.c_str());
    out << W << " " << H;
    out.close();
}

int main(int argc, char *argv[]) {
    int c;
    int digit_optind = 0;
    int option_index = 0;

    struct option long_options[] = {
        {"calibrate", required_argument, 0, 'c'},
        {"add_colors", no_argument, 0, 'a'},
        {"exit", no_argument, 0, 'e'}};

    while ((c = getopt_long(argc, argv, "caen", long_options, &option_index)) != -1) {
        int this_option_optind = optind ? optind : 1;
        switch (c) {
            case 'c':
                //add the width and height of a "master" piece to calibrate the camera
                saveCalibrationOnFile();
                break;
            case 'a':
                cout << "Add colours..." << endl;
                selectColoursWithMouse();
                break;
            case 'e':
                detectInRealTime(readFileCreateColous());
                cout << "Exiting..." << endl;
                exit(EXIT_SUCCESS);
            case 'n':
                cout << "Detect LEGO pieces in Real Time" << endl;
                detectInRealTime();
                break;
            default:
                cout << "USAGE: " << endl;
        }
    }
}