#include "SeparationAlgorithms.hpp"

#include "LinearConstraint.hpp"
#include "LinearVariableComposition.hpp"
#include "Model.hpp"
#include "SupportGraphs.hpp"
#include "Variable.hpp"
#include "WeightManager.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/range/iterator_range.hpp>

#include <xtensor/xtensor.hpp>
#include <xtensor/xvectorize.hpp>
#include <xtensor/xview.hpp>

namespace tsplp::graph
{
    using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
    using UndirectedGraph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty>;

    Separator::Separator(const xt::xtensor<Variable, 3>& variables, const WeightManager& weightManager, const Model& model)
        : m_variables(variables), m_weightManager(weightManager), m_model(model), m_spSupportGraph(std::make_unique<PiSigmaSupportGraph>(variables, weightManager.Dependencies(), model))
    {
    }

    Separator::~Separator() noexcept = default;

    std::optional<LinearConstraint> Separator::Ucut() const
    {
        const auto N = m_weightManager.N();
        const auto vf = xt::vectorize([this](Variable v) { return v.GetObjectiveValue(m_model); });
        const auto values = vf(m_variables);

        UndirectedGraph graph(N);
        for (size_t u = 0; u < N; ++u)
        {
            for (size_t v = u + 1; v < N; ++v)
            {
                const auto weight = xt::sum(xt::view(values, xt::all(), u, v))() + xt::sum(xt::view(values, xt::all(), v, u))();
                boost::add_edge(u, v, weight, graph);
            }
        }

        const auto parities = boost::make_one_bit_color_map(N, get(boost::vertex_index, graph));

        const auto cutSize = boost::stoer_wagner_min_cut(graph, get(boost::edge_weight, graph), boost::parity_map(parities));

        if (cutSize >= 2.0 - 1.e-10)
            return std::nullopt;

        LinearVariableComposition sum;
        for (size_t u = 0; u < N; ++u)
            for (size_t v = 0; v < N; ++v)
                if (get(parities, u) != get(parities, v))
                    sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

        return sum >= 2;
    }

    std::optional<LinearConstraint> Separator::Pi() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        const auto N = m_weightManager.N();
        const auto A = m_weightManager.A();

        for (size_t n = 0; n < N; ++n)
        {
            if (m_weightManager.Dependencies().GetIncomingSpan(n).empty())
                continue;

            for (size_t a = 0; a < A; ++a)
            {
                const auto e = m_weightManager.EndPositions()[a];
                if (n == e)
                    continue;

                const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(n, e, PiSigmaSupportGraph::ConstraintType::Pi);

                if (cutSize < 1.0 - 1.e-10)
                {

                    LinearVariableComposition sum;
                    for (const auto& [u, v] : cutEdges)
                        sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                    assert(std::abs(sum.Evaluate(m_model) - cutSize) < 1.e-10);
                    auto constraint = sum >= 1;
                    assert(!constraint.Evaluate(m_model));

                    return constraint;
                }
            }
        }

        return std::nullopt;
    }

    std::optional<LinearConstraint> Separator::Sigma() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        const auto N = m_weightManager.N();
        const auto A = m_weightManager.A();

        for (size_t n = 0; n < N; ++n)
        {
            if (m_weightManager.Dependencies().GetOutgoingSpan(n).empty())
                continue;

            for (size_t a = 0; a < A; ++a)
            {
                const auto s = m_weightManager.StartPositions()[a];
                if (n == s)
                    continue;

                const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(s, n, PiSigmaSupportGraph::ConstraintType::Sigma);

                if (cutSize < 1.0 - 1.e-10)
                {
                    LinearVariableComposition sum;
                    for (const auto& [u, v] : cutEdges)
                        sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                    auto constraint = sum >= 1;
                    assert(!constraint.Evaluate(m_model));

                    return constraint;
                }
            }
        }

        return std::nullopt;
    }

    std::optional<LinearConstraint> Separator::PiSigma() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        for (const auto& [s, t] : m_weightManager.Dependencies().GetArcs())
        {
            const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(s, t, PiSigmaSupportGraph::ConstraintType::PiSigma);

            if (cutSize < 1.0 - 1.e-10)
            {
                LinearVariableComposition sum;
                for (const auto& [u, v] : cutEdges)
                    sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                assert(std::abs(sum.Evaluate(m_model) - cutSize) < 1.e-10);
                auto constraint = sum >= 1;
                assert(!constraint.Evaluate(m_model));

                return constraint;
            }
        }

        return std::nullopt;
    }
}


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#ifndef __INPUT_DISTANCE_HPP__
#define __INPUT_DISTANCE_HPP__

#include "calibrationConfig.hpp"
#include "camera.hpp"

#include <ceres/ceres.h>
#include <Eigen/Dense>

#include <unordered_map>
#include <assert.h>

namespace VideoStitch {
namespace Calibration {

#define INDEX_HFOCAL_CAM1 0
#define INDEX_HFOCAL_CAM2 1
#define INDEX_VFOCAL_CAM1 2
#define INDEX_VFOCAL_CAM2 3
#define INDEX_HCENTER_CAM1 4
#define INDEX_HCENTER_CAM2 5
#define INDEX_VCENTER_CAM1 6
#define INDEX_VCENTER_CAM2 7
#define INDEX_LENSDISTORTA_CAM1 8
#define INDEX_LENSDISTORTA_CAM2 9
#define INDEX_LENSDISTORTB_CAM1 10
#define INDEX_LENSDISTORTB_CAM2 11
#define INDEX_LENSDISTORTC_CAM1 12
#define INDEX_LENSDISTORTC_CAM2 13
#define INDEX_ROTATION_CAM1 14
#define INDEX_ROTATION_CAM2 15
#define INDEX_TX_CAM1 16
#define INDEX_TX_CAM2 17
#define INDEX_TY_CAM1 18
#define INDEX_TY_CAM2 19
#define INDEX_TZ_CAM1 20
#define INDEX_TZ_CAM2 21
#define INDEX_3DPOINT 22
#define INDEX_LAST 23

#define SIZE_VFOCAL_CAM1 4
#define SIZE_VFOCAL_CAM2 4
#define SIZE_HFOCAL_CAM1 4
#define SIZE_HFOCAL_CAM2 4
#define SIZE_VCENTER_CAM1 4
#define SIZE_VCENTER_CAM2 4
#define SIZE_HCENTER_CAM1 4
#define SIZE_HCENTER_CAM2 4
#define SIZE_LENSDISTORTA_CAM1 4
#define SIZE_LENSDISTORTA_CAM2 4
#define SIZE_LENSDISTORTB_CAM1 4
#define SIZE_LENSDISTORTB_CAM2 4
#define SIZE_LENSDISTORTC_CAM1 4
#define SIZE_LENSDISTORTC_CAM2 4
#define SIZE_ROTATION_CAM1 9
#define SIZE_ROTATION_CAM2 9
#define SIZE_TX_CAM1 4
#define SIZE_TX_CAM2 4
#define SIZE_TY_CAM1 4
#define SIZE_TY_CAM2 4
#define SIZE_TZ_CAM1 4
#define SIZE_TZ_CAM2 4
#define SIZE_3DPOINT 3

class inputDistanceCostFunction : public ceres::CostFunction {
 public:
  inputDistanceCostFunction(const std::shared_ptr<Camera>& cam1, const std::shared_ptr<Camera>& cam2,
                            const Eigen::Vector2d& impt1, const Eigen::Vector2d& impt2, const CalibrationConfig& config,
                            const double sphereScale)
      : camera1(cam1->clone()),
        camera2(cam2->clone()),
        impt1(impt1),
        impt2(impt2),
        sphereScale(sphereScale),
        needsScaling(std::abs(sphereScale - 1.) > 1e-6) {
    // prepare hashmap of parameter indexes, to get continuously increasing indices and parameter availability through
    // the has() function not all of them may be used in a cost function, depending on the calibration config this
    // hashmap is used to find
    hashmap.reserve(INDEX_LAST);
    int index = 0;

    hashmap[INDEX_HFOCAL_CAM1] = index++;
    if (!config.hasSingleFocal()) {
      hashmap[INDEX_HFOCAL_CAM2] = index++;
    }
    hashmap[INDEX_VFOCAL_CAM1] = index++;
    if (!config.hasSingleFocal()) {
      hashmap[INDEX_VFOCAL_CAM2] = index++;
    }
    hashmap[INDEX_HCENTER_CAM1] = index++;
    hashmap[INDEX_HCENTER_CAM2] = index++;
    hashmap[INDEX_VCENTER_CAM1] = index++;
    hashmap[INDEX_VCENTER_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTA_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTA_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTB_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTB_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTC_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTC_CAM2] = index++;
    hashmap[INDEX_ROTATION_CAM1] = index++;
    hashmap[INDEX_ROTATION_CAM2] = index++;
    assert(index <= INDEX_LAST);

    set_num_residuals(2);
    std::vector<int>* blocks = mutable_parameter_block_sizes();

    // adds the block params if index is in hashmap
#define CONDITIONAL_ADD_BLOCK(PARAM) \
  if (has(INDEX_##PARAM)) blocks->push_back(SIZE_##PARAM)

    CONDITIONAL_ADD_BLOCK(HFOCAL_CAM1);
    CONDITIONAL_ADD_BLOCK(HFOCAL_CAM2);
    CONDITIONAL_ADD_BLOCK(VFOCAL_CAM1);
    CONDITIONAL_ADD_BLOCK(VFOCAL_CAM2);
    CONDITIONAL_ADD_BLOCK(HCENTER_CAM1);
    CONDITIONAL_ADD_BLOCK(HCENTER_CAM2);
    CONDITIONAL_ADD_BLOCK(VCENTER_CAM1);
    CONDITIONAL_ADD_BLOCK(VCENTER_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTA_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTA_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTB_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTB_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTC_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTC_CAM2);
    CONDITIONAL_ADD_BLOCK(ROTATION_CAM1);
    CONDITIONAL_ADD_BLOCK(ROTATION_CAM2);

#undef CONDITIONAL_ADD_BLOCK
  }

  bool has(char parameter_index) const {
    assert(parameter_index < INDEX_LAST);
    return hashmap.find(parameter_index) != hashmap.end();
  }

  size_t map(char parameter_index) const {
    assert(parameter_index < INDEX_LAST);
    return hashmap.at(parameter_index);
  }

  void resetJacobians(double** jacobians) const {
    if (jacobians == nullptr) {
      return;
    }

#define RESET_JACOBIAN(PARAM, ROWS, COLS)                             \
  if (has(INDEX_##PARAM) && jacobians[map(INDEX_##PARAM)] != nullptr) \
  Eigen::Map<Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor> >(jacobians[map(INDEX_##PARAM)]).fill(0)

    RESET_JACOBIAN(HFOCAL_CAM1, 2, 4);
    RESET_JACOBIAN(HFOCAL_CAM2, 2, 4);
    RESET_JACOBIAN(VFOCAL_CAM1, 2, 4);
    RESET_JACOBIAN(VFOCAL_CAM2, 2, 4);
    RESET_JACOBIAN(HCENTER_CAM1, 2, 4);
    RESET_JACOBIAN(HCENTER_CAM2, 2, 4);
    RESET_JACOBIAN(VCENTER_CAM1, 2, 4);
    RESET_JACOBIAN(VCENTER_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTA_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTA_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTB_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTB_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTC_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTC_CAM2, 2, 4);
    RESET_JACOBIAN(ROTATION_CAM1, 2, 9);
    RESET_JACOBIAN(ROTATION_CAM2, 2, 9);

#undef RESET_JACOBIAN
  }

  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    bool validProj, validLift;
    Eigen::Vector3d refpt;
    Eigen::Vector2d estimpt;
    Eigen::Matrix<double, 3, 4> Jhfocal_lift;
    Eigen::Matrix<double, 3, 4> Jvfocal_lift;
    Eigen::Matrix<double, 3, 4> Jhcenter_lift;
    Eigen::Matrix<double, 3, 4> Jvcenter_lift;
    Eigen::Matrix<double, 3, 4> JdistortA_lift;
    Eigen::Matrix<double, 3, 4> JdistortB_lift;
    Eigen::Matrix<double, 3, 4> JdistortC_lift;
    Eigen::Matrix<double, 3, 9> Jrotation_lift;
    Eigen::Matrix<double, 3, 4> JtX_lift;
    Eigen::Matrix<double, 3, 4> JtY_lift;
    Eigen::Matrix<double, 3, 4> JtZ_lift;
    Eigen::Matrix<double, 2, 3> Jpoint_project;
    Eigen::Matrix<double, 2, 4> Jhfocal_project;
    Eigen::Matrix<double, 2, 4> Jvfocal_project;
    Eigen::Matrix<double, 2, 4> Jhcenter_project;
    Eigen::Matrix<double, 2, 4> Jvcenter_project;
    Eigen::Matrix<double, 2, 4> JdistortA_project;
    Eigen::Matrix<double, 2, 4> JdistortB_project;
    Eigen::Matrix<double, 2, 4> JdistortC_project;
    Eigen::Matrix<double, 2, 9> Jrotation_project;
    Eigen::Matrix<double, 2, 4> JtX_project;
    Eigen::Matrix<double, 2, 4> JtY_project;
    Eigen::Matrix<double, 2, 4> JtZ_project;

    residuals[0] = 0.0;
    residuals[1] = 0.0;
    resetJacobians(jacobians);

    if (has(INDEX_HFOCAL_CAM1)) {
      camera1->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM1)]);
    }
    if (has(INDEX_HFOCAL_CAM2)) {
      camera2->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM2)]);
    } else {
      camera2->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM1)]);
    }
    if (has(INDEX_VFOCAL_CAM1)) {
      camera1->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM1)]);
    }
    if (has(INDEX_VFOCAL_CAM2)) {
      camera2->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM2)]);
    } else {
      camera2->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM1)]);
    }
    if (has(INDEX_HCENTER_CAM1)) {
      camera1->setHorizontalCenter(parameters[map(INDEX_HCENTER_CAM1)]);
    }
    if (has(INDEX_HCENTER_CAM2)) {
      camera2->setHorizontalCenter(parameters[map(INDEX_HCENTER_CAM2)]);
    }
    if (has(INDEX_VCENTER_CAM1)) {
      camera1->setVerticalCenter(parameters[map(INDEX_VCENTER_CAM1)]);
    }
    if (has(INDEX_VCENTER_CAM2)) {
      camera2->setVerticalCenter(parameters[map(INDEX_VCENTER_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTA_CAM1)) {
      camera1->setDistortionA(parameters[map(INDEX_LENSDISTORTA_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTA_CAM2)) {
      camera2->setDistortionA(parameters[map(INDEX_LENSDISTORTA_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTB_CAM1)) {
      camera1->setDistortionB(parameters[map(INDEX_LENSDISTORTB_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTB_CAM2)) {
      camera2->setDistortionB(parameters[map(INDEX_LENSDISTORTB_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTC_CAM1)) {
      camera1->setDistortionC(parameters[map(INDEX_LENSDISTORTC_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTC_CAM2)) {
      camera2->setDistortionC(parameters[map(INDEX_LENSDISTORTC_CAM2)]);
    }
    if (has(INDEX_ROTATION_CAM1)) {
      camera1->setRotation(parameters[map(INDEX_ROTATION_CAM1)]);
    }
    if (has(INDEX_ROTATION_CAM2)) {
      camera2->setRotation(parameters[map(INDEX_ROTATION_CAM2)]);
    }

    validLift =
        camera1->lift(refpt, Jhfocal_lift, Jvfocal_lift, Jhcenter_lift, Jvcenter_lift, JdistortA_lift, JdistortB_lift,
                      JdistortC_lift, Jrotation_lift, JtX_lift, JtY_lift, JtZ_lift, impt1, sphereScale);
    if (!validLift) {
      return true;
    }

    assert(std::abs(refpt.norm() - sphereScale) < 1e-6 && (bool)"lift() failed to put point at sphereScale");

    validProj = camera2->project(estimpt, Jpoint_project, Jhfocal_project, Jvfocal_project, Jhcenter_project,
                                 Jvcenter_project, JdistortA_project, JdistortB_project, JdistortC_project,
                                 Jrotation_project, JtX_project, JtY_project, JtZ_project, refpt);
    if (!validProj) {
      return true;
    }

    residuals[0] = estimpt(0) - impt2(0);
    residuals[1] = estimpt(1) - impt2(1);

    if (VS_ISNAN(residuals[0])) {
      residuals[0] = 0;
      residuals[1] = 0;
      return true;
    }

    if (jacobians == nullptr) {
      return true;
    }

#define SET_JACOBIAN(PARAM, ROWS, COLS, VALUE)                                                        \
  if (has(INDEX_##PARAM) && jacobians[map(INDEX_##PARAM)] != nullptr) {                               \
    Eigen::Map<Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor> > J(jacobians[map(INDEX_##PARAM)]); \
    J = (VALUE);                                                                                \
  }

    SET_JACOBIAN(HFOCAL_CAM1, 2, 4, Jpoint_project * Jhfocal_lift);
    SET_JACOBIAN(HFOCAL_CAM2, 2, 4, Jhfocal_project);
    SET_JACOBIAN(VFOCAL_CAM1, 2, 4, Jpoint_project * Jvfocal_lift);
    SET_JACOBIAN(VFOCAL_CAM2, 2, 4, Jvfocal_project);
    SET_JACOBIAN(HCENTER_CAM1, 2, 4, Jpoint_project * Jhcenter_lift);
    SET_JACOBIAN(HCENTER_CAM2, 2, 4, Jhcenter_project);
    SET_JACOBIAN(VCENTER_CAM1, 2, 4, Jpoint_project * Jvcenter_lift);
    SET_JACOBIAN(VCENTER_CAM2, 2, 4, Jvcenter_project);
    SET_JACOBIAN(LENSDISTORTA_CAM1, 2, 4, Jpoint_project * JdistortA_lift);
    SET_JACOBIAN(LENSDISTORTA_CAM2, 2, 4, JdistortA_project);
    SET_JACOBIAN(LENSDISTORTB_CAM1, 2, 4, Jpoint_project * JdistortB_lift);
    SET_JACOBIAN(LENSDISTORTB_CAM2, 2, 4, JdistortB_project);
    SET_JACOBIAN(LENSDISTORTC_CAM1, 2, 4, Jpoint_project * JdistortC_lift);
    SET_JACOBIAN(LENSDISTORTC_CAM2, 2, 4, JdistortC_project);
    SET_JACOBIAN(ROTATION_CAM1, 2, 9, Jpoint_project * Jrotation_lift);
    SET_JACOBIAN(ROTATION_CAM2, 2, 9, Jrotation_project);

#undef SET_JACOBIAN

    return true;
  }

 private:
  std::shared_ptr<Camera> camera1;
  std::shared_ptr<Camera> camera2;
  Eigen::Vector2d impt1;
  Eigen::Vector2d impt2;
  double sphereScale;
  bool needsScaling;
  std::unordered_map<char, size_t> hashmap;
};

}  // namespace Calibration
}  // namespace VideoStitch

#endif


#include <iostream>
#include <map>
#include "processing.h"
#include "feature_extraction.h"
#include "segmentation.h"
#include "util.h"
#include "classifier.h"

#include <opencv4/opencv2/opencv.hpp>
#include <boost/filesystem.hpp>

using namespace cv;
using namespace std;
namespace fs = boost::filesystem;

std::vector<double> image_to_vector(const Mat &I, bool show) {
    Mat focused_image = boundingBox(I);

    if (show) {
        bimshow("Segment", focused_image);
        waitKey(0);
    }

    double area = calculateArea(focused_image);
    double perimeter = calculatePerimeter(focused_image);
    Moments m = moments(focused_image, true);
    return {
            calculateShapeCoefficient(area, perimeter),
//        m.mu20, m.mu11, m.mu02, m.mu30, m.mu21, m.mu12, m.mu03,
//        m.nu20, m.nu11, m.nu02, m.nu30, m.nu21, m.nu12, m.nu03,
            calculateMomentInvariant(focused_image, 3),
            calculateMomentInvariant(focused_image, 7)
    };
}

std::vector<double> file_to_vector(const std::string &file_path) {
    Mat raw_image = cv::imread(file_path);

    Mat gray_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    toGrayscale(raw_image, gray_image);

    Mat binary_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    threshold(gray_image, binary_image, 128, 255);

    Mat morph_K = getStructuringElement(MORPH_RECT, Size(5, 5));
    dilate(binary_image, morph_K);
    erode(binary_image, morph_K);

    return image_to_vector(binary_image, false);
}

void plateDetector(Mat raw_image, Classifier clf) {
    Mat low_pass_K = Mat::ones(Size(5, 5), CV_64F) / 25;
    filter(raw_image, low_pass_K);

    Mat gray_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    toGrayscale(raw_image, gray_image);

    Mat binary_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    threshold(gray_image, binary_image, 192, 255);

    Mat morph_K = getStructuringElement(MORPH_ELLIPSE, Size(25, 25));
    dilate(binary_image, morph_K);
    erode(binary_image, morph_K);

    Mat segmented_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    std::vector<Point> points = generatePoints(segmented_image, 255, false);
    floodFill(binary_image, segmented_image, points, 1, true);

    std::set<uchar> distinct_values;
    for (int r = 0; r < segmented_image.rows; ++r) {
        for (int c = 0; c < segmented_image.cols; ++c) {
            distinct_values.insert(segmented_image.at<uchar>(r, c));
        }
    }
    distinct_values.erase(0);

    for (uchar value: distinct_values) {
        Mat shape = cv::Mat::zeros(raw_image.size(), CV_8UC1);
        for (int r = 0; r < segmented_image.rows; ++r) {
            for (int c = 0; c < segmented_image.cols; ++c) {
                shape.at<uchar>(r, c) = segmented_image.at<uchar>(r, c) == value ? SHAPE : BACKGROUND;
            }
        }

        if (calculateArea(shape) > 5000) {
            Matrix X;
            X.addRow(image_to_vector(shape, true));
            std::cout << clf.predict_proba(X) << std::endl;
        }
    }
}

void cutleryDetector(Mat raw_image, Classifier clf) {
    adjustBrightness(raw_image, -50);
    adjustContrast(raw_image, 50);

    Mat low_pass_K = Mat::ones(Size(5, 5), CV_64F) / 25;
    filter(raw_image, low_pass_K);

    Mat hsl_image = cv::Mat::zeros(raw_image.size(), CV_8UC3);
    toHSL(raw_image, hsl_image);

    Mat lsh[3];
    split(hsl_image, lsh);

    Mat l = lsh[0];
    Mat l_binary_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    threshold(l, l_binary_image, 16, 255);

    Mat s = lsh[1];
    Mat s_binary_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    threshold(s, s_binary_image, 0, 63);

    Mat binary_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    bitwise_and(l_binary_image, s_binary_image, binary_image);

    Mat segmented_image = cv::Mat::zeros(raw_image.size(), CV_8UC1);
    std::vector<Point> points = generatePoints(segmented_image, 255, false);
    floodFill(binary_image, segmented_image, points, 1, true);

    std::set<uchar> distinct_values;
    for (int r = 0; r < segmented_image.rows; ++r) {
        for (int c = 0; c < segmented_image.cols; ++c) {
            distinct_values.insert(segmented_image.at<uchar>(r, c));
        }
    }
    distinct_values.erase(0);

    for (uchar value: distinct_values) {
        Mat shape = cv::Mat::zeros(raw_image.size(), CV_8UC1);
        for (int r = 0; r < segmented_image.rows; ++r) {
            for (int c = 0; c < segmented_image.cols; ++c) {
                shape.at<uchar>(r, c) = segmented_image.at<uchar>(r, c) == value ? SHAPE : BACKGROUND;
            }
        }

        Mat morph_small_K = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));
        dilate(shape, morph_small_K);
        erode(shape, morph_small_K);

        if (calculateArea(shape) < 5000 && calculatePerimeter(shape) > 500) {
            Mat morph_big_K = getStructuringElement(MORPH_ELLIPSE, Size(9, 9));
            dilate(shape, morph_big_K);
            erode(shape, morph_big_K);

            Matrix X;
            X.addRow(image_to_vector(shape, true));
            std::cout << clf.predict_proba(X) << std::endl;
        }
    }
}

int main(int argc, char **argv) {
    // Load reference data
    Matrix X;
    Matrix y;
    std::map<std::string, std::vector<double>> name_to_class = {
            {"plate", {1.0, 0.0, 0.0, 0.0}},
            {"fork", {0.0, 1.0, 0.0, 0.0}},
            {"knife", {0.0, 0.0, 1.0, 0.0}},
            {"spoon", {0.0, 0.0, 0.0, 1.0}}
    };

    std::cout << "Loading references for: " << std::endl;
    fs::path targetDir("./data/resized/reference");
    fs::directory_iterator dir_it(targetDir);
    for (fs::path const &dir_path: dir_it) {
        fs::directory_iterator file_it(dir_path);
        std::cout << "\t* " << dir_path.filename().string() << std::endl;
        for (fs::path const &file_path: file_it) {
            X.addRow(file_to_vector(file_path.string()));
            y.addRow(name_to_class[dir_path.filename().string()]);
        }
    }

    std::cout << "Preparing classifier..." << std::endl;
    Classifier clf;
    clf.fit(X, y);
    std::cout << "Classifier accuracy on reference pictures: " << clf.accuracy(X, y) << std::endl;

    String paths[] = {
            "./data/resized/target/all.jpg",
            "./data/resized/target/fork.jpg",
            "./data/resized/target/knife.jpg",
            "./data/resized/target/plate.jpg",
            "./data/resized/target/spoon.jpg",
    };

    for (const String &path: paths) {
        Mat raw_image = cv::imread(path);
        Matrix vectors;

        namedWindow("Raw Image", WINDOW_NORMAL);
        imshow("Raw Image", raw_image);
        waitKey(0);

        plateDetector(raw_image.clone(), clf);
        cutleryDetector(raw_image.clone(), clf);
    }
    return 0;
}


/*
 * $Revision: 223 $ $Date: 2010-03-30 05:44:44 -0700 (Tue, 30 Mar 2010) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "Spectrum.h"
#include <Eigen/Array>
#include <Eigen/LU>
#include <cmath>

using namespace vesta;
using namespace Eigen;
using namespace std;

static float XYZtoSRGBMatrixValues[9] =
{
    3.2410f, -1.5374f, -0.4986f,
   -0.9692f,  1.8760f,  0.0416f,
    0.0556f, -0.2040f,  1.0570f
};

static Matrix3f XYZtoSRGB = Matrix3f::Map(XYZtoSRGBMatrixValues).transpose();
static Matrix3f SRGBtoXYZ = XYZtoSRGB.inverse();

/** Normalize the spectrum so that the largest component is equal
  * to 1.0.
  */
void
Spectrum::normalize()
{
    float maxValue = m_samples.cwise().abs().maxCoeff();
    if (maxValue > 0.0f)
    {
        m_samples /= maxValue;
    }
}

/** Convert from CIE XYZ color space to linear sRGB. sRGB gamma
  * correction must be applied in order to convert to the standard
  * sRGB color space. \see Spectrum::LinearSRGBtoSRGB
  */
Spectrum
Spectrum::XYZtoLinearSRGB(const Spectrum& xyz)
{
    Vector3f srgb = XYZtoSRGB * xyz.m_samples.start<3>();
    return Spectrum(srgb.x(), srgb.y(), srgb.z());
}

/** Convert from linear sRGB color space to CIE XYZ.
  */
Spectrum
Spectrum::LinearSRGBtoXYZ(const Spectrum& srgb)
{
    Vector3f xyz = SRGBtoXYZ * srgb.m_samples.start<3>();
    return Spectrum(xyz.x(), xyz.y(), xyz.z());
}

static float fromLinearSRGB(float x)
{
    return x < 0.0031308f ? 12.92f * x : 1.055 * pow(x, 1.0f / 2.4f) - 0.055f;
}

static float toLinearSRGB(float x)
{
    return x < 0.04045f ? x / 12.92f : pow((x + 0.055f) / 1.055f, 2.4f);
}

/** Apply the inverse sRGB gamma correction step to convert from 'linear sRGB'
  * color to sRGB color.
  */
Spectrum
Spectrum::LinearSRGBtoSRGB(const Spectrum& srgb)
{
    return Spectrum(fromLinearSRGB(srgb.red()),
                    fromLinearSRGB(srgb.green()),
                    fromLinearSRGB(srgb.blue()));
}

/** Apply the sRGB gamma correction step to convert from sRGB color space to
  * a linear color space that uses the sRGB tristimulus values.
  */
Spectrum
Spectrum::SRGBtoLinearSRGB(const Spectrum& srgb)
{
    return Spectrum(toLinearSRGB(srgb.red()),
                    toLinearSRGB(srgb.green()),
                    toLinearSRGB(srgb.blue()));
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Simulation/DistanceFieldCollisionDetection.h"
#include "Utils/OBJLoader.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setStiffness(const void* value, void* clientData);
void TW_CALL getStiffness(void* value, void* clientData);
void TW_CALL setPoissonRatio(const void* value, void* clientData);
void TW_CALL getPoissonRatio(void* value, void* clientData);
void TW_CALL setVolumeStiffness(const void* value, void* clientData);
void TW_CALL getVolumeStiffness(void* value, void* clientData);
void TW_CALL setNormalizeStretch(const void* value, void* clientData);
void TW_CALL getNormalizeStretch(void* value, void* clientData);
void TW_CALL setNormalizeShear(const void* value, void* clientData);
void TW_CALL getNormalizeShear(void* value, void* clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);
void TW_CALL setContactTolerance(const void *value, void *clientData);
void TW_CALL getContactTolerance(void *value, void *clientData);
void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData);
void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData);

DemoBase *base;
DistanceFieldCollisionDetection cd;

const unsigned int width = 30;
const unsigned int depth = 5;
const unsigned int height = 5;
short simulationMethod = 2;
Real stiffness = 1.0;
Real poissonRatio = 0.3;
bool normalizeStretch = false;
bool normalizeShear = false;
Real volumeStiffness = 1.0;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	base = new DemoBase();
	base->init(argc, argv, "Bar collision demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	base->createParameterGUI();

	// OpenGL
	MiniGL::setClientIdleFunc (timeStep);
	MiniGL::addKeyFunc('r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
			" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}, 5 {XPBD volume constraints}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "stiffness", TW_TYPE_REAL, setStiffness, getStiffness, model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Solid' ");
	TwAddVarCB(MiniGL::getTweakBar(), "poissonRatio", TW_TYPE_REAL, setPoissonRatio, getPoissonRatio, model, " label='Poisson ratio'  min=0.0 step=0.1 precision=4 group='Solid' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Solid' ");
	TwAddVarCB(MiniGL::getTweakBar(), "normalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Solid' ");
	TwAddVarCB(MiniGL::getTweakBar(), "volumeStiffness", TW_TYPE_REAL, setVolumeStiffness, getVolumeStiffness, model, " label='Volume stiffness'  min=0.0 step=0.1 precision=4 group='Solid' ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance'  min=0.0 step=0.001 precision=3 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, model, " label='Contact stiffness RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, model, " label='Contact stiffness Particle-RB'  min=0.0 step=0.1 precision=2 group=Simulation ");

	MiniGL::mainLoop ();

	base->cleanup();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();

	Simulation::getCurrent()->getModel()->cleanup();
	Simulation::getCurrent()->getTimeStep()->getCollisionDetection()->cleanup();

	buildModel();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTetModels().size(); i++)
		model->getTetModels()[i]->updateMeshNormals(model->getParticles());
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<OBJLoader::Vec2f> texCoords;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);

	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	const unsigned int nTexCoords = (unsigned int)texCoords.size();
	mesh.initMesh(nPoints, nFaces * 2, nFaces);
	vd.reserve(nPoints);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nTexCoords; i++)
	{
		mesh.addUV(texCoords[i][0], texCoords[i][1]);
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		int texIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
			if (nTexCoords > 0)
			{
				texIndices[j] = faces[i].texIndices[j] - 1;
				mesh.addUVIndex(texIndices[j]);
			}
		}

		mesh.addFace(&posIndices[0]);
	}
	mesh.buildNeighbors();

	mesh.updateNormals(vd, 0);
	mesh.updateVertexNormals(vd);

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));
	SimulationModel *model = Simulation::getCurrent()->getModel();

	createMesh();

	// create static rigid body
	string fileName = FileSystem::normalizePath(base->getExePath() + "/resources/models/cube.obj");
	IndexedFaceMesh mesh;
	VertexData vd;
	loadObj(fileName, vd, mesh, Vector3r::Ones());
	mesh.setFlatShading(true);

	string fileNameTorus = FileSystem::normalizePath(base->getExePath() + "/resources/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	loadObj(fileNameTorus, vdTorus, meshTorus, Vector3r::Ones());

	SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
	rb.resize(2);

	// floor
	rb[0] = new RigidBody();
	rb[0]->initBody(1.0,
		Vector3r(0.0, -5.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh,
		Vector3r(100.0, 1.0, 100.0));
	rb[0]->setMass(0.0);

	// torus
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(5.0, -1.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdTorus, meshTorus,
		Vector3r(3.0, 3.0, 3.0));
	rb[1]->setMass(0.0);
	rb[1]->setFrictionCoeff(static_cast<Real>(0.1));

	Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, &cd);
	cd.setTolerance(static_cast<Real>(0.05));

	const std::vector<Vector3r> &vertices1 = rb[0]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert1 = static_cast<unsigned int>(vertices1.size());
	cd.addCollisionBox(0, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, vertices1.data(), nVert1, Vector3r(100.0, 1.0, 100.0));

	const std::vector<Vector3r> &vertices2 = rb[1]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert2 = static_cast<unsigned int>(vertices2.size());
	cd.addCollisionTorus(1, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, vertices2.data(), nVert2, Vector2r(3.0, 1.5));

	SimulationModel::TetModelVector &tm = model->getTetModels();
	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < tm.size(); i++)
	{
		const unsigned int nVert = tm[i]->getParticleMesh().numVertices();
		unsigned int offset = tm[i]->getIndexOffset();
		tm[i]->setFrictionCoeff(static_cast<Real>(0.1));
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, true);
	}
}

void render ()
{
	base->render();
}

void createMesh()
{
	SimulationModel* model = Simulation::getCurrent()->getModel();
	model->addRegularTetModel(width, height, depth,
		Vector3r(4.5, 3, 0), Matrix3r::Identity(), Vector3r(9.0, 1.5, 1.5));

	// init constraints
	stiffness = 1.0;
	if (simulationMethod == 5)
		stiffness = 100000;

	volumeStiffness = 1.0;
	if (simulationMethod == 5)
		volumeStiffness = 100000;

	ParticleData& pd = model->getParticles();
	for (unsigned int cm = 0; cm < model->getTetModels().size(); cm++)
	{
		model->addSolidConstraints(model->getTetModels()[cm], simulationMethod, stiffness,
			poissonRatio, volumeStiffness, normalizeStretch, normalizeShear);

		model->getTetModels()[cm]->updateMeshNormals(pd);

		LOG_INFO << "Number of tets: " << model->getTetModels()[cm]->getParticleMesh().numTets();
		LOG_INFO << "Number of vertices: " << width * height * depth;
	}
}

void TW_CALL setStiffness(const void* value, void* clientData)
{
	stiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTetConstraint, Real, &FEMTetConstraint::m_stiffness>(stiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTetConstraint, Real, &StrainTetConstraint::m_stretchStiffness>(stiffness);
	((SimulationModel*)clientData)->setConstraintValue<StrainTetConstraint, Real, &StrainTetConstraint::m_shearStiffness>(stiffness);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint, Real, &DistanceConstraint::m_stiffness>(stiffness);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint_XPBD, Real, &DistanceConstraint_XPBD::m_stiffness>(stiffness);
	((SimulationModel*)clientData)->setConstraintValue<ShapeMatchingConstraint, Real, &ShapeMatchingConstraint::m_stiffness>(stiffness);
}

void TW_CALL getStiffness(void* value, void* clientData)
{
	*(Real*)(value) = stiffness;
}

void TW_CALL setVolumeStiffness(const void* value, void* clientData)
{
	volumeStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<VolumeConstraint, Real, &VolumeConstraint::m_stiffness>(volumeStiffness);
	((SimulationModel*)clientData)->setConstraintValue<VolumeConstraint_XPBD, Real, &VolumeConstraint_XPBD::m_stiffness>(volumeStiffness);
}

void TW_CALL getVolumeStiffness(void* value, void* clientData)
{
	*(Real*)(value) = volumeStiffness;
}

void TW_CALL getPoissonRatio(void* value, void* clientData)
{
	*(Real*)(value) = poissonRatio;
}

void TW_CALL setPoissonRatio(const void* value, void* clientData)
{
	poissonRatio = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<FEMTetConstraint, Real, &FEMTetConstraint::m_poissonRatio>(poissonRatio);
}

void TW_CALL getNormalizeStretch(void* value, void* clientData)
{
	*(bool*)(value) = normalizeStretch;
}

void TW_CALL setNormalizeStretch(const void* value, void* clientData)
{
	normalizeStretch = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTetConstraint, bool, &StrainTetConstraint::m_normalizeStretch>(normalizeStretch);
}

void TW_CALL getNormalizeShear(void* value, void* clientData)
{
	*(bool*)(value) = normalizeShear;
}

void TW_CALL setNormalizeShear(const void* value, void* clientData)
{
	normalizeShear = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<StrainTetConstraint, bool, &StrainTetConstraint::m_normalizeShear>(normalizeShear);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessRigidBody(val);
}

void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessRigidBody();
}

void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessParticleRigidBody(val);
}

void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessParticleRigidBody();
}

void TW_CALL setContactTolerance(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((DistanceFieldCollisionDetection*)clientData)->setTolerance(val);
}

void TW_CALL getContactTolerance(void *value, void *clientData)
{
	*(Real *)(value) = ((DistanceFieldCollisionDetection*)clientData)->getTolerance();
}


#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <igl/IO>
#include <igl/edges.h>
#include <igl/Timer.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/broad_phase/spatial_hash.hpp>
#include <ipc/broad_phase/brute_force.hpp>

using namespace ipc;

static const bool BENCHMARK_EV = false;
static const bool BENCHMARK_EE = false;
static const bool BENCHMARK_FV = true;

TEST_CASE(
    "Benchmark different spatial hashes",
    "[!benchmark][spatial_hash][hash_grid]")
{
    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;

#ifdef NDEBUG
    std::string filename =
        GENERATE(std::string("cube.obj"), std::string("bunny.obj"));
#else
    std::string filename = "cube.obj";
#endif
    std::string mesh_path = std::string(TEST_DATA_DIR) + filename;
    bool success = igl::read_triangle_mesh(mesh_path, V0, F);
    REQUIRE(success);

    VectorMax3d min_V = V0.colwise().minCoeff();
    VectorMax3d max_V = V0.colwise().maxCoeff();
    VectorMax3d center = (max_V + min_V) / 2;
    V0.rowwise() -= center.transpose();

    SECTION("Squish")
    {
        igl::edges(F, E);
        double s = GENERATE(0.1, 0.0, -0.1);
        V1 = V0;
        V1.col(0) *= s;
    }
    SECTION("Interobject")
    {
        VectorMax3d scale = max_V - min_V;
        for (int j = 0; j < V0.cols(); j++) {
            V0.col(j).array() /= scale(j);
        }

        int n = V0.rows();

        Eigen::MatrixXd V(2 * n, V0.cols());
        V.topRows(n) = V0;
        V.block(0, 0, n, 1).array() += 0.5;
        V.bottomRows(n) = V0;
        V.block(n, 0, n, 1).array() -= 0.5;
        V0 = V;

        double dx = GENERATE(0.0, 0.25, 0.75, 1.0);
        V1 = V0;
        V1.block(0, 0, n, 1).array() -= dx;
        V1.block(n, 0, n, 1).array() += dx;

        Eigen::MatrixXi F_stack(2 * F.rows(), F.cols());
        F_stack.topRows(F.rows()) = F;
        F_stack.bottomRows(F.rows()) = F.array() + n;
        F = F_stack;

        igl::edges(F, E);
    }

    double inflation_radius = 1e-2; // GENERATE(take(5, random(0.0, 0.1)));

    BENCHMARK("SpatialHash")
    {
        SpatialHash sh;
        sh.build(V0, V1, E, F, inflation_radius);

        Candidates candidates;
        sh.queryMeshForCandidates(
            V0, V1, E, F, candidates,
            /*queryEV=*/BENCHMARK_EV,
            /*queryEE=*/BENCHMARK_EE,
            /*queryFV=*/BENCHMARK_FV);
    };

    BENCHMARK("HashGrid")
    {
        HashGrid hashgrid;
        hashgrid.resize(V0, V1, E, inflation_radius);
        hashgrid.addVertices(V0, V1, inflation_radius);
        hashgrid.addEdges(V0, V1, E, inflation_radius);
        hashgrid.addFaces(V0, V1, F, inflation_radius);

        Candidates candidates;
        if (BENCHMARK_EV) {
            hashgrid.getVertexEdgePairs(E, candidates.ev_candidates);
        }
        if (BENCHMARK_EE) {
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates);
        }
        if (BENCHMARK_FV) {
            hashgrid.getFaceVertexPairs(F, candidates.fv_candidates);
        }
    };

    BENCHMARK("BruteForce")
    {
        Candidates candidates;
        detect_collision_candidates_brute_force(
            V0, V1, E, F, candidates,
            /*detect_edge_vertex=*/BENCHMARK_EV,
            /*detect_edge_edge=*/BENCHMARK_EE,
            /*detect_face_vertex=*/BENCHMARK_FV,
            /*perform_aabb_check=*/true, inflation_radius);
    };
}

TEST_CASE(
    "Benchmark different spatial hashes on real data",
    "[!benchmark][spatial_hash][hash_grid][real_data]")
{
    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;

    std::string mesh_path =
        std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s0.obj";
    bool success = igl::read_triangle_mesh(mesh_path, V0, F);
    if (!success) {
        return; // Data is private
    }

    mesh_path = std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s1.obj";
    success = igl::read_triangle_mesh(mesh_path, V1, F);
    if (!success) {
        return; // Data is private
    }

    igl::edges(F, E);

    double inflation_radius = 1e-2; // GENERATE(take(5, random(0.0, 0.1)));

    BENCHMARK("SpatialHash")
    {
        SpatialHash sh;
        sh.build(V0, V1, E, F, inflation_radius);

        Candidates candidates;
        sh.queryMeshForCandidates(
            V0, V1, E, F, candidates,
            /*queryEV=*/BENCHMARK_EV,
            /*queryEE=*/BENCHMARK_EE,
            /*queryFV=*/BENCHMARK_FV);
    };

    BENCHMARK("HashGrid")
    {
        HashGrid hashgrid;
        hashgrid.resize(V0, V1, E, inflation_radius);
        hashgrid.addVertices(V0, V1, inflation_radius);
        hashgrid.addEdges(V0, V1, E, inflation_radius);
        hashgrid.addFaces(V0, V1, F, inflation_radius);

        Candidates candidates;
        if (BENCHMARK_EV) {
            hashgrid.getVertexEdgePairs(E, candidates.ev_candidates);
        }
        if (BENCHMARK_EE) {
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates);
        }
        if (BENCHMARK_FV) {
            hashgrid.getFaceVertexPairs(F, candidates.fv_candidates);
        }
    };

    BENCHMARK("BruteForce")
    {
        Candidates candidates;
        detect_collision_candidates_brute_force(
            V0, V1, E, F, candidates,
            /*detect_edge_vertex=*/BENCHMARK_EV,
            /*detect_edge_edge=*/BENCHMARK_EE,
            /*detect_face_vertex=*/BENCHMARK_FV,
            /*perform_aabb_check=*/true, inflation_radius);
    };
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/*! \file PID.hpp
  \brief Proportional-Integral-Derivative Controller
*/

#ifndef PID_H
#define PID_H

#include "Actuator.hpp"
#include "SiconosAlgebraTypeDef.hpp"
#include <boost/circular_buffer.hpp>

class PID : public Actuator
{
private:
  /** default constructor */
  PID() {};

  /** serialization hooks
   */
  ACCEPT_SERIALIZATION(PID);

  /** error vector */
  std::shared_ptr<boost::circular_buffer<double> > _err;

  /** reference we are tracking */
  double _ref;

  double _curDeltaT;

  /** vector of gains */
  SP::SiconosVector _K;

public:

  /** Constructor.
   * \param sensor the ControlSensor feeding the Actuator
   * \param B the B matrix
   */
  PID(SP::ControlSensor sensor, SP::SimpleMatrix B = std::shared_ptr<SimpleMatrix>());

  /** destructor
   */
  virtual ~PID();

  /** initialize actuator data.
   * \param nsds a NonSmoothDynamicalSystem
   * \param s the simulation
   */
  virtual void initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s);

  /** Compute the new control law at each event
   * Here we are using the following formula:
   * \f$ u_k = u_{k-1} + c_1 e_k + c_2 e_{k-1} + c_3 e_{k-2} \f$ , where
   * \f{array} c_1 &= K_P - \frac{K_D}{\Delta t} + K_I \Delta t \\
   * c_2 &= -1 - \frac{2K_D}{\Delta t} \\
   * c_3 &= \frac{K_D}{\Delta t} \\
   * \f}
   */
  void actuate();

  /** Set K
   * \param K SP::SiconosVector \f$ [K_P, K_I, K_D] \f$
   */
  void setK(SP::SiconosVector K);

  /** Set the value of _ref to reference
   * \param reference the new value
   */
  void inline setRef(double reference)
  {
    _ref = reference;
  }

  /** Get the timestep from the TimeDiscretisation associated with this PID controller
  *  \param td the TimeDiscretisation for this Actuator
  */
  virtual void setTimeDiscretisation(const TimeDiscretisation& td);

  void setDeltaT(double deltaT)
  {
    _curDeltaT = deltaT;
  }
/** display the data of the Actuator on the standard output
   */
  virtual void display() const;

};
#endif


#include "DSHandler3D.h"

#include <Eigen/Geometry>

namespace SparsetoDense {
namespace gui {

using namespace pangolin;
using namespace std;

void DSHandler3D::UpdateModelViewMatrix() {
  // Do NOT do a barrel roll. In fact, don't roll at all because it's not really useful when
  // flying through a scene.
  double roll = 0.0f;

  // The ordering of the rotation angles is specific to InfiniTAM's axis conventions.
  Eigen::Quaterniond rot_quat = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(yaw_accum_, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pitch_accum_, Eigen::Vector3d::UnitZ());

  // This is a bit dirty, but it works for what we need.
  direction = rot_quat * Eigen::Vector3d(1.0f, 0.0f, 0.0f);

  Eigen::Vector3d target = eye + direction;
  cam_state_->SetModelViewMatrix(pangolin::ModelViewLookAt(eye[0], eye[1], eye[2],
                                 target[0], target[1], target[2],
                                 enforce_up_));
}

void DSHandler3D::MouseMotion(pangolin::View &view, int x, int y, int button_state) {

  const float delta[2] = {(float) x - last_pos_[0], (float) y - last_pos_[1]};
  const float mag = delta[0] * delta[0] + delta[1] * delta[1];

  if (mag < 50.0f * 50.0f) {
    const GLprecision *up = AxisDirectionVector[enforce_up_];
    if (button_state == MouseButtonLeft) {
      // Left drag: in-plane translate.
      Eigen::Vector3d up_v(up[0], up[1], up[2]);

      float dx = delta[0];
      float dy = -delta[1];

      Eigen::Vector3d newMotionX = direction.cross(up_v).normalized() * dx * 0.05f * trans_rot_scale_;
      Eigen::Vector3d newMotionY = direction.cross(up_v).cross(direction).normalized() * dy * 0.05f * trans_rot_scale_;
      eye += newMotionX;
      eye += newMotionY;

      UpdateModelViewMatrix();
    }
    else if (button_state == MouseButtonRight) {
      // Right drag: turn camera around.
      GLprecision aboutx = -0.005 * trans_rot_scale_ * delta[0];
      GLprecision abouty = -0.005 * trans_rot_scale_ * delta[1];

      yaw_accum_ -= aboutx;
      pitch_accum_ -= abouty;

      if (yaw_accum_ < -2 * M_PI) {
        yaw_accum_ += 2 * M_PI;
      }
      if (yaw_accum_ >  2 * M_PI) {
        yaw_accum_ -= 2 * M_PI;
      }

      float pitch_lim = static_cast<float>(M_PI_2 * 0.99);
      if (pitch_accum_ > pitch_lim) {
        pitch_accum_ = pitch_lim;
      }
      if (pitch_accum_ < -pitch_lim) {
        pitch_accum_ = -pitch_lim;
      }

      UpdateModelViewMatrix();
    }
  }

  last_pos_[0] = (float) x;
  last_pos_[1] = (float) y;
}

void DSHandler3D::Mouse(pangolin::View &view,
                        pangolin::MouseButton button,
                        int x,
                        int y,
                        bool pressed,
                        int button_state) {
  last_pos_[0] = static_cast<float>(x);
  last_pos_[1] = static_cast<float>(y);

  if (pressed) {
    if (button == pangolin::MouseWheelUp) {
      eye += direction.normalized() * zoom_scale_;
    }
    else if(button == pangolin::MouseWheelDown) {
      eye -= direction.normalized() * zoom_scale_;
    }

    UpdateModelViewMatrix();
  }
}

} // namespace gui
} // namespace SparsetoDense


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

// Original version: Melonee Wise <mwise@willowgarage.com>
#include <cstdio>
#include "youbot_driver/generic/PidController.hpp"
#include <boost/math/special_functions/fpclassify.hpp>

namespace youbot {

PidController::PidController(double P, double I, double D, double I1, double I2) :
  p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
  last_i_error = 0.0;
}

PidController::~PidController()
{
}

void PidController::initPid(double P, double I, double D, double I1, double I2)
{
  p_gain_ = P;
  i_gain_ = I;
  d_gain_ = D;
  i_max_ = I1;
  i_min_ = I2;

  reset();
}

void PidController::reset()
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
}

void PidController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
  p = p_gain_;
  i = i_gain_;
  d = d_gain_;
  i_max = i_max_;
  i_min = i_min_;
}

void PidController::setGains(double P, double I, double D, double I1, double I2)
{
  p_gain_ = P;
  i_gain_ = I;
  d_gain_ = D;
  i_max_ = I1;
  i_min_ = I2;
}

double PidController::updatePid(double error, boost::posix_time::time_duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  double deltatime = (double)dt.total_microseconds()/1000.0; //in milli seconds

  if (deltatime == 0.0 || boost::math::isnan(error) || boost::math::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error

  i_error_ = last_i_error + deltatime * p_error_;
  last_i_error = deltatime * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate the derivative error
  if (deltatime != 0)
  {
    d_error_ = (p_error_ - p_error_last_) / deltatime;
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = -p_term - i_term - d_term;

 // printf(" p_error_ %lf  i_error_ %lf  p_term %lf i_term %lf  dt %lf out %lf\n", p_error_, i_error_, p_term, i_term, deltatime, cmd_);

  return cmd_;
}

double PidController::updatePid(double error, double error_dot, boost::posix_time::time_duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  d_error_ = error_dot;
  double deltatime = (double)dt.total_microseconds()/1000.0;  //in milli seconds

  if (deltatime == 0.0 || boost::math::isnan(error) || boost::math::isinf(error) || boost::math::isnan(error_dot) || boost::math::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = last_i_error + deltatime * p_error_;
  last_i_error = deltatime * p_error_;

 // i_error_ = i_error_ + deltatime * p_error_;
 //   printf("i_error_ %lf dt.fractional_seconds() %lf\n", i_error_, deltatime);

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = -p_term - i_term - d_term;

  return cmd_;
}

void PidController::setCurrentCmd(double cmd)
{
  cmd_ = cmd;
}

double PidController::getCurrentCmd()
{
  return cmd_;
}

void PidController::getCurrentPIDErrors(double& pe, double& ie, double& de)
{
  pe = p_error_;
  ie = i_error_;
  de = d_error_;
}

}


/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

/*---------------------------------------------------------------
    APPLICATION: Camera calibration GUI
    AUTHOR: Jose Luis Blanco, based on code from the OpenCV library.
  ---------------------------------------------------------------*/

#include "camera_calib_guiMain.h"
#include "CDlgCalibWizardOnline.h"
#include "CDlgPoseEst.h"

//(*InternalHeaders(camera_calib_guiDialog)
#include <wx/settings.h>
#include <wx/font.h>
#include <wx/intl.h>
#include <wx/string.h>
//*)

#include <wx/filedlg.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>

#include <mrpt/gui/WxUtils.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>

#include <fstream>
#include <iostream>

#include <Eigen/Dense>
#include <mrpt/vision/pnp_algos.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::vision;
using namespace mrpt::gui;
using namespace std;

#include <mrpt/gui/CMyRedirector.h>

#include "CAboutBox.h"

#define USE_EXTERNAL_STORAGE_IMGS 1

// VARIABLES  ================================

TCalibrationImageList  lst_images;	// Here are all the images: file_name -> data
mrpt::utils::TCamera     camera_params;

// END VARIABLES  ============================

#include "imgs/icono_main.xpm"
#include "../wx-common/mrpt_logo.xpm"

// A custom Art provider for customizing the icons:
class MyArtProvider : public wxArtProvider
{
protected:
    virtual wxBitmap CreateBitmap(const wxArtID& id,
                                  const wxArtClient& client,
                                  const wxSize& size);
};

// CreateBitmap function
wxBitmap MyArtProvider::CreateBitmap(const wxArtID& id,
                                     const wxArtClient& client,
                                     const wxSize& size)
{
    if (id == wxART_MAKE_ART_ID(MAIN_ICON))   return wxBitmap(icono_main_xpm);
    if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO))  return wxBitmap(mrpt_logo_xpm);

    // Any wxWidgets icons not implemented here
    // will be provided by the default art provider.
    return wxNullBitmap;
}

//(*IdInit(camera_calib_guiDialog)
const long camera_calib_guiDialog::ID_BUTTON8 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON1 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON2 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON9 = wxNewId();
const long camera_calib_guiDialog::ID_LISTBOX1 = wxNewId();
const long camera_calib_guiDialog::ID_STATICTEXT5 = wxNewId();
const long camera_calib_guiDialog::ID_CHOICE1 = wxNewId();
const long camera_calib_guiDialog::ID_STATICTEXT1 = wxNewId();
const long camera_calib_guiDialog::ID_SPINCTRL1 = wxNewId();
const long camera_calib_guiDialog::ID_STATICTEXT2 = wxNewId();
const long camera_calib_guiDialog::ID_SPINCTRL2 = wxNewId();
const long camera_calib_guiDialog::ID_RADIOBOX1 = wxNewId();
const long camera_calib_guiDialog::ID_STATICTEXT3 = wxNewId();
const long camera_calib_guiDialog::ID_TEXTCTRL1 = wxNewId();
const long camera_calib_guiDialog::ID_STATICTEXT6 = wxNewId();
const long camera_calib_guiDialog::ID_TEXTCTRL3 = wxNewId();
const long camera_calib_guiDialog::ID_CHECKBOX1 = wxNewId();
const long camera_calib_guiDialog::ID_TEXTCTRL2 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON3 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON6 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON7 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON5 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON4 = wxNewId();
const long camera_calib_guiDialog::ID_CUSTOM2 = wxNewId();
const long camera_calib_guiDialog::ID_SCROLLEDWINDOW2 = wxNewId();
const long camera_calib_guiDialog::ID_PANEL2 = wxNewId();
const long camera_calib_guiDialog::ID_CUSTOM1 = wxNewId();
const long camera_calib_guiDialog::ID_SCROLLEDWINDOW3 = wxNewId();
const long camera_calib_guiDialog::ID_PANEL3 = wxNewId();
const long camera_calib_guiDialog::ID_XY_GLCANVAS = wxNewId();
const long camera_calib_guiDialog::ID_PANEL1 = wxNewId();
const long camera_calib_guiDialog::ID_NOTEBOOK1 = wxNewId();
const long camera_calib_guiDialog::ID_BUTTON10 = wxNewId();
//*)

BEGIN_EVENT_TABLE(camera_calib_guiDialog,wxDialog)
    //(*EventTable(camera_calib_guiDialog)
    //*)
END_EVENT_TABLE()

camera_calib_guiDialog::camera_calib_guiDialog(wxWindow* parent,wxWindowID id)
{
	// Load my custom icons:
#if wxCHECK_VERSION(2, 8, 0)
    wxArtProvider::Push(new MyArtProvider);
#else
    wxArtProvider::PushProvider(new MyArtProvider);
#endif

	//(*Initialize(camera_calib_guiDialog)
	wxStaticBoxSizer* StaticBoxSizer2;
	wxFlexGridSizer* FlexGridSizer4;
	wxFlexGridSizer* FlexGridSizer16;
	wxStaticBoxSizer* StaticBoxSizer4;
	wxFlexGridSizer* FlexGridSizer10;
	wxFlexGridSizer* FlexGridSizer3;
	wxFlexGridSizer* FlexGridSizer5;
	wxFlexGridSizer* FlexGridSizer9;
	wxFlexGridSizer* FlexGridSizer2;
	wxFlexGridSizer* FlexGridSizer7;
	wxStaticBoxSizer* StaticBoxSizer3;
	wxFlexGridSizer* FlexGridSizer15;
	wxFlexGridSizer* FlexGridSizer18;
	wxFlexGridSizer* FlexGridSizer8;
	wxFlexGridSizer* FlexGridSizer13;
	wxFlexGridSizer* FlexGridSizer12;
	wxFlexGridSizer* FlexGridSizer6;
	wxStaticBoxSizer* StaticBoxSizer1;
	wxFlexGridSizer* FlexGridSizer1;
	wxFlexGridSizer* FlexGridSizer17;
	wxStaticBoxSizer* StaticBoxSizer5;

	Create(parent, id, _("Camera calibration GUI - Part of the MRPT project"), wxDefaultPosition, wxDefaultSize, wxDEFAULT_DIALOG_STYLE|wxRESIZE_BORDER|wxMAXIMIZE_BOX, _T("id"));
	FlexGridSizer1 = new wxFlexGridSizer(1, 2, 0, 0);
	FlexGridSizer1->AddGrowableCol(1);
	FlexGridSizer1->AddGrowableRow(0);
	FlexGridSizer2 = new wxFlexGridSizer(3, 1, 0, 0);
	FlexGridSizer2->AddGrowableCol(0);
	FlexGridSizer2->AddGrowableRow(0);
	FlexGridSizer2->AddGrowableRow(2);
	StaticBoxSizer1 = new wxStaticBoxSizer(wxHORIZONTAL, this, _("List of images"));
	FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer4->AddGrowableCol(0);
	FlexGridSizer4->AddGrowableRow(1);
	FlexGridSizer5 = new wxFlexGridSizer(0, 4, 0, 0);
	btnCaptureNow = new wxButton(this, ID_BUTTON8, _("Grab now..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON8"));
	wxFont btnCaptureNowFont(wxDEFAULT,wxDEFAULT,wxFONTSTYLE_NORMAL,wxBOLD,false,wxEmptyString,wxFONTENCODING_DEFAULT);
	btnCaptureNow->SetFont(btnCaptureNowFont);
  	FlexGridSizer5->Add(btnCaptureNow, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	btnPoseEstimateNow = new wxButton(this, ID_BUTTON10, _("Pose Est. now..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON10"));
	wxFont btnPoseEstimateNowFont(wxDEFAULT,wxDEFAULT,wxFONTSTYLE_NORMAL,wxFONTWEIGHT_BOLD,false,wxEmptyString,wxFONTENCODING_DEFAULT);
	btnPoseEstimateNow->SetFont(btnPoseEstimateNowFont);
	FlexGridSizer5->Add(btnPoseEstimateNow, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	Button11 = new wxButton(this, ID_BUTTON1, _("Add image(s)..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON1"));
	FlexGridSizer5->Add(Button11, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	Button22 = new wxButton(this, ID_BUTTON2, _("Clear all"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON2"));
	FlexGridSizer5->Add(Button22, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	btnSaveImages = new wxButton(this, ID_BUTTON9, _("Save all..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON9"));
	btnSaveImages->Disable();
	FlexGridSizer5->Add(btnSaveImages, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	FlexGridSizer4->Add(FlexGridSizer5, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 0);
	FlexGridSizer15 = new wxFlexGridSizer(1, 2, 0, 0);
	FlexGridSizer15->AddGrowableCol(0);
	FlexGridSizer15->AddGrowableRow(0);
	lbFiles = new wxListBox(this, ID_LISTBOX1, wxDefaultPosition, wxSize(294,84), 0, 0, wxLB_ALWAYS_SB|wxVSCROLL|wxHSCROLL|wxALWAYS_SHOW_SB, wxDefaultValidator, _T("ID_LISTBOX1"));
	FlexGridSizer15->Add(lbFiles, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	FlexGridSizer16 = new wxFlexGridSizer(0, 1, 0, 0);
	StaticText5 = new wxStaticText(this, ID_STATICTEXT5, _("Zoom:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT5"));
	FlexGridSizer16->Add(StaticText5, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	cbZoom = new wxChoice(this, ID_CHOICE1, wxDefaultPosition, wxDefaultSize, 0, 0, 0, wxDefaultValidator, _T("ID_CHOICE1"));
	cbZoom->Append(_("25%"));
	cbZoom->Append(_("50%"));
	cbZoom->Append(_("75%"));
	cbZoom->SetSelection( cbZoom->Append(_("100%")) );
	cbZoom->Append(_("150%"));
	cbZoom->Append(_("200%"));
	cbZoom->Append(_("300%"));
	cbZoom->Append(_("400%"));
	cbZoom->Append(_("500%"));
	FlexGridSizer16->Add(cbZoom, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer15->Add(FlexGridSizer16, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer4->Add(FlexGridSizer15, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	StaticBoxSizer1->Add(FlexGridSizer4, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer2->Add(StaticBoxSizer1, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 2);
	StaticBoxSizer3 = new wxStaticBoxSizer(wxHORIZONTAL, this, _("Checkerboard detection parameters"));
	FlexGridSizer6 = new wxFlexGridSizer(2, 2, 0, 0);
	FlexGridSizer6->AddGrowableCol(0);
	FlexGridSizer6->AddGrowableCol(1);
	StaticBoxSizer4 = new wxStaticBoxSizer(wxHORIZONTAL, this, _("Number of inner corners: "));
	FlexGridSizer17 = new wxFlexGridSizer(1, 4, 0, 0);
	StaticText1 = new wxStaticText(this, ID_STATICTEXT1, _("In X:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	FlexGridSizer17->Add(StaticText1, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	edSizeX = new wxSpinCtrl(this, ID_SPINCTRL1, _T("9"), wxDefaultPosition, wxSize(50,-1), 0, 1, 200, 5, _T("ID_SPINCTRL1"));
	edSizeX->SetValue(_T("9"));
	FlexGridSizer17->Add(edSizeX, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText2 = new wxStaticText(this, ID_STATICTEXT2, _("In Y:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT2"));
	FlexGridSizer17->Add(StaticText2, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	edSizeY = new wxSpinCtrl(this, ID_SPINCTRL2, _T("6"), wxDefaultPosition, wxSize(50,-1), 0, 1, 200, 8, _T("ID_SPINCTRL2"));
	edSizeY->SetValue(_T("6"));
	FlexGridSizer17->Add(edSizeY, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer4->Add(FlexGridSizer17, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer6->Add(StaticBoxSizer4, 1, wxALL|wxALIGN_LEFT|wxALIGN_CENTER_VERTICAL, 2);
	wxString __wxRadioBoxChoices_1[2] =
	{
		_("OpenCV\'s default"),
		_("Scaramuzza et al.\'s")
	};
	rbMethod = new wxRadioBox(this, ID_RADIOBOX1, _(" Detector method: "), wxDefaultPosition, wxDefaultSize, 2, __wxRadioBoxChoices_1, 1, 0, wxDefaultValidator, _T("ID_RADIOBOX1"));
	FlexGridSizer6->Add(rbMethod, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 2);
	StaticBoxSizer5 = new wxStaticBoxSizer(wxHORIZONTAL, this, _(" Size of quads (in mm): "));
	FlexGridSizer18 = new wxFlexGridSizer(1, 4, 0, 0);
	StaticText3 = new wxStaticText(this, ID_STATICTEXT3, _("In X:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT3"));
	FlexGridSizer18->Add(StaticText3, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	edLengthX = new wxTextCtrl(this, ID_TEXTCTRL1, _("25.0"), wxDefaultPosition, wxSize(40,-1), 0, wxDefaultValidator, _T("ID_TEXTCTRL1"));
	FlexGridSizer18->Add(edLengthX, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticText6 = new wxStaticText(this, ID_STATICTEXT6, _("In Y:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT6"));
	FlexGridSizer18->Add(StaticText6, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	edLengthY = new wxTextCtrl(this, ID_TEXTCTRL3, _("25.0"), wxDefaultPosition, wxSize(40,-1), 0, wxDefaultValidator, _T("ID_TEXTCTRL3"));
	FlexGridSizer18->Add(edLengthY, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer5->Add(FlexGridSizer18, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer6->Add(StaticBoxSizer5, 1, wxALL|wxALIGN_LEFT|wxALIGN_CENTER_VERTICAL, 2);
	cbNormalize = new wxCheckBox(this, ID_CHECKBOX1, _("Normalize image"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX1"));
	cbNormalize->SetValue(true);
	FlexGridSizer6->Add(cbNormalize, 1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer3->Add(FlexGridSizer6, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer2->Add(StaticBoxSizer3, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 2);
	StaticBoxSizer2 = new wxStaticBoxSizer(wxHORIZONTAL, this, _("Calibration"));
	FlexGridSizer7 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer7->AddGrowableCol(0);
	FlexGridSizer7->AddGrowableRow(0);
	FlexGridSizer9 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer9->AddGrowableCol(0);
	FlexGridSizer9->AddGrowableRow(0);
	txtLog = new wxTextCtrl(this, ID_TEXTCTRL2, _("(Calibration results)"), wxDefaultPosition, wxDefaultSize, wxTE_MULTILINE|wxTE_READONLY|wxHSCROLL|wxVSCROLL, wxDefaultValidator, _T("ID_TEXTCTRL2"));
	wxFont txtLogFont = wxSystemSettings::GetFont(wxSYS_OEM_FIXED_FONT);
	if ( !txtLogFont.Ok() ) txtLogFont = wxSystemSettings::GetFont(wxSYS_DEFAULT_GUI_FONT);
	txtLogFont.SetPointSize(9);
	txtLog->SetFont(txtLogFont);
	FlexGridSizer9->Add(txtLog, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	FlexGridSizer7->Add(FlexGridSizer9, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer8 = new wxFlexGridSizer(2, 3, 0, 0);
	FlexGridSizer8->AddGrowableCol(0);
	FlexGridSizer8->AddGrowableCol(1);
	btnRunCalib = new wxButton(this, ID_BUTTON3, _("Calibrate"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON3"));
	btnRunCalib->SetDefault();
	wxFont btnRunCalibFont(wxDEFAULT,wxDEFAULT,wxFONTSTYLE_NORMAL,wxBOLD,false,wxEmptyString,wxFONTENCODING_DEFAULT);
	btnRunCalib->SetFont(btnRunCalibFont);
	FlexGridSizer8->Add(btnRunCalib, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	btnSave = new wxButton(this, ID_BUTTON6, _("Save matrices..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON6"));
	FlexGridSizer8->Add(btnSave, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	FlexGridSizer8->Add(-1,-1,1, wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	btnManualRect = new wxButton(this, ID_BUTTON7, _("Manual params..."), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON7"));
	FlexGridSizer8->Add(btnManualRect, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	btnAbout = new wxButton(this, ID_BUTTON5, _("About"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON5"));
	FlexGridSizer8->Add(btnAbout, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	btnClose = new wxButton(this, ID_BUTTON4, _("Quit"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON4"));
	FlexGridSizer8->Add(btnClose, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 5);
	FlexGridSizer7->Add(FlexGridSizer8, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	StaticBoxSizer2->Add(FlexGridSizer7, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer2->Add(StaticBoxSizer2, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 2);
	FlexGridSizer1->Add(FlexGridSizer2, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	FlexGridSizer3 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer3->AddGrowableCol(0);
	FlexGridSizer3->AddGrowableRow(0);
	Notebook1 = new wxNotebook(this, ID_NOTEBOOK1, wxDefaultPosition, wxSize(719,570), 0, _T("ID_NOTEBOOK1"));
	Panel2 = new wxPanel(Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2"));
	FlexGridSizer13 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer13->AddGrowableCol(0);
	FlexGridSizer13->AddGrowableRow(0);
	ScrolledWindow2 = new wxScrolledWindow(Panel2, ID_SCROLLEDWINDOW2, wxDefaultPosition, wxDefaultSize, wxHSCROLL|wxVSCROLL, _T("ID_SCROLLEDWINDOW2"));
	FlexGridSizer11 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer11->AddGrowableCol(0);
	FlexGridSizer11->AddGrowableRow(0);
	bmpOriginal = new mrpt::gui::wxMRPTImageControl(ScrolledWindow2,ID_CUSTOM2,wxDefaultPosition.x,wxDefaultPosition.y,wxSize(640,480).GetWidth(), wxSize(640,480).GetHeight() );
	FlexGridSizer11->Add(bmpOriginal, 1, wxALL|wxALIGN_LEFT|wxALIGN_TOP, 0);
	ScrolledWindow2->SetSizer(FlexGridSizer11);
	FlexGridSizer11->Fit(ScrolledWindow2);
	FlexGridSizer11->SetSizeHints(ScrolledWindow2);
	FlexGridSizer13->Add(ScrolledWindow2, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	Panel2->SetSizer(FlexGridSizer13);
	FlexGridSizer13->Fit(Panel2);
	FlexGridSizer13->SetSizeHints(Panel2);
	Panel3 = new wxPanel(Notebook1, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL3"));
	FlexGridSizer10 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer10->AddGrowableCol(0);
	FlexGridSizer10->AddGrowableRow(0);
	ScrolledWindow3 = new wxScrolledWindow(Panel3, ID_SCROLLEDWINDOW3, wxDefaultPosition, wxDefaultSize, wxHSCROLL|wxVSCROLL, _T("ID_SCROLLEDWINDOW3"));
	FlexGridSizer14 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer14->AddGrowableCol(0);
	FlexGridSizer14->AddGrowableRow(0);
	bmpRectified = new mrpt::gui::wxMRPTImageControl(ScrolledWindow3,ID_CUSTOM1,wxDefaultPosition.x,wxDefaultPosition.y,wxSize(640,480).GetWidth(), wxSize(640,480).GetHeight() );
	FlexGridSizer14->Add(bmpRectified, 1, wxALL|wxALIGN_LEFT|wxALIGN_TOP, 0);
	ScrolledWindow3->SetSizer(FlexGridSizer14);
	FlexGridSizer14->Fit(ScrolledWindow3);
	FlexGridSizer14->SetSizeHints(ScrolledWindow3);
	FlexGridSizer10->Add(ScrolledWindow3, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	Panel3->SetSizer(FlexGridSizer10);
	FlexGridSizer10->Fit(Panel3);
	FlexGridSizer10->SetSizeHints(Panel3);
	Panel1 = new wxPanel(Notebook1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
	FlexGridSizer12 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer12->AddGrowableCol(0);
	FlexGridSizer12->AddGrowableRow(0);
	m_3Dview = new CMyGLCanvas(Panel1,ID_XY_GLCANVAS,wxDefaultPosition,wxSize(-1,300),wxTAB_TRAVERSAL,_T("ID_XY_GLCANVAS"));
	FlexGridSizer12->Add(m_3Dview, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	Panel1->SetSizer(FlexGridSizer12);
	FlexGridSizer12->Fit(Panel1);
	FlexGridSizer12->SetSizeHints(Panel1);
	Notebook1->AddPage(Panel2, _("Original Image"), false);
	Notebook1->AddPage(Panel3, _("Rectified image and reprojected points"), true);
	Notebook1->AddPage(Panel1, _("3D view"), false);
	FlexGridSizer3->Add(Notebook1, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 2);
	FlexGridSizer1->Add(FlexGridSizer3, 1, wxALL|wxEXPAND|wxALIGN_LEFT|wxALIGN_TOP, 0);
	SetSizer(FlexGridSizer1);
	FlexGridSizer1->Fit(this);
	FlexGridSizer1->SetSizeHints(this);

	Connect(ID_BUTTON8,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnCaptureNowClick);
	Connect(ID_BUTTON1,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnAddImage);
	Connect(ID_BUTTON2,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnListClear);
	Connect(ID_BUTTON9,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnSaveImagesClick);
	Connect(ID_LISTBOX1,wxEVT_COMMAND_LISTBOX_SELECTED,(wxObjectEventFunction)&camera_calib_guiDialog::OnlbFilesSelect);
	Connect(ID_CHOICE1,wxEVT_COMMAND_CHOICE_SELECTED,(wxObjectEventFunction)&camera_calib_guiDialog::OncbZoomSelect);
	Connect(ID_BUTTON3,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnRunCalibClick);
	Connect(ID_BUTTON6,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnSaveClick);
	Connect(ID_BUTTON7,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnManualRectClick);
	Connect(ID_BUTTON5,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnAboutClick);
	Connect(ID_BUTTON4,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnCloseClick);
	Connect(ID_BUTTON10,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&camera_calib_guiDialog::OnbtnPoseEstimateNowClick);
	//*)

	camera_params.intrinsicParams(0,0) = 0; // Indicate calib didn't run yet.

	wxIcon icon;
	icon.CopyFromBitmap( wxBitmap(wxImage( icono_main_xpm )) );
	this->SetIcon( icon );

	this->show3Dview(); // Empty 3D scene

	Center();
	this->SetTitle( _U( format("Camera calibration %s - Part of the MRPT project",CAMERA_CALIB_GUI_VERSION).c_str() ) );
	Maximize();
}

camera_calib_guiDialog::~camera_calib_guiDialog()
{
    //(*Destroy(camera_calib_guiDialog)
    //*)
	this->clearListImages();
}

// Ask the user for new files to add to the list:
void camera_calib_guiDialog::OnAddImage(wxCommandEvent& event)
{
	try
	{

	wxFileDialog 	dlg(
		this,
		_("Select image(s) to open"),
		_("."),
		_(""),
		_("Image files (*.bmp;*.png;*.jpg)|*.bmp;*.png;*.jpg|All files (*.*)|*.*"),
		wxFD_OPEN | wxFD_FILE_MUST_EXIST | wxFD_MULTIPLE | wxFD_PREVIEW );

	if (wxID_OK!=dlg.ShowModal()) return;

	wxBusyCursor waitcur;

	wxArrayString files;
	dlg.GetPaths(files);

	wxProgressDialog    progDia(
		wxT("Adding image files"),
		wxT("Processing..."),
		files.Count() , // range
		this, // parent
		wxPD_CAN_ABORT |
		wxPD_APP_MODAL |
		wxPD_SMOOTH |
		wxPD_AUTO_HIDE |
		wxPD_ELAPSED_TIME |
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

	wxTheApp->Yield();  // Let the app. process messages

	int counter_loops=0;

	for (unsigned int i=0;i<files.Count();i++)
	{
		if (counter_loops++ % 5 == 0)
		{
			if (!progDia.Update( i ))
				break;
			wxTheApp->Yield();  // Let the app. process messages
		}

		const string fil = string(files[i].mb_str());

		TImageCalibData	dat;

#if USE_EXTERNAL_STORAGE_IMGS
		// Optimization: Use external storage:
		// ---------------------------------------
		//string tmp_check     = mrpt::system::getTempFileName()+".jpg";
		//string tmp_rectified = mrpt::system::getTempFileName()+".jpg";
		//dat.img_original.saveToFile(tmp_check);
		//dat.img_original.saveToFile(tmp_rectified);

		// mark all imgs. as external storage:
		dat.img_original.setExternalStorage(fil);
		//dat.img_checkboard.setExternalStorage(tmp_check);
		//dat.img_rectified.setExternalStorage(tmp_rectified);

		dat.img_original.unload();
		//dat.img_checkboard.unload();
		//dat.img_rectified.unload();
#else
		// All in memory:
		if (!dat.img_original.loadFromFile(fil))
		{
			wxMessageBox(_U(format("Error loading file: %s",fil.c_str()).c_str()), _("Error"));
			this->updateListOfImages();
			return;
		}

#endif

		lst_images[fil] = dat;
	}

	this->updateListOfImages();

	}
	catch(std::exception &e)
	{
		wxMessageBox(_U(e.what()),_("Error"),wxICON_INFORMATION,this);
	}
}

void camera_calib_guiDialog::clearListImages()
{
	lst_images.clear();
}

// Clear list of images.
void camera_calib_guiDialog::OnListClear(wxCommandEvent& event)
{
	this->clearListImages();
	this->updateListOfImages();
}

void camera_calib_guiDialog::OnbtnRunCalibClick(wxCommandEvent& event)
{
	try
	{

	txtLog->Clear();

	CMyRedirector	redire(txtLog, true, 10);

	const unsigned int  check_size_x = edSizeX->GetValue();
	const unsigned int  check_size_y = edSizeY->GetValue();
	const double        check_squares_length_X_meters = 0.001 * atof( string(edLengthX->GetValue().mb_str()).c_str() );
	const double        check_squares_length_Y_meters = 0.001 * atof( string(edLengthY->GetValue().mb_str()).c_str() );

	const bool			normalize_image = cbNormalize->GetValue();

	const bool			useScaramuzzaAlternativeDetector =  rbMethod->GetSelection() == 1;

	wxBusyCursor waitcur;

	bool res = mrpt::vision::checkerBoardCameraCalibration(
		lst_images,
		check_size_x,
		check_size_y,
		check_squares_length_X_meters,
		check_squares_length_Y_meters,
		camera_params,
		normalize_image,
		NULL /* MSE */,
		false /* skip draw */,
		useScaramuzzaAlternativeDetector);

	refreshDisplayedImage();

	if (!res)
		wxMessageBox(_("Calibration finished with error: Please check the text log to see what's wrong"), _("Error"));

	if (res)
		show3Dview();

	}
	catch(std::exception &e)
	{
		wxMessageBox(_U(e.what()),_("Error"),wxICON_INFORMATION,this);
	}
}

void camera_calib_guiDialog::OnbtnCloseClick(wxCommandEvent& event)
{
	Close();
}

void camera_calib_guiDialog::OnbtnAboutClick(wxCommandEvent& event)
{
	CAboutBox	dlg(this);
	dlg.ShowModal();
}

// save matrices:
void camera_calib_guiDialog::OnbtnSaveClick(wxCommandEvent& event)
{
	if (camera_params.intrinsicParams(0,0)==0)
	{
		wxMessageBox(_("Run the calibration first"),_("Error"));
		return;
	}

	{
		wxFileDialog 	dlg(
			this,
			_("Save intrinsic parameters matrix"),
			_("."),
			_("intrinsic_matrix.txt"),
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*"),
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT );

		if (wxID_OK!=dlg.ShowModal()) return;

		camera_params.intrinsicParams.saveToTextFile(	string(dlg.GetPath().mb_str()) );
	}

	{
		wxFileDialog 	dlg(
			this,
			_("Save distortion parameters"),
			_("."),
			_("distortion_matrix.txt"),
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*"),
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT );

		if (wxID_OK!=dlg.ShowModal()) return;

		CMatrixDouble  M(1,5);
		for (unsigned i=0;i<5;i++)
			M(0,i) = camera_params.dist[i];

		M.saveToTextFile( string(dlg.GetPath().mb_str()) );
	}
}

// Update the listbox from lst_img_files
void camera_calib_guiDialog::updateListOfImages()
{
	lbFiles->Clear();
	for (TCalibrationImageList::iterator s=lst_images.begin();s!=lst_images.end();++s)
		lbFiles->Append(_U(s->first.c_str()));

	btnSaveImages->Enable( !lst_images.empty() );

	refreshDisplayedImage();
}

// Shows the image selected in the listbox:
void camera_calib_guiDialog::refreshDisplayedImage()
{
	try
	{

	if (!lbFiles->GetCount())
	{
		// No images:
		return;
	}

	// Assure there's one selected:
	if (lbFiles->GetSelection()==wxNOT_FOUND)
		lbFiles->SetSelection(0);

	const string selFile = string(lbFiles->GetStringSelection().mb_str());

	TCalibrationImageList::iterator it = lst_images.find(selFile);
	if (it==lst_images.end()) return;

	// Zoom:
	const std::string strZoom = string(cbZoom->GetStringSelection().mb_str());
	const double zoomVal = 0.01*atof(strZoom.c_str());

	ASSERT_(zoomVal>0 && zoomVal<30)

	TImageSize  imgSizes(0,0);

	// Generate the images on-the-fly:
	CImage  imgOrgColor;
	it->second.img_original.colorImage(imgOrgColor);

	imgSizes = imgOrgColor.getSize();

	// Rectify:
	CImage  imgRect;
	if (camera_params.intrinsicParams(0,0)==0)
	{
		// Not calibrated yet:
		imgRect = imgOrgColor;
		imgRect.scaleImage(imgSizes.x*zoomVal,imgSizes.y*zoomVal, IMG_INTERP_NN);
	}
	else
	{
		imgOrgColor.rectifyImage(imgRect,camera_params);
		imgRect.scaleImage(imgSizes.x*zoomVal,imgSizes.y*zoomVal, IMG_INTERP_NN);

		// Draw reprojected:
		for (unsigned int k=0;k<it->second.projectedPoints_undistorted.size();k++)
			imgRect.drawCircle( zoomVal*it->second.projectedPoints_undistorted[k].x, zoomVal*it->second.projectedPoints_undistorted[k].y, 4, TColor(0,255,64) );

		imgRect.drawCircle( 10,10, 4, TColor(0,255,64) );
		imgRect.textOut(18,4,"Reprojected corners",TColor::white);
	}

	// Zoom images:
	imgOrgColor.scaleImage(imgSizes.x*zoomVal,imgSizes.y*zoomVal, IMG_INTERP_NN);

	imgSizes = imgOrgColor.getSize();

	CImage  imgCheck = imgOrgColor;

	// Draw the board:
	for (unsigned int k=0;k<it->second.detected_corners.size();k++)
	{
		imgCheck.cross(it->second.detected_corners[k].x *zoomVal, it->second.detected_corners[k].y *zoomVal, TColor::blue, '+', 3 );
		imgCheck.drawCircle( it->second.projectedPoints_distorted[k].x*zoomVal, it->second.projectedPoints_distorted[k].y*zoomVal, 4, TColor(0,255,64) );
	}
	imgCheck.drawCircle( 10,10, 4, TColor(0,255,64) );
	imgCheck.textOut(18,4,"Reprojected corners",TColor::white);

	imgCheck.cross( 10,30, TColor::blue, '+', 3 );
	imgCheck.textOut(18,24,"Detected corners",TColor::white);

	this->bmpOriginal->AssignImage( imgCheck );
	this->bmpRectified->AssignImage( imgRect );

	it->second.img_original.unload();

	// Plot:

	this->bmpOriginal->SetMinSize(wxSize(imgSizes.x,imgSizes.y));
	this->bmpOriginal->SetMaxSize(wxSize(imgSizes.x,imgSizes.y));
	this->bmpOriginal->SetSize(imgSizes.x,imgSizes.y);

	this->bmpRectified->SetMinSize(wxSize(imgSizes.x,imgSizes.y));
	this->bmpRectified->SetMaxSize(wxSize(imgSizes.x,imgSizes.y));
	this->bmpRectified->SetSize(imgSizes.x,imgSizes.y);

	this->FlexGridSizer11->RecalcSizes();
	this->FlexGridSizer14->RecalcSizes();

	//this->ScrolledWindow2->SetVirtualSize(wxSize(imgSizes.x,imgSizes.y));
	//this->ScrolledWindow3->SetVirtualSize(wxSize(imgSizes.x,imgSizes.y));
	this->ScrolledWindow2->SetScrollbars(1,1,imgSizes.x,imgSizes.y);
	this->ScrolledWindow3->SetScrollbars(1,1,imgSizes.x,imgSizes.y);

	this->bmpOriginal->Refresh(false);
	this->bmpRectified->Refresh(false);

	}
	catch(std::exception &e)
	{
		wxMessageBox(_U(e.what()),_("Error"),wxICON_INFORMATION,this);
	}
}

void camera_calib_guiDialog::OnlbFilesSelect(wxCommandEvent& event)
{
	refreshDisplayedImage();
}

void camera_calib_guiDialog::show3Dview()
{
	mrpt::opengl::COpenGLScenePtr	scene = mrpt::opengl::COpenGLScene::Create();

	const unsigned int  check_size_x = edSizeX->GetValue();
	const unsigned int  check_size_y = edSizeY->GetValue();
	const double        check_squares_length_X_meters = 0.001 * atof( string(edLengthX->GetValue().mb_str()).c_str() );
	const double        check_squares_length_Y_meters = 0.001 * atof( string(edLengthY->GetValue().mb_str()).c_str() );

	if (!check_squares_length_X_meters || !check_squares_length_Y_meters) return;

	opengl::CGridPlaneXYPtr	grid = opengl::CGridPlaneXY::Create(0,check_size_x*check_squares_length_X_meters, 0, check_size_y*check_squares_length_Y_meters, 0, check_squares_length_X_meters );
	scene->insert( grid );

	for (TCalibrationImageList::iterator it=lst_images.begin();it!=lst_images.end();++it)
	{
		if (!it->second.detected_corners.empty())
		{
			mrpt::opengl::CSetOfObjectsPtr	cor = mrpt::opengl::stock_objects::CornerXYZ();
			cor->setName( mrpt::system::extractFileName(it->first) );
			cor->enableShowName(true);
			cor->setScale(0.1f);
			cor->setPose( it->second.reconstructed_camera_pose );

			scene->insert( cor );
		}
	}

	//scene->insert( mrpt::opengl::stock_objects::CornerXYZ() );

	this->m_3Dview->m_openGLScene = scene;
	this->m_3Dview->Refresh();

}

// Enter calib. params manually:
void camera_calib_guiDialog::OnbtnManualRectClick(wxCommandEvent& event)
{
	wxMessageBox(_("Please, enter calibration parameters manually next to overpass automatically obtained parameters."),_("Manual parameters"));

	wxString s;

	if (camera_params.intrinsicParams(0,0)==0)
	{
		wxMessageBox(_("Run the calibration first"),_("Error"));
		return;
	}

	s = wxGetTextFromUser(_("Focus length in X pixel size (fx):"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.intrinsicParams(0,0)), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.intrinsicParams(0,0))) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Focus length in Y pixel size (fy):"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.intrinsicParams(1,1)), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.intrinsicParams(1,1))) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Image center X (cx):"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.intrinsicParams(0,2)), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.intrinsicParams(0,2))) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Image center Y (cy):"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.intrinsicParams(1,2)), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.intrinsicParams(1,2))) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Distortion param p1:"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.dist[0]), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.dist[0])) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Distortion param p2:"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.dist[1]), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.dist[1])) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Distortion param k1:"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.dist[2]), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.dist[2])) { wxMessageBox(_("Invalid number")); return; }

	s = wxGetTextFromUser(_("Distortion param k2:"),_("Manual parameters"),wxString::Format(wxT("%.07f"),camera_params.dist[3]), this );
	if (s.IsEmpty()) return;
	if (!s.ToDouble(&camera_params.dist[3])) { wxMessageBox(_("Invalid number")); return; }

	refreshDisplayedImage();
}

void camera_calib_guiDialog::OnbtnCaptureNowClick(wxCommandEvent& event)
{
  	CDlgCalibWizardOnline dlg(this);

	// Set pattern params:
	dlg.edLengthX->SetValue( this->edLengthX->GetValue() );
	dlg.edLengthY->SetValue( this->edLengthY->GetValue() );
	dlg.edSizeX->SetValue( this->edSizeX->GetValue() );
	dlg.edSizeY->SetValue( this->edSizeY->GetValue() );
	dlg.cbNormalize->SetValue( this->cbNormalize->GetValue() );

	// Run:
	dlg.ShowModal();

	// Get params:
	this->edLengthX->SetValue( dlg.edLengthX->GetValue() );
	this->edLengthY->SetValue( dlg.edLengthY->GetValue() );
	this->edSizeX->SetValue( dlg.edSizeX->GetValue() );
	this->edSizeY->SetValue( dlg.edSizeY->GetValue() );
	this->cbNormalize->SetValue( dlg.cbNormalize->GetValue() );

	// Get images:
	lst_images = dlg.m_calibFrames;
	this->updateListOfImages();

}

void camera_calib_guiDialog::OnbtnPoseEstimateNowClick(wxCommandEvent& event)
{
	// Compute pose using PnP Algorithms toolkit

  	CDlgPoseEst dlg(this);

	// Set pattern params:
	dlg.edLengthX->SetValue( this->edLengthX->GetValue() );
	dlg.edLengthY->SetValue( this->edLengthY->GetValue() );
	dlg.edSizeX->SetValue( this->edSizeX->GetValue() );
	dlg.edSizeY->SetValue( this->edSizeY->GetValue() );
	dlg.cbNormalize->SetValue( this->cbNormalize->GetValue() );

	// Run:
	dlg.ShowModal();

	// Get params:
	this->edLengthX->SetValue( dlg.edLengthX->GetValue() );
	this->edLengthY->SetValue( dlg.edLengthY->GetValue() );
	this->edSizeX->SetValue( dlg.edSizeX->GetValue() );
	this->edSizeY->SetValue( dlg.edSizeY->GetValue() );
	this->cbNormalize->SetValue( dlg.cbNormalize->GetValue() );

	// Get images:
	lst_images = dlg.m_calibFrames;
	this->updateListOfImages();
}

void camera_calib_guiDialog::OnbtnSaveImagesClick(wxCommandEvent& event)
{
	try
	{
		if (lst_images.empty()) return;

		wxDirDialog  dlg(this,_("Select the directory where to save the images"),_("."));

		if (dlg.ShowModal()==wxID_OK)
		{
			string dir = string(dlg.GetPath().mb_str());

			for (TCalibrationImageList::iterator s=lst_images.begin();s!=lst_images.end();++s)
				s->second.img_original.saveToFile( dir+string("/")+s->first+string(".png") );
		}
	}
	catch(std::exception &e)
	{
		wxMessageBox(_U(e.what()),_("Error"),wxICON_INFORMATION,this);
	}
}

void camera_calib_guiDialog::OncbZoomSelect(wxCommandEvent& event)
{
	refreshDisplayedImage();
}