// Copyright 2021 Reda Vince

#include "mapf_environment/pathfinder.h"
#include <Eigen/CXX11/Tensor>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <vector>

Node::Node(): parent(nullptr) {}

int Node::distance(const Node& other)
{
    int dist_x = std::abs(other.x - x);
    int dist_y = std::abs(other.y - y);
    if (dist_x > dist_y)
        return dist_y * 14 + (dist_x - dist_y) * 10;
    return dist_x * 14 + (dist_y - dist_x) * 10;
}

AStar::AStar(cv::Mat _map, bool _diag, bool _init_all)
{
    grid_map = _map;
    diag     = _diag;
    init_all = _init_all;

    if (init_all)
    {
        // auto time_start = std::chrono::high_resolution_clock::now();
        int n_paths = 0;

        paths.resize(grid_map.size().width, grid_map.size().height,
            grid_map.size().width, grid_map.size().height);

        for (int start_x=0; start_x < grid_map.size().width; start_x++)
        {
            for (int start_y=0; start_y < grid_map.size().height; start_y++)
            {
                if (static_cast<int>(grid_map.at<unsigned char>(start_y, start_x)) < 255/2)
                    continue;

                for (int goal_x=0; goal_x < grid_map.size().width; goal_x++)
                {
                    for (int goal_y=0; goal_y < grid_map.size().height; goal_y++)
                    {
                        if ((start_x == goal_x) and (start_y == goal_y))
                            continue;

                        if (static_cast<int>(grid_map.at<unsigned char>(goal_y, goal_x)) < 255/2)
                            continue;

                        // check if path has already been calculated in the other direction
                        t_path route;
                        float dist;
                        std::tie(route, dist) = paths(goal_x, goal_y, start_x, start_y);
                        if (route.size() > 0)
                        {
                            std::reverse(route.begin(), route.end());
                            paths(start_x, start_y, goal_x, goal_y) =
                                std::make_tuple(route, dist);
                        }
                        else
                            // calculate route
                            paths(start_x, start_y, goal_x, goal_y)
                                = find_path(start_x, start_y, goal_x, goal_y);

                        n_paths++;
                    }
                }
            }
        }
        // auto time_end  = std::chrono::high_resolution_clock::now();
        // auto time_diff = time_end - time_start;
        // float secs = static_cast<std::chrono::duration<float, std::milli>>(time_diff).count() / 1e3;
        // std::cout << "Global path planning calculated " << n_paths << " paths, in " <<
        //     secs << " s" << std::endl;
    }
}

std::tuple<t_path, float> AStar::find(int start_x, int start_y, int goal_x, int goal_y) const
{
    assert(static_cast<int>(grid_map.at<unsigned char>(start_y, start_x)) > 255/2);
    assert(static_cast<int>(grid_map.at<unsigned char>(goal_y, goal_x)) > 255/2);

    if (((start_x == goal_x) and (start_y == goal_y)))
        return std::make_tuple(t_path(), 0.);

    if (init_all)
        return paths(start_x, start_y, goal_x, goal_y);
    return find_path(start_x, start_y, goal_x, goal_y);
}

std::tuple<t_path, float> AStar::find_path(int start_x, int start_y, int goal_x, int goal_y) const
{
    assert(not (static_cast<int>(grid_map.at<unsigned char>(start_y, start_x)) < 255/2));
    assert(not (static_cast<int>(grid_map.at<unsigned char>(goal_y, goal_x)) < 255/2));
    assert(not ((start_x == goal_x) and (start_y == goal_y)));

    // create grid of nodes
    Eigen::Tensor<Node, 2> nodes;
    nodes.resize(grid_map.size().width, grid_map.size().height);

    // create opened and closed containers
    std::vector<Node*> opened, closed;

    // create start and goal node
    Node* const start = &nodes(start_x, start_y);
    Node* const goal = &nodes(goal_x, goal_y);

    start->x = start_x;
    start->y = start_y;
    start->g_cost = 0;
    start->h_cost = start->distance(*goal);
    start->f_cost = start->h_cost;
    opened.push_back(start);

    goal->x = goal_x;
    goal->y = goal_y;

    Node* current = start;

    for (int loop=0; loop < 1000; loop++)
    {
        current = opened[0];
        for (auto node : opened)
        {
            if (node->f_cost < current->f_cost or
                    (node->f_cost == current->f_cost and node->h_cost < current->h_cost))
                current = node;
        }

        opened.erase(std::find(opened.begin(), opened.end(), current));
        closed.push_back(current);

        if (current == goal)
            break;

        // foreach neighbour
        auto neighbours = get_neighbours(nodes, current);
        for (auto neig : neighbours)
        {
            if (static_cast<int>(grid_map.at<unsigned char>(neig->y, neig->x) < 255/2))
                continue;

            if (std::find(closed.begin(), closed.end(), neig) != closed.end())
                continue;

            int new_cost = current->g_cost + current->distance(*neig);
            if ((std::find(opened.begin(), opened.end(), neig) == opened.end()) or
                    new_cost < neig->g_cost)
            {
                neig->g_cost = new_cost;
                neig->h_cost = neig->distance(*goal);
                neig->f_cost = neig->g_cost + neig->h_cost;
                neig->parent = current;

                if (std::find(opened.begin(), opened.end(), neig) == opened.end())
                    opened.push_back(neig);
            }
        }
    }

    std::vector<std::tuple<int, int>> p_vec;
    current = goal;
    while (true)
    {
        auto pos = std::make_tuple(current->x, current->y);
        p_vec.push_back(pos);

        if (current->parent == nullptr)
            break;
        current = current->parent;
        if (current == start)
            break;
    }

    std::reverse(p_vec.begin(), p_vec.end());
    return std::make_tuple(p_vec, goal->f_cost / 10.);
}

std::vector<Node*> AStar::get_neighbours(Eigen::Tensor<Node, 2> &grid, const Node* node) const
{
    std::vector<Node*> neighbours;

    for (int i=-1; i < 2; i++)
    {
        for (int j=-1; j < 2; j++)
        {
            if (i == 0 and j == 0)  // center, current node
                continue;

            if (not diag and ((i == -1 and j == -1) or
                              (i == -1 and j == 1) or
                              (i == 1  and j == -1) or
                              (i == 1  and j == 1)))
                continue;

            int neig_x = node->x + i;
            int neig_y = node->y + j;
            if ((neig_x < 0) or (neig_x >= grid_map.size().width))
                continue;
            if ((neig_y < 0) or (neig_y >= grid_map.size().height))
                continue;

            Node* neig = &grid(neig_x, neig_y);
            neig->x = neig_x;
            neig->y = neig_y;

            neighbours.push_back(neig);
        }
    }

    return neighbours;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FMA_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FMA_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/fma.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
// TO DO define a proper fma policy

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fma_, tag::cpu_,
                      (A0)(X),
                      ((simd_<arithmetic_<A0>,X>))
                      ((simd_<arithmetic_<A0>,X>))
                      ((simd_<arithmetic_<A0>,X>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return a0*a1+a2;
    }
  };
} } }
#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_REALSQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REALSQRT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing realsqrt capabilities

    Returns sqrt function, but asserts
    if called with non real positive values.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = realsqrt(x);
    @endcode

    is similar to:

    @code
    T r = sqrt(x);
    @endcode

    @see sqrt

  **/
  const boost::dispatch::functor<tag::realsqrt_> realsqrt = {};
} }
#endif

#include <boost/simd/function/scalar/realsqrt.hpp>
#include <boost/simd/function/simd/realsqrt.hpp>

#endif


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <limits>
#include <chrono>
#include <filesystem>

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include <pcl/common/common_headers.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>

#include <opencv2/core/eigen.hpp>

#include "tapl.hpp"

inline double deg2rad(double deg) { return deg * M_PI / 180.0; }
inline double rad2deg(double rad) { return rad * 180.0 / M_PI; }

int main(int argc, const char *argv[])
{
    ///////////////////////////////////////
    // Load Images
    ///////////////////////////////////////
    // paths
    std::string calibPath = "../data/calib/statue_camera_model.yaml";
    std::string dataPath = "../data/statue";

    // number of images
    int nImages = -1;   // last file index to load

    // images
    std::vector<cv::Mat> imgs;

    // Read camera calibration
    cv::FileStorage opencv_file(calibPath, cv::FileStorage::READ);
    cv::Mat camera_matrix;
    opencv_file["camera_matrix"] >> camera_matrix;
    cv::Mat dist_coeff;
    opencv_file["dist_coeff"] >> dist_coeff;
    opencv_file.release();

    namespace fs = std::filesystem;
    std::vector<std::string> fnames;
    /* Loop over all the images */
    for (auto& p: fs::directory_iterator(dataPath)) {
        fnames.push_back(p.path());
    }
    std::sort(fnames.begin(), fnames.end());

    int imgIndex = 0;
    for (auto it=fnames.begin(); it!=fnames.end(); ++it) {
        // load image from file, undistort, and convert to rgb
        cv::Mat img, img_undistorted, img_gray;
        img = cv::imread(*it);
        cv::undistort(img, img_undistorted, camera_matrix, dist_coeff);
        cv::cvtColor(img_undistorted, img_undistorted, cv::COLOR_BGR2RGB);

        // push image into data frame buffer
        imgs.push_back(img_undistorted);
        imgIndex++;
        if((nImages != -1) && (imgIndex >= nImages)) break;
    }

    ///////////////////////////////////////
    // Structure-from-Motion
    ///////////////////////////////////////
    // create an sfm object
    auto sfm = tapl::cve::StructureFromMotion(imgs, camera_matrix);
    // compute structure-from-motion
    std::vector<tapl::Point3dColor> points;
    std::vector<tapl::Pose6dof> poses;
    std::vector<tapl::CameraPairs> framePairs;
    if (sfm.process(points, poses, framePairs) != tapl::SUCCESS) {
        TLOG_ERROR << "could not compute structure-from-motion";
        exit(1);
    }

    ///////////////////////////////////////
    // Visualization
    ///////////////////////////////////////
    // prepare a viewer
    tapl::viz::Visualizer * visualizer = new tapl::viz::Visualizer(0,0,0,tapl::viz::FPS,2);
    // add reference pose at the origin
    Eigen::Affine3f affinePose = Eigen::Affine3f::Identity();
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    // rotate from world to camera coordinate
    double Rx, Ry, Rz;
    Rx = deg2rad(-90);
    Ry = deg2rad(0);
    Rz = deg2rad(-90);
    Eigen::Matrix3f R = ( Eigen::AngleAxis<float>(Rz, Eigen::Vector3f::UnitZ()) *
                          Eigen::AngleAxis<float>(Ry, Eigen::Vector3f::UnitY()) *
                          Eigen::AngleAxis<float>(Rx, Eigen::Vector3f::UnitX()))
                            .toRotationMatrix();
    Eigen::Matrix<float, 3, 1> t(0.0, 0.0, 0.0);
    globalPose.block(0, 0, 3, 3) = R;
    globalPose.block(0, 3, 3, 1) = t;
    Eigen::Matrix4f cam2PclT = globalPose.matrix();

    // render reference pose
    affinePose = cam2PclT * affinePose.matrix();
    visualizer->renderPose(0.25, affinePose, "ref_pose");
    pcl::PointXYZ origin_pt = pcl::PointXYZ(0,0,0);
    visualizer->renderSphere(origin_pt, 0.1, 1.0, 0.0, 0.0, "ref_sphere");
    pcl::PointXYZ prev_point = pcl::PointXYZ(0.0,0.0,0.0);

    // build point-cloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud->points.resize (points.size());

    for(int i = 0; i < points.size(); i++) {
        pcl::PointXYZRGB &point = cloud->points[i];
        point.x = points[i].x;
        point.y = points[i].y;
        point.z = points[i].z;
        point.r = points[i].r;
        point.g = points[i].g;
        point.b = points[i].b;
    }

    // convert to PCL's coordinate frame
    pcl::transformPointCloud (*cloud, *cloud, cam2PclT);
    visualizer->renderPointCloud<pcl::PointXYZRGB> (cloud, 5.0, -1.0, -1.0, -1.0, "cloud");

    // go through each pose and render
    uint16_t frameIdx = 0;
    for (auto &pose : poses) {
        // Eigen::Matrix4f eigenPose;
        cv::cv2eigen(pose.T, globalPose);
        affinePose = cam2PclT * globalPose.matrix();

        // render pose
        visualizer->renderPose(0.25, affinePose, "pose_" + std::to_string(frameIdx));
        pcl::PointXYZ cam_pose_pt = pcl::PointXYZ(affinePose(0, 3), affinePose(1, 3), affinePose(2, 3));
        visualizer->renderSphere(cam_pose_pt, 0.1, 1.0, 0.0, 0.0, "sphere_"+std::to_string(frameIdx));
        visualizer->renderLine(prev_point, cam_pose_pt, 0.0, 0.0, 1.0, "line_"+std::to_string(frameIdx));
        prev_point = cam_pose_pt;
        visualizer->renderScene(10);

        // render 2D keypoints and triangulated 3D keypoints
        std::vector<tapl::Point2d> trackedKpts;
        if (framePairs[frameIdx].getTrackedKpts(trackedKpts) != tapl::SUCCESS) {
            TLOG_ERROR << "could not find tracked keypoints";
            exit(1);
        }
        std::vector<tapl::Point3dColor> triangulatedPts;
        if (framePairs[frameIdx].getTriangulatedPoints(triangulatedPts) != tapl::SUCCESS) {
            TLOG_ERROR << "could not find triangulated 3D points";
            exit(1);
        }
        cv::Mat imgViz;
        if (framePairs[frameIdx].first->getImage(imgViz) != tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve image";
            exit(1);
        }
        // blue points are 2D, red points are 2D projections of triangulated 3D points
        cv::cvtColor(imgViz, imgViz, cv::COLOR_RGB2BGR);
        imgViz = tapl::cve::drawKeypoints(imgViz, trackedKpts, cv::Scalar(255,0,0));
        imgViz = tapl::cve::drawKeypoints3D(imgViz, triangulatedPts, camera_matrix, cv::Scalar(0,0,255));
        cv::imshow("frame_"+std::to_string(frameIdx), imgViz);
        cv::waitKey(1);
        frameIdx ++;

    }
    // // write to file
    // pcl::io::savePLYFileBinary("sfm.ply", *cloud);
    cv::waitKey(0);
    visualizer->renderSceneAndHold();

    // return
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_BESSEL_FUNCTIONS_SCALAR_J1_HPP_INCLUDED
#define NT2_BESSEL_FUNCTIONS_SCALAR_J1_HPP_INCLUDED
#include <nt2/bessel/functions/j1.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/digits.hpp>

#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/cos.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/functions/scalar/is_less.hpp>
#include <nt2/include/functions/scalar/is_greater.hpp>
#include <nt2/include/functions/scalar/if_else.hpp>
#include <nt2/include/functions/scalar/splat.hpp>
#include <boost/simd/sdk/config.hpp>

#include <boost/simd/sdk/math.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j1_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::j1(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j1_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_ltz(a0)) return Nan<A0>();
#ifndef BOOST_SIMD_NO_INVALIDS
      if(nt2::is_inf(a0)) return Zero<A0>();
#endif
      if (is_eqz(a0)) return Zero<A0>();
    #if defined(BOOST_SIMD_HAS__J1)
      return ::_j1(a0);
    #elif defined(BOOST_SIMD_HAS_J1)
      return ::j1(a0);
    #else
      #error j1 not supported
    #endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j1_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_ltz(a0)) return Nan<A0>();
#ifndef BOOST_SIMD_NO_INVALIDS
      if(nt2::is_inf(a0)) return Zero<A0>();
#endif
      if (is_eqz(a0)) return Zero<A0>();
      A0 x = nt2::abs(a0);
      if (lt(x,Two<A0>()))
      {
        A0 z = sqr(x);
        return (z-single_constant<A0,0x416ae95a> ())*x*
          horner< NT2_HORNER_COEFF_T(A0, 5,
                             (0xb1a7a246,
                              0x35214df5,
                              0xb83e7a4f,
                              0x3afdefd1,
                              0xbd0b7da6
                              ) ) > (z);
        }
      A0 q = rec(x);
      A0 w = nt2::sqrt(q);
      A0 p3 = w *
        horner< NT2_HORNER_COEFF_T(A0, 8,
                           (0x3d8d98f9,
                            0xbe69f6b3,
                            0x3ea0ad85,
                            0xbe574699,
                            0x3bb21b25,
                            0x3e18ec50,
                            0x36a6f7c5,
                            0x3f4c4229
                            ) ) > (q);
          w = sqr(q);
          A0 xn =  q*
          horner< NT2_HORNER_COEFF_T(A0, 8,
                         (0xc233e16d,
                        0x424af04a,
                        0xc1c6dca7,
                        0x40e72299,
                        0xbfc5bd69,
                        0x3eb364d9,
                        0xbe27bad7,
                        0x3ebfffdd
                        ) ) > (w)-single_constant<A0,0x4016cbe4> ();
          return p3*nt2::cos(xn+x);
    }
  };
} }

#endif


#pragma once

#include <armadillo>

#include "constants.hpp"
#include "equationofstate/equationofstatebase.hpp"

/*!
 * \brief The IdealGas class implements the equation of state for an ideal gas.
 */
class IdealGas : public EquationOfStateBase
{
public:
    /*!
     * \brief IdealGas constructor. Composition is only used to determine molar
     * mass (and density) of the gas.
     */
    explicit IdealGas(const Composition&) {}

    /*!
     * \brief Override. Independent of pressure and temperature.
     *
     * The compressibility is 1, derivatives are zero, and the heat capacities
     * are the heat capacities of an ideal monoatomic gas.
     *
     * \return Z = 1, derivatives = 0 and \f$c_p\f$ and \f$c_v\f$ of an ideal monoatomic gas.
     */
    virtual arma::vec evaluate(const double, const double) const override
    {
        arma::vec Z = arma::zeros<arma::vec>(6);
        Z(0) = 1.0; // Z factor
        Z(1) = 0.0; // derivatives are zero
        Z(2) = 0.0; // derivatives are zero
        Z(3) = 0.0; // derivatives are zero
        Z(4) = 5/2*constants::gasConstant; // c_p of ideal monoatomic gas
        Z(5) = 3/2*constants::gasConstant; // c_v of ideal monoatomic gas
        return Z;
    }

    /*!
     * \brief Override. Independent of pressure and temperature.
     * \return Always returns 1.0 (ideal gas).
     */
    virtual double calculateCompressibility(const double, const double) const override
    {
        return 1.0;
    }
};


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 1, 5> test_long_1_5(const Eigen::Matrix<boost::int64_t, 1, 5> & M)
{
	return M;
}
void export_long_1_5()
{
	boost::python::def("test_long_1_5",test_long_1_5);
}


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

 * 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   Matrix.cpp
 * @brief  matrix class
 * @author Christian Potthast
 */

#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h>

#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tokenizer.hpp>

#include <cstdarg>
#include <cstring>
#include <iomanip>
#include <list>
#include <fstream>
#include <limits>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Matrix zeros( size_t m, size_t n ) {
  return Matrix::Zero(m,n);
}

/* ************************************************************************* */
Matrix ones( size_t m, size_t n ) {
  return Matrix::Ones(m,n);
}

/* ************************************************************************* */
Matrix eye( size_t m, size_t n) {
  return Matrix::Identity(m, n);
}

/* ************************************************************************* */
Matrix diag(const Vector& v) {
  return v.asDiagonal();
}

/* ************************************************************************* */
bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) {

  if (equal_with_abs_tol(expected,actual,tol)) return true;

  size_t n1 = expected.cols(), m1 = expected.rows();
  size_t n2 = actual.cols(), m2 = actual.rows();

  cout << "not equal:" << endl;
  print(expected,"expected = ");
  print(actual,"actual = ");
  if(m1!=m2 || n1!=n2)
    cout << m1 << "," << n1 << " != " << m2 << "," << n2 << endl;
  else {
    Matrix diff = actual-expected;
    print(diff, "actual - expected = ");
  }
  return false;
}

/* ************************************************************************* */
bool assert_inequal(const Matrix& A, const Matrix& B, double tol) {
  if (!equal_with_abs_tol(A,B,tol)) return true;
  cout << "Erroneously equal:" << endl;
  print(A, "A = ");
  print(B, "B = ");
  return false;
}

/* ************************************************************************* */
bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol) {
  if (As.size() != Bs.size()) return false;

  list<Matrix>::const_iterator itA, itB;
  itA = As.begin(); itB = Bs.begin();
  for (; itA != As.end(); itA++, itB++)
    if (!assert_equal(*itB, *itA, tol))
      return false;

  return true;
}

/* ************************************************************************* */
static bool is_linear_dependent(const Matrix& A, const Matrix& B, double tol) {
  // This local static function is used by linear_independent and
  // linear_dependent just below.
  size_t n1 = A.cols(), m1 = A.rows();
  size_t n2 = B.cols(), m2 = B.rows();

  bool dependent = true;
  if(m1!=m2 || n1!=n2) dependent = false;

  for(size_t i=0; dependent && i<m1; i++) {
    if (!gtsam::linear_dependent(Vector(row(A,i)), Vector(row(B,i)), tol))
      dependent = false;
  }

  return dependent;
}

/* ************************************************************************* */
bool linear_independent(const Matrix& A, const Matrix& B, double tol) {
  if(!is_linear_dependent(A, B, tol))
    return true;
  else {
    cout << "not linearly dependent:" << endl;
    print(A,"A = ");
    print(B,"B = ");
    if(A.rows()!=B.rows() || A.cols()!=B.cols())
      cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
    return false;
  }
}

/* ************************************************************************* */
bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
  if(is_linear_dependent(A, B, tol))
    return true;
  else {
    cout << "not linearly dependent:" << endl;
    print(A,"A = ");
    print(B,"B = ");
    if(A.rows()!=B.rows() || A.cols()!=B.cols())
      cout << A.rows() << "x" << A.cols() << " != " << B.rows() << "x" << B.cols() << endl;
    return false;
  }
}

/* ************************************************************************* */
void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) {
  e += alpha * A * x;
}

/* ************************************************************************* */
void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
  e += A * x;
}

/* ************************************************************************* */
Vector operator^(const Matrix& A, const Vector & v) {
  if (A.rows()!=v.size()) throw std::invalid_argument(
      boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size()));
//  Vector vt = v.transpose();
//  Vector vtA = vt * A;
//  return vtA.transpose();
  return A.transpose() * v;
}

/* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) {
  x += alpha * A.transpose() * e;
}

/* ************************************************************************* */
void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
  x += A.transpose() * e;
}

/* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
  x += alpha * A.transpose() * e;
}

/* ************************************************************************* */
void print(const Matrix& A, const string &s, ostream& stream) {
  size_t m = A.rows(), n = A.cols();

  // print out all elements
  stream << s << "[\n";
  for( size_t i = 0 ; i < m ; i++) {
    for( size_t j = 0 ; j < n ; j++) {
      double aij = A(i,j);
      if(aij != 0.0)
        stream << setw(12) << setprecision(9) << aij << ",\t";
      else
        stream << "         0.0,\t";
    }
    stream << endl;
  }
  stream << "];" << endl;
}

/* ************************************************************************* */
void save(const Matrix& A, const string &s, const string& filename) {
  fstream stream(filename.c_str(), fstream::out | fstream::app);
  print(A, s + "=", stream);
  stream.close();
}

/* ************************************************************************* */
istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
  string line;
  FastList<vector<double> > coeffs;
  bool first = true;
  size_t width = 0;
  size_t height = 0;
  while(getline(inputStream, line)) {
    // Read coefficients from file
    coeffs.push_back(vector<double>());
    if(!first)
      coeffs.back().reserve(width);
    stringstream lineStream(line);
    std::copy(istream_iterator<double>(lineStream), istream_iterator<double>(),
      back_insert_iterator<vector<double> >(coeffs.back()));
    if(first)
      width = coeffs.back().size();
    if(coeffs.back().size() != width)
      throw runtime_error("Error reading matrix from input stream, inconsistent numbers of elements in rows");
    ++ height;
  }

  // Copy coefficients to matrix
  destinationMatrix.resize(height, width);
  int row = 0;
  BOOST_FOREACH(const vector<double>& rowVec, coeffs) {
    destinationMatrix.row(row) = Eigen::Map<const Eigen::RowVectorXd>(&rowVec[0], width);
    ++ row;
  }

  return inputStream;
}

/* ************************************************************************* */
void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
  fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}

/* ************************************************************************* */
Matrix diag(const std::vector<Matrix>& Hs) {
  size_t rows = 0, cols = 0;
  for (size_t i = 0; i<Hs.size(); ++i) {
    rows+= Hs[i].rows();
    cols+= Hs[i].cols();
  }
  Matrix results = zeros(rows,cols);
  size_t r = 0, c = 0;
  for (size_t i = 0; i<Hs.size(); ++i) {
    insertSub(results, Hs[i], r, c);
    r+=Hs[i].rows();
    c+=Hs[i].cols();
  }
  return results;
}

/* ************************************************************************* */
void insertColumn(Matrix& A, const Vector& col, size_t j) {
  A.col(j) = col;
}

/* ************************************************************************* */
void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) {
  A.col(j).segment(i, col.size()) = col;
}

/* ************************************************************************* */
Vector columnNormSquare(const Matrix &A) {
  Vector v (A.cols()) ;
  for ( size_t i = 0 ; i < (size_t) A.cols() ; ++i ) {
    v[i] = A.col(i).dot(A.col(i));
  }
  return v ;
}

/* ************************************************************************* */
void solve(Matrix& A, Matrix& B) {
  // Eigen version - untested
  B = A.fullPivLu().solve(B);
}

/* ************************************************************************* */
Matrix inverse(const Matrix& A) {
  return A.inverse();
}

/* ************************************************************************* */
/** Householder QR factorization, Golub & Van Loan p 224, explicit version    */
/* ************************************************************************* */
pair<Matrix,Matrix> qr(const Matrix& A) {
  const size_t m = A.rows(), n = A.cols(), kprime = min(m,n);

  Matrix Q=eye(m,m),R(A);
  Vector v(m);

  // loop over the kprime first columns
  for(size_t j=0; j < kprime; j++){

    // we now work on the matrix (m-j)*(n-j) matrix A(j:end,j:end)
    const size_t mm=m-j;

    // copy column from matrix to xjm, i.e. x(j:m) = A(j:m,j)
    Vector xjm(mm);
    for(size_t k = 0 ; k < mm; k++)
      xjm(k) = R(j+k, j);

    // calculate the Householder vector v
    double beta; Vector vjm;
    boost::tie(beta,vjm) = house(xjm);

    // pad with zeros to get m-dimensional vector v
    for(size_t k = 0 ; k < m; k++)
      v(k) = k<j ? 0.0 : vjm(k-j);

    // create Householder reflection matrix Qj = I-beta*v*v'
    Matrix Qj = eye(m) - beta * v * v.transpose();

    R = Qj * R; // update R
    Q = Q * Qj; // update Q

  } // column j

  return make_pair(Q,R);
}

/* ************************************************************************* */
list<boost::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
  size_t m = A.rows(), n = A.cols(); // get size(A)
  size_t maxRank = min(m,n);

  // create list
  list<boost::tuple<Vector, double, double> > results;

  Vector pseudo(m); // allocate storage for pseudo-inverse
  Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once

  // We loop over all columns, because the columns that can be eliminated
  // are not necessarily contiguous. For each one, estimate the corresponding
  // scalar variable x as d-rS, with S the separator (remaining columns).
  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
  for (size_t j=0; j<n; ++j) {
    // extract the first column of A
    Vector a(column(A, j));

    // Calculate weighted pseudo-inverse and corresponding precision
    double precision = weightedPseudoinverse(a, weights, pseudo);

    // if precision is zero, no information on this column
    if (precision < 1e-8) continue;

    // create solution and copy into r
    Vector r(basis(n, j));
    for (size_t j2=j+1; j2<n; ++j2)
      r(j2) = pseudo.dot(A.col(j2));

    // create the rhs
    double d = pseudo.dot(b);

    // construct solution (r, d, sigma)
    // TODO: avoid sqrt, store precision or at least variance
    results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));

    // exit after rank exhausted
    if (results.size()>=maxRank) break;

    // update A, b, expensive, using outer product
    // A' \define A_{S}-a*r and b'\define b-d*a
    A -= a * r.transpose();
    b -= d * a;
  }

  return results;
}

/* ************************************************************************* */
/** Imperative version of Householder QR factorization, Golub & Van Loan p 224
 * version with Householder vectors below diagonal, as in GVL
 */

/* ************************************************************************* */
void householder_(Matrix& A, size_t k, bool copy_vectors) {
  const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
  // loop over the kprime first columns
  for(size_t j=0; j < kprime; j++) {
    // copy column from matrix to vjm, i.e. v(j:m) = A(j:m,j)
    Vector vjm = A.col(j).segment(j, m-j);

    // calculate the Householder vector, in place
    double beta = houseInPlace(vjm);

    // do outer product update A(j:m,:) = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
    gttic(householder_update); // bottleneck for system
    // don't touch old columns
    Vector w = beta * A.block(j,j,m-j,n-j).transpose() * vjm;
    A.block(j,j,m-j,n-j) -= vjm * w.transpose();
    gttoc(householder_update);

    // the Householder vector is copied in the zeroed out part
    if (copy_vectors) {
      gttic(householder_vector_copy);
      A.col(j).segment(j+1, m-(j+1)) = vjm.segment(1, m-(j+1));
      gttoc(householder_vector_copy);
    }
  } // column j
}

/* ************************************************************************* */
void householder(Matrix& A, size_t k) {
  // version with zeros below diagonal
  gttic(householder_);
  householder_(A,k,false);
  gttoc(householder_);
//  gttic(householder_zero_fill);
//  const size_t m = A.rows(), n = A.cols(), kprime = min(k,min(m,n));
//  for(size_t j=0; j < kprime; j++)
//    A.col(j).segment(j+1, m-(j+1)).setZero();
//  gttoc(householder_zero_fill);
}

/* ************************************************************************* */
Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
  // @return the solution x of L*x=b
  assert(L.rows() == L.cols());
  if (unit)
    return L.triangularView<Eigen::UnitLower>().solve(b);
  else
    return L.triangularView<Eigen::Lower>().solve(b);
}

/* ************************************************************************* */
Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
  // @return the solution x of U*x=b
  assert(U.rows() == U.cols());
  if (unit)
    return U.triangularView<Eigen::UnitUpper>().solve(b);
  else
    return U.triangularView<Eigen::Upper>().solve(b);
}

/* ************************************************************************* */
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
  // @return the solution x of x'*U=b'
  assert(U.rows() == U.cols());
  if (unit)
    return U.triangularView<Eigen::UnitUpper>().transpose().solve<Eigen::OnTheLeft>(b);
  else
    return U.triangularView<Eigen::Upper>().transpose().solve<Eigen::OnTheLeft>(b);
}

/* ************************************************************************* */
Matrix stack(size_t nrMatrices, ...)
{
  size_t dimA1 = 0;
  size_t dimA2 = 0;
  va_list ap;
  va_start(ap, nrMatrices);
  for(size_t i = 0 ; i < nrMatrices ; i++) {
    Matrix *M = va_arg(ap, Matrix *);
    dimA1 += M->rows();
    dimA2 =  M->cols();  // TODO: should check if all the same !
  }
  va_end(ap);
  va_start(ap, nrMatrices);
  Matrix A(dimA1, dimA2);
  size_t vindex = 0;
  for( size_t i = 0 ; i < nrMatrices ; i++) {
    Matrix *M = va_arg(ap, Matrix *);
    for(size_t d1 = 0; d1 < (size_t) M->rows(); d1++)
      for(size_t d2 = 0; d2 < (size_t) M->cols(); d2++)
        A(vindex+d1, d2) = (*M)(d1, d2);
    vindex += M->rows();
  }

  return A;
}

/* ************************************************************************* */
Matrix stack(const std::vector<Matrix>& blocks) {
  if (blocks.size() == 1) return blocks.at(0);
  DenseIndex nrows = 0, ncols = blocks.at(0).cols();
  BOOST_FOREACH(const Matrix& mat, blocks) {
    nrows += mat.rows();
    if (ncols != mat.cols())
      throw invalid_argument("Matrix::stack(): column size mismatch!");
  }
  Matrix result(nrows, ncols);

  DenseIndex cur_row = 0;
  BOOST_FOREACH(const Matrix& mat, blocks) {
    result.middleRows(cur_row, mat.rows()) = mat;
    cur_row += mat.rows();
  }
  return result;
}

/* ************************************************************************* */
Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
{
  // if we have known and constant dimensions, use them
  size_t dimA1 = m;
  size_t dimA2 = n*matrices.size();
  if (!m && !n) {
    BOOST_FOREACH(const Matrix* M, matrices) {
      dimA1 =  M->rows();  // TODO: should check if all the same !
      dimA2 += M->cols();
    }
  }

  // stl::copy version
  Matrix A(dimA1, dimA2);
  size_t hindex = 0;
  BOOST_FOREACH(const Matrix* M, matrices) {
    size_t row_len = M->cols();
    A.block(0, hindex, dimA1, row_len) = *M;
    hindex += row_len;
  }

  return A;
}

/* ************************************************************************* */
Matrix collect(size_t nrMatrices, ...)
{
  vector<const Matrix *> matrices;
  va_list ap;
  va_start(ap, nrMatrices);
  for( size_t i = 0 ; i < nrMatrices ; i++) {
    Matrix *M = va_arg(ap, Matrix *);
    matrices.push_back(M);
  }
  return collect(matrices);
}

/* ************************************************************************* */
// row scaling, in-place
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
  const DenseIndex m = A.rows();
  if (inf_mask) {
    // only scale the first v.size() rows of A to support augmented Matrix
    for (DenseIndex i=0; i<v.size(); ++i) {
      const double& vi = v(i);
      if (std::isfinite(vi))
        A.row(i) *= vi;
    }
  } else {
    for (DenseIndex i=0; i<m; ++i)
      A.row(i) *= v(i);
  }
}

/* ************************************************************************* */
// row scaling
Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask) {
  Matrix M(A);
  vector_scale_inplace(v, M, inf_mask);
  return M;
}

/* ************************************************************************* */
// column scaling
Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
  Matrix M(A);
  const size_t n = A.cols();
  if (inf_mask) {
    for (size_t j=0; j<n; ++j) {
      const double& vj = v(j);
      if (std::isfinite(vj))
        M.col(j) *= vj;
    }
  } else {
    for (size_t j=0; j<n; ++j)
      M.col(j) *= v(j);
  }
  return M;
}

/* ************************************************************************* */
Matrix3 skewSymmetric(double wx, double wy, double wz)
{
  return (Matrix3() <<
      0.0, -wz, +wy,
      +wz, 0.0, -wx,
      -wy, +wx, 0.0).finished();
}

/* ************************************************************************* */
Matrix LLt(const Matrix& A)
{
  Eigen::LLT<Matrix> llt(A);
  return llt.matrixL();
}

/* ************************************************************************* */
Matrix RtR(const Matrix &A)
{
  Eigen::LLT<Matrix> llt(A);
  return llt.matrixU();
}

/*
 * This is not ultra efficient, but not terrible, either.
 */
Matrix cholesky_inverse(const Matrix &A)
{
  Eigen::LLT<Matrix> llt(A);
  Matrix inv = eye(A.rows());
  llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
  return inv*inv.transpose();
}

/* ************************************************************************* */
// Semantics:
// if B = inverse_square_root(A), then all of the following are true:
// inv(B) * inv(B)' == A
// inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) {
  Eigen::LLT<Matrix> llt(A);
  Matrix inv = eye(A.rows());
  llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
  return inv.transpose();
}

/* ************************************************************************* */
void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V) {
  Eigen::JacobiSVD<Matrix> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
  U = svd.matrixU();
  S = svd.singularValues();
  V = svd.matrixV();
}

/* ************************************************************************* */
boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {

  // Check size of A
  size_t n = A.rows(), p = A.cols(), m = min(n,p);

  // Do SVD on A
  Eigen::JacobiSVD<Matrix> svd(A, Eigen::ComputeFullV);
  Vector s = svd.singularValues();
  Matrix V = svd.matrixV();

  // Find rank
  size_t rank = 0;
  for (size_t j = 0; j < m; j++)
    if (s(j) > rank_tol) rank++;

  // Return rank, error, and corresponding column of V
  double error = m<p ? 0 : s(m-1);
  return boost::tuple<int, double, Vector>((int)rank, error, Vector(column(V, p-1)));
}

/* ************************************************************************* */
Matrix expm(const Matrix& A, size_t K) {
  Matrix E = eye(A.rows()), A_k = eye(A.rows());
  for(size_t k=1;k<=K;k++) {
    A_k = A_k*A/double(k);
    E = E + A_k;
  }
  return E;
}

/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
  size_t n = A.cols();
  assert(A.rows() == n);

  // original
//  const Matrix I = eye(n);
//  return (I-A)*inverse(I+A);

  // inlined to let Eigen do more optimization
  return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse();
}

/* ************************************************************************* */
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
{
  stringstream ss;
  const string firstline = label;
  ss << firstline;
  const string padding(firstline.size(), ' ');
  const bool transposeMatrix = makeVectorHorizontal && matrix.cols() == 1 && matrix.rows() > 1;
  const DenseIndex effectiveRows = transposeMatrix ? matrix.cols() : matrix.rows();

  if(matrix.rows() > 0 && matrix.cols() > 0)
  {
    stringstream matrixPrinted;
    if(transposeMatrix)
      matrixPrinted << matrix.transpose();
    else
      matrixPrinted << matrix;
    const std::string matrixStr = matrixPrinted.str();
    boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));

    DenseIndex row = 0;
    BOOST_FOREACH(const std::string& line, tok)
    {
      assert(row < effectiveRows);
      if(row > 0)
        ss << padding;
      ss << "[ " << line << " ]";
      if(row < effectiveRows - 1)
        ss << "\n";
      ++ row;
    }
  } else {
    ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
  }
  return ss.str();
}

} // namespace gtsam


/*
 * hdverify.cpp
 *
 * Author: P. Wild (pwild@cosy.sbg.ac.at)
 *
 * Evaluates an algorithm performing hamming distance comparison
 *
 */
#include <cstdio>
#include <map>
#include <vector>
#include <string>
#include <cstring>
#include <algorithm>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using namespace std;
using namespace cv;

/** no globbing in win32 mode **/
int _CRT_glob = 0;

/** Algorithms **/
static const int ALG_MINHD = 0, ALG_MAXHD = 1, ALG_SSF = 2;
/** Evaluation modes **/
static const int EVAL_ALL = 0, EVAL_BALANCED = 1;
/** Program modes **/
static const int MODE_MAIN = 1, MODE_HELP = 2;

static const int htlut[256] = {0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8};

/*
 * Print command line usage for this program
 */
void printUsage() {
	printf("+-----------------------------------------------------------------------------+\n");
    printf("| hdverify - estimates performance of HD-based verification of iris codes     |\n");
    printf("|                                                                             |\n");
    printf("| MODES                                                                       |\n");
    printf("|                                                                             |\n");
    printf("| (# 1) HD-based verification results for input images (cross comparison)     |\n");
    printf("| (# 2) usage                                                                 |\n");
    printf("|                                                                             |\n");
    printf("| ARGUMENTS                                                                   |\n");
    printf("|                                                                             |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| Name | Parameters | # | ? | Description                                     |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| -i   | infile     | 1 | N | source image (* = any)                          |\n");
    printf("|      | class      |   |   | class label (?n = n-th * in infile)             |\n");
    printf("| -s   | param+     | 1 | Y | min max: min/max (0 0) number of bit shifts     |\n");
    printf("|      |            |   | Y | img: shifted src (?n = n-th * in infile1,* =any)|\n");
    printf("| -m   | maskfile1  | 1 | Y | source mask (?n/!n = n-th * in infile1/img)     |\n");
    printf("|      | maskfile2  |   |   | infile mask (?n = n-th * in infile)             |\n");
    printf("| -c   | comparison | 1 | Y | comparison mode (all)                           |\n");
    printf("|      |            |   |   | all: cross comparison with all pairs            |\n");
    printf("|      |            |   |   | balanced: only first template between users     |\n");
    printf("| -a   | algorithm  | 1 | Y | HD-based algorithm (minhd)                      |\n");
    printf("|      |            |   |   | minhd: minimum HD for all shifts                |\n");
    printf("|      |            |   |   | maxhd: 1-maximum HD for all shifts              |\n");
    printf("|      |            |   |   | ssf: shift score fusion                         |\n");
    printf("|      |            |   |   | exact: HD for specific shift                    |\n");
    printf("| -s   | param+     | 1 | Y | min max: min/max (0 0) number of bit shifts     |\n");
    printf("|      |            |   | Y | img: shifted src (?n = n-th * in infile1,* =any)|\n");
    printf("| -n   | from to    | 1 | Y | starting (0) and ending bit (MAX)               |\n");
    printf("| -b   | bins       | 1 | Y | number of histogram bins (1000)                 |\n");
    printf("| -o   | genuinefile| 1 | Y | target match file paths (HD matching scores of  |\n");
    printf("|      | impostfile | 1 | Y | genuines and imposters, file-sorted)            |\n");
    printf("| -r   | rocfile    | 1 | Y | target roc-file path (FMR/FNMR pairs)           |\n");
    printf("| -d   | distfile   | 1 | Y | target distributions-file path  (Gen/Imp pairs) |\n");
    printf("| -q   |            | 1 | Y | quiet mode on (off)                             |\n");
    printf("| -t   |            | 1 | Y | time progress on (off)                          |\n");
    printf("| -h   |            | 2 | N | prints usage                                    |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("|                                                                             |\n");
	printf("| EXAMPLE USAGE                                                               |\n");
    printf("|                                                                             |\n");
    printf("| -i files/class*/*.tiff ?1 -t -s -7 7                                        |\n");
    printf("| -i */*.tiff ?1 -m ?1/?2_mask.png -s -7 7 -o gen.txt imp.txt -q -t           |\n");
    printf("|                                                                             |\n");
    printf("| AUTHOR                                                                      |\n");
    printf("|                                                                             |\n");
    printf("| Peter Wild (pwild@cosy.sbg.ac.at)                                           |\n");
    printf("|                                                                             |\n");
    printf("| COPYRIGHT                                                                   |\n");
    printf("|                                                                             |\n");
    printf("| (C) 2010 All rights reserved. Do not distribute without written permission. |\n");
    printf("+-----------------------------------------------------------------------------+\n");
}

/**
 * Fast hamming distance estimation
 * a: sample iris code
 * b: reference iris code
 * start8: starting 8-bit block
 * stop8: ending 8-bit block
 */
unsigned int hd(const Mat a, const Mat b, const unsigned int start8, const unsigned int stop8, const Mat mask = Mat()){
	unsigned int dist = 0;
	MatConstIterator_<uchar> pa = a.begin<uchar>();
	MatConstIterator_<uchar> pb = b.begin<uchar>();
	MatConstIterator_<uchar> enda = pa + stop8;
	pa += start8;
	pb += start8;
	if (!mask.empty()){
		MatConstIterator_<uchar> pm = mask.begin<uchar>();
		//unsigned int * pm = (unsigned int*) mask.data;
		pm += start8;
		for (;pa<enda; pa++, pb++, pm++){
			dist += htlut[(*pa ^*pb) & *pm];

			/*unsigned int val = (*pa ^*pb) & *pm;
			while(val)
			{
				++dist;
				val &= val - 1;
			}*/
		}
	}
	else {
		for (;pa<enda; pa++, pb++){
			//unsigned int val = *pa ^*pb;
			dist += htlut[*pa ^*pb];
			/*
			while(val)
			{
				++dist;
				val &= val - 1;
			}*/
		}
	}
	return dist;

}

/**
 * Shifts source by a given shift count
 * src: source iris code
 * dst: destination (shifted) iris code
 * shifts: shift count (positive values indicate left shifts)
 */
void shift(const Mat src, Mat dst, const int shifts){
	int size = src.cols*src.rows; // size in bytes
	MatConstIterator_<uchar> psrc = src.begin<uchar>();
	MatConstIterator_<uchar> endsrc = src.end<uchar>();
	MatIterator_<uchar> pdst = dst.begin<uchar>();
	MatIterator_<uchar> enddst = dst.end<uchar>();

	if (shifts >= 0){ // left shift
		unsigned int offset = shifts / 8;
		psrc += offset;
		unsigned int shiftCount = shifts % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		for (;pdst<enddst; pdst++){
			*pdst = (*psrc << shiftCount);
			psrc++;
			if (psrc == endsrc) psrc = src.begin<uchar>();
			*pdst |= (*psrc >> (ishiftCount));
		}
	}
	else { // right shift
		unsigned int offset = (-shifts) / 8;
		offset = (size-offset-1) % size;
		psrc += offset;
		unsigned int shiftCount = (-shifts) % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		for (;pdst<enddst; pdst++){
			*pdst = (*psrc << ishiftCount);
			psrc++;
			if (psrc == endsrc) psrc = src.begin<uchar>();
			*pdst |= (*psrc >> (shiftCount));
		}
	}
}

/**
 * Shifts matrix a by a given shift count and intersects result with b
 * a: sample source iris mask
 * b: reference source iris mask
 * dst: destination (shifted) iris mask
 * shifts: shift count (positive values indicate left shifts)
 */
void intersectShifted(const Mat a, const Mat b, Mat dst, const int shifts){
	uchar * pb = b.data;
	uchar * pa = a.data;
	uchar * pdst = dst.data;
	unsigned int size = dst.cols;
	unsigned int offset;
	if (shifts >= 0){ // left shift
		offset = shifts / 8;
		uchar * enddst = pdst + dst.cols;
		unsigned int shiftCount = shifts % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		offset = (size + offset) % size;
		for (;pdst<enddst; pdst++, pb++){
			*pdst = (pa[offset] << shiftCount);
			offset = ((offset+1)%size);
			*pdst |= (pa[offset] >> (ishiftCount));
			*pdst &= *pb;
		}
	}
	else { // right shift
		offset = (-shifts) / 8;
		uchar * enddst = pdst + dst.cols;
		unsigned int shiftCount = (-shifts) % 8;
		unsigned int ishiftCount = 8 - shiftCount;
		offset = (size-offset-1) % size;
		for (;pdst<enddst; pdst++, pb++){
			*pdst = (pa[offset] << ishiftCount);
			offset = ((offset+1)%size);
			*pdst |= (pa[offset] >> (shiftCount));
			*pdst &= *pb;
		}
	}
}

/**
 * determines the best fractional Hamming Distance of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
double minHD(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts, const Mat aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 1;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
		}
		result = hamdist;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
		}
		result = (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the best fractional Hamming Distance of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
double minHD(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 1;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
		}
		result = hamdist;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
		}
		result = (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the worst fractional Hamming Distance of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
double maxHD(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts, const Mat aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist > hamdist) hamdist = shiftedHamdist;
		}
		result = 1 - hamdist;
	}
	else {

		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist > hamdist) hamdist = shiftedHamdist;
		}
		result = 1-(((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines the worst fractional Hamming Distance of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
double maxHD(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		double hamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist > hamdist) hamdist = shiftedHamdist;
		}
		result = 1 - hamdist;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist > hamdist) hamdist = shiftedHamdist;
		}
		result = 1- (((double)hamdist) / (codeLengthBits));
	}
	return result;
}

/**
 * determines Shifting Score Fusion of two iris codes
 * a: first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * shifts: number of shifts
 * aMask: mask for first iris code
 * bMask: mask for second iris code
 */
double ssf(const Mat& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const int minShifts, const int maxShifts, const Mat aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		intersectShifted(aMask,bMask,mask,0);
		double hamdist = 1;
		double maxhamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			intersectShifted(aMask,bMask,mask,s);
			int codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(imgSmplShifted,b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result = ((1-maxhamdist) +  hamdist)/2;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		unsigned int maxhamdist = 0;
		Mat imgSmplShifted(a.rows,a.cols,CV_8UC1);
		for (int s=minShifts; s<=maxShifts; s++){
			shift(a,imgSmplShifted,s);
			unsigned int shiftedHamdist = hd(imgSmplShifted,b,start8,stop8);
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result = ((1-(((double)maxhamdist) / (codeLengthBits))) + (((double)hamdist) / (codeLengthBits)))/2;
	}
	return result;
}

/**
 * determines Shifting Score Fusion of an iris code with a list of shifted versions of this code
 * a: shifted versions of first iris code
 * b: second iris code
 * start8: starting 8-bit block (inclusive)
 * stop8: ending 8-bit block (exclusive)
 * aMask: masks for corresponding shifted versions of first iris code
 * bMask: mask for second iris code
 */
double ssf(const vector<Mat>& a, const Mat& b, const unsigned int start8, const unsigned int stop8, const vector<Mat>& aMask, const Mat bMask = Mat()){
	double result = 0;
	if (!aMask.empty() && !bMask.empty()){
		CV_Assert(aMask.size() == a.size());
		Mat mask(b.rows,b.cols,CV_8UC1);
		Mat zero(b.rows,b.cols,CV_8UC1);
		zero.setTo(Scalar(0));
		int codeLengthBits = 0;
		double hamdist = 1;
		double maxhamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			intersectShifted(aMask[i],bMask,mask,0);
			codeLengthBits = hd(mask, zero, start8, stop8);
			double shiftedHamdist = (codeLengthBits == 0) ? 0 : ((double)hd(a[i],b,start8,stop8,mask)) / codeLengthBits;
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result = ((1-maxhamdist) +  hamdist)/2;
	}
	else {
		int codeLengthBits = 8*(stop8-start8);
		unsigned int hamdist = codeLengthBits;
		unsigned int maxhamdist = 0;
		for (unsigned int i=0; i<a.size();i++){
			unsigned int shiftedHamdist = hd(a[i],b,start8,stop8);
			if (shiftedHamdist < hamdist) hamdist = shiftedHamdist;
			if (shiftedHamdist > maxhamdist) maxhamdist = shiftedHamdist;
		}
		result = ((1-(((double)maxhamdist) / (codeLengthBits))) + (((double)hamdist) / (codeLengthBits)))/2;
	}
	return result;
}

/**
 * Compares two iris codes
 * imgSmpl: first iris code
 * imgRef: second iris code
 * bitStart: starting 8-bit block (inclusive)
 * bitStop: ending 8-bit block (exclusive)
 * shifts: number of shifts
 */
double compare(const vector<Mat>& imgSmpl, const Mat& imgRef, const unsigned int from, const unsigned int bitStop, const int minShifts, const int maxShifts, int alg, const vector<Mat>& maskSmpl, const Mat& maskRef, bool shiftedfiles){
	if (alg == ALG_MINHD) {
		if (shiftedfiles) return minHD(imgSmpl,imgRef,from,bitStop, maskSmpl, maskRef); else return minHD(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef);
	}
	else if (alg == ALG_MAXHD) {
		if (shiftedfiles) return maxHD(imgSmpl,imgRef,from,bitStop, maskSmpl, maskRef); else return maxHD(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef);
	}
	else if (shiftedfiles) return ssf(imgSmpl,imgRef,from,bitStop,maskSmpl, maskRef); else return ssf(imgSmpl[0],imgRef,from,bitStop,minShifts, maxShifts, (maskSmpl.size() > 0) ? maskSmpl[0] : Mat(), maskRef);
}

/** ------------------------------- commandline functions ------------------------------- **/

/**
 * Parses a command line
 * This routine should be called for parsing command lines for executables.
 * Note, that all options require '-' as prefix and may contain an arbitrary
 * number of optional arguments.
 *
 * cmd: commandline representation
 * argc: number of parameters
 * argv: string array of argument values
 */
void cmdRead(map<string ,vector<string> >& cmd, int argc, char *argv[]){
	for (int i=1; i< argc; i++){
		char * argument = argv[i];
		if (strlen(argument) > 1 && argument[0] == '-' && (argument[1] < '0' || argument[1] > '9')){
			cmd[argument]; // insert
			char * argument2;
			while (i + 1 < argc && (strlen(argument2 = argv[i+1]) <= 1 || argument2[0] != '-'  || (argument2[1] >= '0' && argument2[1] <= '9'))){
				cmd[argument].push_back(argument2);
				i++;
			}
		}
		else {
			CV_Error(CV_StsBadArg,"Invalid command line format");
		}
	}
}

/**
 * Checks, if each command line option is valid, i.e. exists in the options array
 *
 * cmd: commandline representation
 * validOptions: list of valid options separated by pipe (i.e. |) character
 */
void cmdCheckOpts(map<string ,vector<string> >& cmd, const string validOptions){
	vector<string> tokens;
	const string delimiters = "|";
	string::size_type lastPos = validOptions.find_first_not_of(delimiters,0); // skip delimiters at beginning
	string::size_type pos = validOptions.find_first_of(delimiters, lastPos); // find first non-delimiter
	while (string::npos != pos || string::npos != lastPos){
		tokens.push_back(validOptions.substr(lastPos,pos - lastPos)); // add found token to vector
		lastPos = validOptions.find_first_not_of(delimiters,pos); // skip delimiters
		pos = validOptions.find_first_of(delimiters,lastPos); // find next non-delimiter
	}
	sort(tokens.begin(), tokens.end());
	for (map<string, vector<string> >::iterator it = cmd.begin(); it != cmd.end(); it++){
		if (!binary_search(tokens.begin(),tokens.end(),it->first)){
			CV_Error(CV_StsBadArg,"Command line parameter '" + it->first + "' not allowed.");
			tokens.clear();
			return;
		}
	}
	tokens.clear();
}

/*
 * Checks, if a specific required option exists in the command line
 *
 * cmd: commandline representation
 * option: option name
 */
void cmdCheckOptExists(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it == cmd.end()) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is required, but does not exist.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * size: appropriate number of parameters for the option
 */
void cmdCheckOptSize(map<string ,vector<string> >& cmd, const string option, const unsigned int size = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it->second.size() != size) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' has unexpected size.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * min: minimum appropriate number of parameters for the option
 * max: maximum appropriate number of parameters for the option
 */
void cmdCheckOptRange(map<string ,vector<string> >& cmd, string option, unsigned int min = 0, unsigned int max = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	unsigned int size = it->second.size();
	if (size < min || size > max) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is out of range.");
}

/*
 * Returns the list of parameters for a given option
 *
 * cmd: commandline representation
 * option: name of the option
 */
vector<string> * cmdGetOpt(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? &(it->second) : 0;
}

/*
 * Returns number of parameters in an option
 *
 * cmd: commandline representation
 * option: name of the option
 */
unsigned int cmdSizePars(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? it->second.size() : 0;
}

/*
 * Returns a specific parameter type (int) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
int cmdGetParInt(map<string ,vector<string> >& cmd, string option, unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atoi(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (float) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
float cmdGetParFloat(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atof(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (string) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
string cmdGetPar(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return it->second[param];
		}
	}
	return 0;
}

/** ------------------------------- timing functions ------------------------------- **/

/**
 * Class for handling timing progress information
 */
class Timing{
public:
	/** integer indicating progress with respect tot total **/
	int progress;
	/** total count for progress **/
	int total;

	/*
	 * Default constructor for timing initializing time.
	 * Automatically calls init()
	 *
	 * seconds: update interval in seconds
	 * eraseMode: if true, outputs sends erase characters at each print command
	 */
	Timing(long seconds, bool eraseMode){
		updateInterval = seconds;
		progress = 1;
		total = 100;
		eraseCount=0;
		erase = eraseMode;
		init();
	}

	/*
	 * Destructor
	 */
	~Timing(){}

	/*
	 * Initializes timing variables
	 */
	void init(void){
		start = boost::posix_time::microsec_clock::universal_time();
		lastPrint = start - boost::posix_time::seconds(updateInterval);
	}

	/*
	 * Clears printing (for erase option only)
	 */
	void clear(void){
		string erase(eraseCount,'\r');
		erase.append(eraseCount,' ');
		erase.append(eraseCount,'\r');
		printf("%s",erase.c_str());
		eraseCount = 0;
	}

	/*
	 * Updates current time and returns true, if output should be printed
	 */
	bool update(void){
		current = boost::posix_time::microsec_clock::universal_time();
		return ((current - lastPrint > boost::posix_time::seconds(updateInterval)) || (progress == total));
	}

	/*
	 * Prints timing object to STDOUT
	 */
	void print(void){
		lastPrint = current;
		float percent = 100.f * progress / total;
		boost::posix_time::time_duration passed = (current - start);
		boost::posix_time::time_duration togo = passed * (total - progress) / max(1,progress);
		if (erase) {
			string erase(eraseCount,'\r');
			printf("%s",erase.c_str());
			int newEraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
			if (newEraseCount < eraseCount) {
				string erase(newEraseCount-eraseCount,' ');
				erase.append(newEraseCount-eraseCount,'\r');
				printf("%s",erase.c_str());
			}
			eraseCount = newEraseCount;
		}
		else {
			eraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
		}
	}
private:
	long updateInterval;
	boost::posix_time::ptime start;
	boost::posix_time::ptime current;
	boost::posix_time::ptime lastPrint;
	int eraseCount;
	bool erase;
};

/** ------------------------------- file pattern matching functions ------------------------------- **/

/*
 * Formats a given string, such that it can be used as a regular expression
 * I.e. escapes special characters and uses * and ? as wildcards
 *
 * pattern: regular expression path pattern
 * pos: substring starting index
 * n: substring size
 *
 * returning: escaped substring
 */
string patternSubstrRegex(string& pattern, size_t pos, size_t n){
	string result;
	for (size_t i=pos, e=pos+n; i < e; i++ ) {
		char c = pattern[i];
		if ( c == '\\' || c == '.' || c == '+' || c == '[' || c == '{' || c == '|' || c == '(' || c == ')' || c == '^' || c == '$' || c == '}' || c == ']') {
			result.append(1,'\\');
			result.append(1,c);
		}
		else if (c == '*'){
			result.append("([^/\\\\]*)");
		}
		else if (c == '?'){
			result.append("([^/\\\\])");
		}
		else {
			result.append(1,c);
		}
	}
	return result;
}

/*
 * Converts a regular expression path pattern into a list of files matching with this pattern by replacing wildcards
 * starting in position pos assuming that all prior wildcards have been resolved yielding intermediate directory path.
 * I.e. this function appends the files in the specified path according to yet unresolved pattern by recursive calling.
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 * pos: an index such that positions 0...pos-1 of pattern are already considered/matched yielding path
 * path: the current directory (or empty)
 */
void patternToFiles(string& pattern, vector<string>& files, const size_t& pos, const string& path){
	size_t first_unknown = pattern.find_first_of("*?",pos); // find unknown * in pattern
	if (first_unknown != string::npos){
		size_t last_dirpath = pattern.find_last_of("/\\",first_unknown);
		size_t next_dirpath = pattern.find_first_of("/\\",first_unknown);
		if (next_dirpath != string::npos){
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,next_dirpath-last_dirpath-1) : patternSubstrRegex(pattern,pos,next_dirpath-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr( ((path.length() > 0) ? path + pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					if (boost::filesystem::is_directory(itr->path())){
						boost::filesystem::path p = itr->path().filename();
						string s =  p.string();
						if (boost::regex_match(s.c_str(), expr)){
							patternToFiles(pattern,files,(int)(next_dirpath+1),((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
						}
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
		else {
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,pattern.length()-last_dirpath-1) : patternSubstrRegex(pattern,pos,pattern.length()-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr(((path.length() > 0) ? path +  pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					boost::filesystem::path p = itr->path().filename();
					string s =  p.string();
					if (boost::regex_match(s.c_str(), expr)){
						files.push_back(((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
	}
	else { // no unknown symbols
		boost::filesystem::path file(((path.length() > 0) ? path + "/" : "") + pattern.substr(pos,pattern.length()-pos));
		if (boost::filesystem::exists(file)){
			files.push_back(file.string());
		}
	}
}

/**
 * Converts a regular expression path pattern into a list of files matching with this pattern
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 */
void patternToFiles(string& pattern, vector<string>& files){
	patternToFiles(pattern,files,0,"");
}

/*
 * Renames a given filename corresponding to the actual file pattern using a renaming pattern.
 * Wildcards can be referred to as ?1, ?2, ... in the order they appeared in the file pattern.
 *
 * pattern: regular expression path pattern
 * renamePattern: renaming pattern using ?1, ?2, ... as placeholders for wildcards
 * infile: path of the file (matching with pattern) to be renamed
 * outfile: path of the renamed file
 * par: used parameter (default: '?')
 */
void patternFileRename(string& pattern, const string& renamePattern, const string& infile, string& outfile, const char par = '?'){
	size_t first_unknown = renamePattern.find_first_of(par,0); // find unknown ? in renamePattern
	if (first_unknown != string::npos){
		string formatOut = "";
		for (size_t i=0, e=renamePattern.length(); i < e; i++ ) {
			char c = renamePattern[i];
			if ( c == par && i+1 < e) {
				c = renamePattern[i+1];
				if (c > '0' && c <= '9'){
					formatOut.append(1,'$');
					formatOut.append(1,c);
				}
				else {
					formatOut.append(1,par);
					formatOut.append(1,c);
				}
				i++;
			}
			else {
				formatOut.append(1,c);
			}
		}
		boost::regex patternOut(patternSubstrRegex(pattern,0,pattern.length()));
		outfile = boost::regex_replace(infile,patternOut,formatOut,boost::match_default | boost::format_perl);
	} else {
		outfile = renamePattern;
	}
}

/** ------------------------------- Program ------------------------------- **/

/*
 * Main program
 */
int main(int argc, char *argv[])
{
	int mode = MODE_HELP;
	map<string,vector<string> > cmd;
	try {
		cmdRead(cmd,argc,argv);
		if (cmd.size() == 0 || cmdGetOpt(cmd,"-h") != 0) mode = MODE_HELP;
		else mode = MODE_MAIN;
		if (mode == MODE_MAIN){
			// validate command line
			cmdCheckOpts(cmd,"-i|-s|-m|-c|-a|-n|-b|-o|-r|-d|-q|-t");
			cmdCheckOptExists(cmd,"-i");
			cmdCheckOptSize(cmd,"-i",2);
			string infiles = cmdGetPar(cmd,"-i",0);
			string users = cmdGetPar(cmd,"-i",1);
			bool shiftedfiles = (cmdGetOpt(cmd,"-s") != 0 && cmdSizePars(cmd,"-s") == 1);
			string shiftfiles = ((shiftedfiles) ? cmdGetPar(cmd,"-s") : "");
			int minShifts = 0;
			int maxShifts = 0;
			if (cmdGetOpt(cmd,"-s") != 0 && !shiftedfiles){
				cmdCheckOptSize(cmd,"-s",2);
				minShifts = cmdGetParInt(cmd,"-s",0);
				maxShifts = cmdGetParInt(cmd,"-s",1);
			}
			int alg = ALG_MINHD;
			if (cmdGetOpt(cmd,"-a") != 0){
				cmdCheckOptSize(cmd,"-a",1);
				string algo = cmdGetPar(cmd,"-a");
				if (algo == "maxhd"){
					alg = ALG_MAXHD;
				}
				else if (algo == "ssf"){
					alg = ALG_SSF;
				}
			}
			unsigned int from = 0;
			unsigned int to = INT_MAX;
			if (cmdGetOpt(cmd,"-n") != 0){
				cmdCheckOptSize(cmd,"-n",2);
				from = cmdGetParInt(cmd,"-n",0);
				CV_Assert(from % 8 == 0);
				from /= 8;
				to = cmdGetParInt(cmd,"-n",1);
				CV_Assert(to % 8 == 0);
				to /= 8;
			}
			bool masks = (cmdGetOpt(cmd,"-m") != 0);
			if (masks && shiftedfiles) cmdCheckOptSize(cmd,"-m",2); else if (masks) cmdCheckOptSize(cmd,"-m",1);
			string maskfiles = ((masks) ? cmdGetPar(cmd,"-m",0) : "");
			string refmaskfiles = (masks && shiftedfiles) ? cmdGetPar(cmd,"-m",1) : maskfiles;
			int mod = EVAL_ALL;
			if (cmdGetOpt(cmd,"-c") != 0){
				cmdCheckOptSize(cmd,"-c",1);
				string mode = cmdGetPar(cmd,"-c");
				if (mode == "balanced"){
					mod = EVAL_BALANCED;
				}
			}
			int bins = 1000;
			if (cmdGetOpt(cmd,"-b") != 0){
				cmdCheckOptSize(cmd,"-b",1);
				bins = cmdGetParInt(cmd,"-b");
			}
			string gsfile;
			string isfile;
			if (cmdGetOpt(cmd,"-o") != 0){
				cmdCheckOptSize(cmd,"-o",2);
				gsfile = cmdGetPar(cmd,"-o",0);
				isfile = cmdGetPar(cmd,"-o",1);
			}
			string roc;
			if (cmdGetOpt(cmd,"-r") != 0){
				cmdCheckOptSize(cmd,"-r",1);
				roc = cmdGetPar(cmd,"-r");
			}
			string dist;
			if (cmdGetOpt(cmd,"-d") != 0){
				cmdCheckOptSize(cmd,"-d",1);
				dist = cmdGetPar(cmd,"-d");
			}
			bool quiet = false;
			if (cmdGetOpt(cmd,"-q") != 0){
				cmdCheckOptSize(cmd,"-q",0);
				quiet = true;
			}
			bool time = false;
			if (cmdGetOpt(cmd,"-t") != 0){
				cmdCheckOptSize(cmd,"-t",0);
				time = true;
			}
			// starting routine
			Timing timing(1,quiet);
			vector<string> files;
			patternToFiles(infiles,files);
			map<string, vector<string> > userTemplates;
			if (!quiet) cout << "Organizing matches ..." << endl;
			CV_Assert(files.size() > 0);
			for (vector<string>::iterator infile = files.begin(); infile != files.end(); ++infile){
				string user, templ;
				patternFileRename(infiles,users,*infile,user);
				userTemplates[user].push_back(*infile);
			}
			int genuinesCount = 0, impostersCount = 0;
			// counting comparisons
			if (mod == EVAL_BALANCED){
				for (map<string, vector<string> >::iterator it = userTemplates.begin(); it != userTemplates.end(); it++){
					int templSize = it->second.size();
					genuinesCount += (templSize * (templSize-1))/2;
					impostersCount ++;
				}
				impostersCount = (impostersCount * (impostersCount - 1))/2;
			}
			else {// mod == EVAL_ALL
				for (map<string, vector<string> >::iterator it = userTemplates.begin(); it != userTemplates.end(); it++){
					int templSize = it->second.size();
					genuinesCount += (templSize * (templSize-1))/2;
					impostersCount += templSize;
				}
				impostersCount = (impostersCount * (impostersCount - 1))/2;
				impostersCount -= genuinesCount;
			}
			if (!quiet) cout << "done" << endl;
			ofstream gfile, ifile;
			if (!gsfile.empty()){
				if (!quiet) cout << "Opening result files '" << gsfile << "','" << isfile << "' ..."<< endl;
				gfile.open(gsfile.c_str(),ios::out | ios::trunc);
				ifile.open(isfile.c_str(),ios::out | ios::trunc);
				if (!(gfile.is_open())) {
					CV_Error(CV_StsError,"Could not open result file '" + gsfile + "'");
				}
				if (!(ifile.is_open())) {
					CV_Error(CV_StsError,"Could not open result file '" + isfile + "'");
				}
				if (!quiet) cout << "done" << endl;
			}
			if (!quiet) cout << "Executing matches (" << genuinesCount << " genuines, " << impostersCount << " imposters) ..."<< endl;
			int * genuines = new int[bins];
			int * imposters = new int[bins];
			memset(genuines, 0,bins*sizeof(int));
			memset(imposters, 0,bins*sizeof(int));
			timing.total = genuinesCount + impostersCount;
			if (mod == EVAL_ALL){
				for (map<string, vector<string> >::iterator it = userTemplates.begin(); it != userTemplates.end(); it++){
					for (vector<string>::iterator itSample = it->second.begin(), itEnd = it->second.end(); itSample != itEnd; itSample++){
						vector<Mat> imgSmpl;
						vector<Mat> maskSmpl;
						if (shiftedfiles){
							string shiftfile;
							patternFileRename(infiles,shiftfiles,*itSample,shiftfile);
							vector<string> shiftsSmpl;
							patternToFiles(shiftfile,shiftsSmpl);
							CV_Assert(shiftsSmpl.size() > 0);
							// now load virtual files
							for (vector<string>::iterator shiftSmpl = shiftsSmpl.begin(); shiftSmpl != shiftsSmpl.end(); ++shiftSmpl){
								Mat img = imread(*shiftSmpl, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(img.data != 0);
								CV_Assert(img.type() == CV_8UC1);
								if (imgSmpl.size() > 0) { CV_Assert(imgSmpl.back().size() == img.size());}
								imgSmpl.push_back(img);
								if (masks){
									string maskfile1, maskfile2;
									patternFileRename(infiles,maskfiles,*itSample,maskfile1);
									patternFileRename(infiles,maskfile1,*itSample,maskfile2,'!');
									Mat msk = imread(maskfile2, CV_LOAD_IMAGE_UNCHANGED);
									CV_Assert(msk.data != 0);
									CV_Assert(msk.type() == CV_8UC1);
									CV_Assert(img.size() == msk.size());
									maskSmpl.push_back(msk);
								}
							}

						}
						else {
							Mat img = imread(*itSample, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(img.data != 0);
							CV_Assert(img.type() == CV_8UC1);
							imgSmpl.push_back(img);
							if (masks){
								string maskSmplFile;
								patternFileRename(infiles,maskfiles,*itSample,maskSmplFile);
								Mat msk = imread(maskSmplFile, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(msk.data != 0);
								CV_Assert(msk.type() == CV_8UC1);
								CV_Assert(msk.size() == img.size());
								maskSmpl.push_back(msk);
							}
						}
						Size codeSize = imgSmpl[0].size();
						unsigned int codeLength = codeSize.height * codeSize.width;
						unsigned int bitStop = min(to,codeLength);
						CV_Assert(codeLength % sizeof(int) == 0);
						// genuine matches
						vector<string>::iterator itRef = itSample;
						for (itRef++; itRef != itEnd; itRef++, timing.progress++){
							Mat imgRef = imread(*itRef, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(imgRef.data != 0);
							CV_Assert(imgRef.type() == CV_8UC1);
							CV_Assert(imgRef.size() == codeSize);
							Mat maskRef;
							if (masks){
								string maskRefFile;
								patternFileRename(infiles,refmaskfiles,*itRef,maskRefFile);
								maskRef = imread(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(maskRef.data != 0);
								CV_Assert(maskRef.type() == CV_8UC1);
								CV_Assert(maskRef.size() == codeSize);
							}
							else {
								maskRef = Mat();
							}

							double score = compare(imgSmpl,imgRef,from,bitStop,minShifts,maxShifts,alg,maskSmpl,maskRef,shiftedfiles);
							int idx = cvFloor(score*bins);
							if (idx == bins) idx--;
							if (!gsfile.empty()){
								gfile << score << endl;
							}
							genuines[idx]++;
							if (time && timing.update()) timing.print();
						}
						// imposter matches
						map<string, vector<string> >::iterator it2 = it;
						for (it2++; it2 != userTemplates.end(); it2++){
							for (vector<string>::iterator itRef = it2->second.begin(), itEnd2 = it2->second.end(); itRef != itEnd2; itRef++, timing.progress++){
								Mat imgRef = imread(*itRef, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(imgRef.data != 0);
								CV_Assert(imgRef.type() == CV_8UC1);
								CV_Assert(imgRef.size() == codeSize);
								Mat maskRef;
								if (masks){
									string maskRefFile;
									patternFileRename(infiles,maskfiles,*itRef,maskRefFile);
									maskRef = imread(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
									CV_Assert(maskRef.data != 0);
									CV_Assert(maskRef.type() == CV_8UC1);
									CV_Assert(maskRef.size() == codeSize);
								}
								else {
									maskRef = Mat();
								}
								double score = compare(imgSmpl,imgRef,from,bitStop,minShifts,maxShifts,alg,maskSmpl,maskRef,shiftedfiles);
								int idx = cvFloor(score*bins);
								if (idx == bins) idx--;
								if (!isfile.empty()){
									ifile << score << endl;
								}
								imposters[idx]++;
								if (time && timing.update()) timing.print();
							}
						}
					}
				}
			}
			else if (mod == EVAL_BALANCED){
				for (map<string, vector<string> >::iterator it = userTemplates.begin(); it != userTemplates.end(); it++){
					vector<string>::iterator itSample = it->second.begin(), itEnd = it->second.end();
					// special treatment for first sample
					vector<Mat> imgSmpl;
					vector<Mat> maskSmpl;
					if (shiftedfiles){
						string shiftfile;
						patternFileRename(infiles,shiftfiles,*itSample,shiftfile);
						vector<string> shiftsSmpl;
						patternToFiles(shiftfile,shiftsSmpl);
						CV_Assert(shiftsSmpl.size() > 0);
						// now load virtual files
						for (vector<string>::iterator shiftSmpl = shiftsSmpl.begin(); shiftSmpl != shiftsSmpl.end(); ++shiftSmpl){
							Mat img = imread(*shiftSmpl, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(img.data != 0);
							CV_Assert(img.type() == CV_8UC1);
							if (imgSmpl.size() > 0) { CV_Assert(imgSmpl.back().size() == img.size());}
							imgSmpl.push_back(img);
							if (masks){
								string maskfile1, maskfile2;
								patternFileRename(infiles,maskfiles,*itSample,maskfile1);
								patternFileRename(infiles,maskfile1,*itSample,maskfile2,'!');
								Mat msk = imread(maskfile2, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(msk.data != 0);
								CV_Assert(msk.type() == CV_8UC1);
								CV_Assert(img.size() == msk.size());
								maskSmpl.push_back(msk);
							}
						}
					}
					else {
						Mat img = imread(*itSample, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(img.data != 0);
						CV_Assert(img.type() == CV_8UC1);
						imgSmpl.push_back(img);
						if (masks){
							string maskSmplFile;
							patternFileRename(infiles,maskfiles,*itSample,maskSmplFile);
							Mat msk = imread(maskSmplFile, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(msk.data != 0);
							CV_Assert(msk.type() == CV_8UC1);
							CV_Assert(msk.size() == img.size());
							maskSmpl.push_back(msk);
						}
					}
					Size codeSize = imgSmpl[0].size();
					unsigned int codeLength = codeSize.height * codeSize.width;
					unsigned int bitStop = min(to,codeLength);
					CV_Assert(codeLength % sizeof(int) == 0);
					// genuine matches
					vector<string>::iterator itRef = itSample;
					for (itRef++; itRef != itEnd; itRef++, timing.progress++){
						Mat imgRef = imread(*itRef, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(imgRef.data != 0);
						CV_Assert(imgRef.type() == CV_8UC1);
						CV_Assert(imgRef.size() == codeSize);
						Mat maskRef;
						if (masks){
							string maskRefFile;
							patternFileRename(infiles,refmaskfiles,*itRef,maskRefFile);
							maskRef = imread(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(maskRef.data != 0);
							CV_Assert(maskRef.type() == CV_8UC1);
							CV_Assert(maskRef.size() == codeSize);
						}
						else {
							maskRef = Mat();
						}
						double score = compare(imgSmpl,imgRef,from,bitStop,minShifts,maxShifts,alg,maskSmpl,maskRef,shiftedfiles);
						int idx = cvFloor(score*bins);
						if (idx == bins) idx--;
						if (!gsfile.empty()){
							gfile << *itSample << ";" << *itRef << ";" << score << endl;
						}
						genuines[idx]++;
						if (time && timing.update()) timing.print();
					}
					// imposter matches
					map<string, vector<string> >::iterator it2 = it;
					for (it2++; it2 != userTemplates.end(); it2++, timing.progress++){
						itRef = it2->second.begin();
						Mat imgRef = imread(*itRef, CV_LOAD_IMAGE_UNCHANGED);
						CV_Assert(imgRef.data != 0);
						CV_Assert(imgRef.type() == CV_8UC1);
						CV_Assert(imgRef.size() == codeSize);
						Mat maskRef;
						if (masks){
							string maskRefFile;
							patternFileRename(infiles,refmaskfiles,*itRef,maskRefFile);
							maskRef = imread(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(maskRef.data != 0);
							CV_Assert(maskRef.type() == CV_8UC1);
							CV_Assert(maskRef.size() == codeSize);
						}
						else {
							maskRef = Mat();
						}
						double score = compare(imgSmpl,imgRef,from,bitStop,minShifts,maxShifts,alg,maskSmpl,maskRef, shiftedfiles);
						int idx = cvFloor(score*bins);
						if (idx == bins) idx--;
						if (!isfile.empty()){
							ifile << *itSample << ";" << *itRef << ";" << score << endl;
						}
						imposters[idx]++;
						if (time && timing.update()) timing.print();
					}
					// all other samples
					for (itSample++; itSample != itEnd; itSample++){
						imgSmpl.clear();
						maskSmpl.clear();
						if (shiftedfiles){
							string shiftfile;
							patternFileRename(infiles,shiftfiles,*itSample,shiftfile);
							vector<string> shiftsSmpl;
							patternToFiles(shiftfile,shiftsSmpl);
							// now load virtual files
							for (vector<string>::iterator shiftSmpl = shiftsSmpl.begin(); shiftSmpl != shiftsSmpl.end(); ++shiftSmpl){
								Mat img = imread(*shiftSmpl, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(img.data != 0);
								CV_Assert(img.type() == CV_8UC1);
								CV_Assert(img.size() == codeSize);
								imgSmpl.push_back(img);
								if (masks){
									string maskfile1, maskfile2;
									patternFileRename(infiles,maskfiles,*itSample,maskfile1);
									patternFileRename(infiles,maskfile1,*itSample,maskfile2,'!');
									Mat msk = imread(maskfile2, CV_LOAD_IMAGE_UNCHANGED);
									CV_Assert(msk.data != 0);
									CV_Assert(msk.type() == CV_8UC1);
									CV_Assert(msk.size() == codeSize);
									maskSmpl.push_back(msk);
								}
							}
						}
						else {
							Mat img = imread(*itSample, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(img.data != 0);
							CV_Assert(img.type() == CV_8UC1);
							CV_Assert(img.size() == codeSize);
							imgSmpl.push_back(img);
							if (masks){
								string maskSmplFile;
								patternFileRename(infiles,maskfiles,*itSample,maskSmplFile);
								Mat msk = imread(maskSmplFile, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(msk.data != 0);
								CV_Assert(msk.type() == CV_8UC1);
								CV_Assert(msk.size() == codeSize);
								maskSmpl.push_back(msk);
							}
						}
						// genuine matches
						itRef = itSample;
						for (itRef++; itRef != itEnd; itRef++, timing.progress++){
							Mat imgRef = imread(*itRef, CV_LOAD_IMAGE_UNCHANGED);
							CV_Assert(imgRef.data != 0);
							CV_Assert(imgRef.type() == CV_8UC1);
							CV_Assert(imgRef.size() == codeSize);
							Mat maskRef;
							if (masks){
								string maskRefFile;
								patternFileRename(infiles,refmaskfiles,*itRef,maskRefFile);
								maskRef = imread(maskRefFile, CV_LOAD_IMAGE_UNCHANGED);
								CV_Assert(maskRef.data != 0);
								CV_Assert(maskRef.type() == CV_8UC1);
								CV_Assert(maskRef.size() == codeSize);
							}
							else {
								maskRef = Mat();
							}
							double score = compare(imgSmpl,imgRef,from,bitStop,minShifts,maxShifts,alg,maskSmpl,maskRef, shiftedfiles);
							int idx = cvFloor(score*bins);
							if (idx == bins) idx--;
							if (!gsfile.empty()){
								gfile << *itSample << ";" << *itRef << ";" << score << endl;
							}
							genuines[idx]++;
							if (time && timing.update()) timing.print();
						}
					}
				}
			}
			if (time && quiet) timing.clear();
			if (!gsfile.empty()){
				gfile.close();
				ifile.close();
			}
			if (!dist.empty()){
				ofstream cfile;
				if (!quiet) printf("Storing distribution file '%s' ...\n",dist.c_str());
				cfile.open(dist.c_str(),ios::out | ios::trunc);
				if (cfile.is_open()) {
					double intervalSize = 1. / bins;
					for (int i=0; i<bins; i++){
						cfile << ((i + 0.5) * intervalSize) << " " << genuines[i] << " " << imposters[i] << " " << (100*((double)genuines[i]) / genuinesCount) << " " << (100*((double)imposters[i]) / impostersCount) << endl;
					}
					cfile.close();
				}
				else {
					CV_Error(CV_StsError,"Could not save distribution file '" + dist + "'");
				}
			}
			if (!roc.empty()){
				ofstream cfile;
				if (!quiet) printf("Storing roc file '%s' ...\n",roc.c_str());
				cfile.open(roc.c_str(),ios::out | ios::trunc);
				if (cfile.is_open()) {
					cfile << "0 100 0" << endl;
					int falseaccepts = 0;
					int falserejects = genuinesCount;
					int lastfalseaccepts = falseaccepts, lastfalserejects = falserejects;
					for (int i=0; i< bins; i++){
						falseaccepts += imposters[i];
						falserejects -= genuines[i];
						if (lastfalseaccepts != falseaccepts || lastfalserejects != falserejects) {
							double fmr = (100*((double)falseaccepts) / impostersCount), fnmr = (100*((double)falserejects) / genuinesCount);
							cfile << fmr << " " << fnmr << " " << (100-fnmr) << endl;
							lastfalserejects = falserejects;
							lastfalseaccepts = falseaccepts;
						}
					}
					cfile.close();
				}
				else {
					CV_Error(CV_StsError,"Could not save roc file '" + roc + "'");
				}
			}
			// calculate EER
			if (!quiet){
				int falseaccepts = 0;
				int falserejects = genuinesCount;
				int lastfalseaccepts = falseaccepts, lastfalserejects = falserejects;
				for (int i=0; i< bins; i++){
					falseaccepts += imposters[i];
					falserejects -= genuines[i];
					if (lastfalseaccepts != falseaccepts || lastfalserejects != falserejects) {
						double fmr = (((double)falseaccepts) / impostersCount), fnmr = (((double)falserejects) / genuinesCount);
						if (fmr == fnmr){
							printf("EER = %f%% at threshold t = %f\n",fmr,((i+1.)/bins));
							break;
						}
						else if (fmr > fnmr){ // interpolate
							double lastfmr = (((double)lastfalseaccepts) / impostersCount);
							double lastfnmr = (((double)lastfalserejects) / genuinesCount);
							printf("EER = %f%% at threshold t = %f \n", (100*(-lastfmr * fnmr + lastfnmr * fmr) / (lastfnmr - lastfmr - fnmr + fmr)), ((i+1.)/bins));
							break;
						}
						lastfalserejects = falserejects;
						lastfalseaccepts = falseaccepts;
					}
				}
			}
			delete [] genuines;
			delete [] imposters;
    	}
    	else if (mode == MODE_HELP){
    		// validate command line
			cmdCheckOpts(cmd,"-h");
			if (cmdGetOpt(cmd,"-h") != 0) cmdCheckOptSize(cmd,"-h",0);
			// starting routine
			printUsage();
    	}
    }
	catch (...){
		printf("Exit with errors.\n");
		exit(EXIT_FAILURE);
	}
    return EXIT_SUCCESS;
}