/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/functional.hpp>

#include <tuple>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto ...xs) {
    return std::make_tuple('f', xs...);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto g = infix(f);

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

int main() {
    // disregard associativity
    BOOST_HANA_CONSTEXPR_ASSERT((x<0> ^g^ x<1>) == f(x<0>, x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(((x<0> ^g)^ x<1>) == f(x<0>, x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT((x<0> ^(g^ x<1>)) == f(x<0>, x<1>));

    // left partial application
    BOOST_HANA_CONSTEXPR_ASSERT((x<0>^g)(x<1>) == f(x<0>, x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT((x<0>^g)(x<1>, x<2>) == f(x<0>, x<1>, x<2>));
    BOOST_HANA_CONSTEXPR_ASSERT((x<0>^g)(x<1>, x<2>, x<3>) == f(x<0>, x<1>, x<2>, x<3>));

    // right partial application
    BOOST_HANA_CONSTEXPR_ASSERT((g^x<1>)(x<0>) == f(x<0>, x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT((g^x<2>)(x<0>, x<1>) == f(x<0>, x<1>, x<2>));
    BOOST_HANA_CONSTEXPR_ASSERT((g^x<3>)(x<0>, x<1>, x<2>) == f(x<0>, x<1>, x<2>, x<3>));

    // equivalence with the base function
    BOOST_HANA_CONSTEXPR_ASSERT(g(x<0>, x<1>) == f(x<0>, x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(g(x<0>, x<1>, x<2>) == f(x<0>, x<1>, x<2>));
    BOOST_HANA_CONSTEXPR_ASSERT(g(x<0>, x<1>, x<2>, x<3>) == f(x<0>, x<1>, x<2>, x<3>));
}


#ifndef FLOW_H_
#define FLOW_H_
#include <Eigen/Core>
#include <list>
#include <map>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

using namespace boost;
using namespace Eigen;

typedef int EdgeWeightType;

// clang-format off
typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
typedef adjacency_list < vecS, vecS, directedS,
    property < vertex_name_t, std::string,
    property < vertex_index_t, long,
    property < vertex_color_t, boost::default_color_type,
    property < vertex_distance_t, long,
    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,

    property < edge_capacity_t, EdgeWeightType,
    property < edge_residual_capacity_t, EdgeWeightType,
    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;
// clang-format on

void AddEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
             property_map<Graph, edge_reverse_t>::type& rev, const double capacity, Graph& g);

void AddDirectEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                   property_map<Graph, edge_reverse_t>::type& rev, const int capacity,
                   const int inv_capacity, Graph& g, Traits::edge_descriptor& e1,
                   Traits::edge_descriptor& e2);

class MaxFlowHelper {
   public:
    MaxFlowHelper() {}
    virtual ~MaxFlowHelper(){};
    virtual void resize(int n, int m) = 0;
    virtual int compute() = 0;
    virtual void AddEdge(int x, int y, int c, int rc, int v) = 0;
    virtual void Apply(std::vector<Vector2i>& edge_diff) = 0;
};

class BoykovMaxFlowHelper : public MaxFlowHelper {
   public:
    BoykovMaxFlowHelper() {
        rev = get(edge_reverse, g);
        num = 0;
    }
    int num;
    void resize(int n, int m) {
        vertex_descriptors.resize(n);
        for (int i = 0; i < n; ++i) {
            vertex_descriptors[i] = add_vertex(g);
        }
        num = n;
    }
    int compute() {
        EdgeWeightType flow =
            boykov_kolmogorov_max_flow(g, vertex_descriptors.front(), vertex_descriptors.back());
        return flow;
    }
    void AddEdge(int x, int y, int c, int rc, int v) {
        Traits::edge_descriptor e1, e2;
        AddDirectEdge(vertex_descriptors[x], vertex_descriptors[y], rev, c, rc, g, e1, e2);
        if (v != -1) {
            edge_to_variables[e1] = std::make_pair(v, -1);
            edge_to_variables[e2] = std::make_pair(v, 1);
        }
    }
    void Apply(std::vector<Vector2i>& edge_diff) {
        property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
        property_map<Graph, edge_residual_capacity_t>::type residual_capacity =
            get(edge_residual_capacity, g);

        graph_traits<Graph>::vertex_iterator u_iter, u_end;
        graph_traits<Graph>::out_edge_iterator ei, e_end;
        for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
            for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
                if (capacity[*ei] > 0) {
                    int flow = (capacity[*ei] - residual_capacity[*ei]);
                    if (flow > 0) {
                        auto it = edge_to_variables.find(*ei);
                        if (it != edge_to_variables.end()) {
                            edge_diff[it->second.first / 2][it->second.first % 2] +=
                                it->second.second * flow;
                        }
                        /*
                        int64_t key = (int64_t)*u_iter * num + target(*ei, g);
                        auto q = edge_to_variable[key];
                        edge_diff[q.first / 2][q.first % 2] += q.second * flow;
                        if (abs(edge_diff[q.first/2][q.first%2]) > 2) {
                            printf("Edge %d %d\n", *u_iter, target(*ei, g));
                            printf("Apply error 1: %d %d %d\n",
                                   edge_diff[q.first / 2][q.first % 2],
                                   q.second, flow);
                            printf("capacity %d\n", capacity[*ei]);
                            exit(0);
                        }
                         */
                    }
                }
    }
    Graph g;
    property_map<Graph, edge_reverse_t>::type rev;
    std::vector<Traits::vertex_descriptor> vertex_descriptors;
    std::map<Traits::edge_descriptor, std::pair<int, int>> edge_to_variables;
};

int flow(std::vector<std::map<int, std::pair<int, int>>>& graph);

inline void AddEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                    property_map<Graph, edge_reverse_t>::type& rev, const double capacity,
                    Graph& g) {
    Traits::edge_descriptor e1 = add_edge(v1, v2, g).first;
    Traits::edge_descriptor e2 = add_edge(v2, v1, g).first;
    put(edge_capacity, g, e1, capacity);
    put(edge_capacity, g, e2, capacity);

    rev[e1] = e2;
    rev[e2] = e1;
}

inline void AddDirectEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                          property_map<Graph, edge_reverse_t>::type& rev, const int capacity,
                          const int inv_capacity, Graph& g, Traits::edge_descriptor& e1,
                          Traits::edge_descriptor& e2) {
    e1 = add_edge(v1, v2, g).first;
    e2 = add_edge(v2, v1, g).first;
    put(edge_capacity, g, e1, capacity);
    put(edge_capacity, g, e2, inv_capacity);

    rev[e1] = e2;
    rev[e2] = e1;
}

class ECMaxFlowHelper : public MaxFlowHelper {
   public:
    struct FlowInfo {
        int id;
        int capacity, flow;
        int v, d;
        FlowInfo* rev;
    };
    struct SearchInfo {
        SearchInfo(int _id, int _prev_id, FlowInfo* _info)
            : id(_id), prev_id(_prev_id), info(_info) {}
        int id;
        int prev_id;
        FlowInfo* info;
    };
    ECMaxFlowHelper() { num = 0; }
    int num;
    std::vector<FlowInfo*> variable_to_edge;
    void resize(int n, int m) {
        graph.resize(n);
        variable_to_edge.resize(m, 0);
        num = n;
    }
    void AddEdge(int x, int y, int c, int rc, int v) {
        FlowInfo flow;
        flow.id = y;
        flow.capacity = c;
        flow.flow = 0;
        flow.v = v;
        flow.d = -1;
        graph[x].push_back(flow);
        auto& f1 = graph[x].back();
        flow.id = x;
        flow.capacity = rc;
        flow.flow = 0;
        flow.v = v;
        flow.d = 1;
        graph[y].push_back(flow);
        auto& f2 = graph[y].back();
        f2.rev = &f1;
        f1.rev = &f2;
    }

    void ApplyFlow(int v1, int v2, int flow) {
        for (auto& it : graph[v1]) {
            if (it.id == v2) {
                it.flow += flow;
                break;
            }
        }
    }
    int compute() {
        int total_flow = 0;
        int count = 0;
        while (true) {
            count += 1;
            std::vector<int> vhash(num, 0);
            std::vector<SearchInfo> q;
            q.push_back(SearchInfo(0, -1, 0));
            vhash[0] = 1;
            int q_front = 0;
            bool found = false;
            while (q_front < q.size()) {
                int vert = q[q_front].id;
                for (auto& l : graph[vert]) {
                    if (vhash[l.id] || l.capacity <= l.flow) continue;
                    q.push_back(SearchInfo(l.id, q_front, &l));
                    vhash[l.id] = 1;
                    if (l.id == num - 1) {
                        found = true;
                        break;
                    }
                }
                if (found) break;
                q_front += 1;
            }
            if (q_front == q.size()) break;
            int loc = q.size() - 1;
            while (q[loc].prev_id != -1) {
                q[loc].info->flow += 1;
                q[loc].info->rev->flow -= 1;
                loc = q[loc].prev_id;
                //                int prev_v = q[loc].id;
                //                ApplyFlow(prev_v, current_v, 1);
                //                ApplyFlow(current_v, prev_v, -1);
            }
            total_flow += 1;
        }
        return total_flow;
    }
    void Apply(std::vector<Vector2i>& edge_diff) {
        for (int i = 0; i < graph.size(); ++i) {
            for (auto& flow : graph[i]) {
                if (flow.flow > 0 && flow.v != -1) {
                    if (flow.flow > 0) {
                        edge_diff[flow.v / 2][flow.v % 2] += flow.d * flow.flow;
                        if (abs(edge_diff[flow.v / 2][flow.v % 2]) > 2) {
                        }
                    }
                }
            }
        }
    }
    std::vector<std::list<FlowInfo>> graph;
};
#endif


#ifndef _GRL_VREP_ROBOT_ARM_JACOBIAN_HPP_
#define _GRL_VREP_ROBOT_ARM_JACOBIAN_HPP_

#include <iostream>
#include <memory>
#include <array>

#include <boost/exception/all.hpp>
#include <boost/algorithm/string.hpp>

#include <spdlog/spdlog.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/Eigen.hpp"

#include "v_repLib.h"

namespace grl { namespace vrep {

/// @brief get the Jacobian as calculated by vrep in an Eigen::MatrixXf in column major format
/// @param driver the vrep arm driver object that provides access to vrep simulation state for a specific arm
/// @param jacobianOnly true calls simComputeJacobian, false uses simCheckIKGroup which incidentally calculates the Jacobian
///
/// The jacobianOnly option is provided because some irregularities were seen with
/// simComputeJacobian (jacobianOnly == true). Thus (jacobianOnly == false) falls
/// back on using the full vrep provided inverse kinematics calculation which is
/// slower but didn't have the previously seen irregularities. The jacobianOnly option
/// should be eliminated in favor of the simComputeJacobian (jacobianOnly == true) case
/// once all issues are fully resolved.
///
/// @return jacobian in ColumnMajor format where each row is a joint from base to tip, first 3 columns are translation component, last 3 columns are rotation component
Eigen::MatrixXf getJacobian(vrep::VrepRobotArmDriver& driver, bool jacobianOnly = false)
{

        // Initialize Variables for update
        auto jointHandles_ = driver.getJointHandles();
        int numJoints =jointHandles_.size();
        std::vector<float> ikCalculatedJointValues(numJoints,0);
        vrep::VrepRobotArmDriver::State currentArmState_;
        driver.getState(currentArmState_);

        /// @todo get target position, probably relative to base
        const auto& handleParams = driver.getVrepHandleParams();
        auto ikGroupHandle_ = std::get<vrep::VrepRobotArmDriver::RobotIkGroup>(handleParams);

        if (jacobianOnly)
        {
            // http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simComputeJacobian
            auto jacobianResult = simComputeJacobian(ikGroupHandle_,NULL,NULL);

            if(jacobianResult == -1)
            {
                spdlog::get("console")->error("VrepInverseKinematicsController: couldn't compute Jacobian");
                return Eigen::MatrixXf();
            }

        }
        else
        {

            auto target = std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams);
            auto tip = std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams);
            // save the current tip to target transform
            Eigen::Affine3d tipToTarget =getObjectTransform( target,tip);

            // set the current transform to the identity so simCheckIkGroup won't fail
            // @see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=3967
            // for details about this issue.
            setObjectTransform( target,tip,Eigen::Affine3d::Identity());

            // debug:
            // std::cout << "TipToTargetTransform:\n" << tipToTarget.matrix() << "\n";

            /// Run inverse kinematics, but all we really want is the jacobian
            /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simCheckIkGroup
            auto ikcalcResult =
            simCheckIkGroup(ikGroupHandle_
                           ,numJoints
                           ,&jointHandles_[0]
                           ,&ikCalculatedJointValues[0]
                           ,NULL /// @todo do we need to use these options?
                           );
            /// @see http://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#ikCalculationResults
            if(ikcalcResult!=sim_ikresult_success && ikcalcResult != sim_ikresult_not_performed)
            {
                spdlog::get("console")->error("VrepInverseKinematicsController: didn't run inverse kinematics");
                return Eigen::MatrixXf();
            }

            setObjectTransform( target,tip,tipToTarget);
        }

        // debug verifying that get and set object transform don't corrupt underlying data
//        Eigen::Affine3d tipToTarget2 =getObjectTransform( target,tip);
//        std::cout << "\ntiptotarget\n" << tipToTarget.matrix();
//        std::cout << "\ntiptotarget2\n" << tipToTarget2.matrix();

        // Get the Jacobian
        int jacobianSize[2];
        float* jacobian=simGetIkGroupMatrix(ikGroupHandle_,0,jacobianSize);
        /// @todo FIX HACK jacobianSize include orientation component, should be 7x6 instead of 7x3

#ifdef IGNORE_ROTATION
        jacobianSize[1] = 3;
#endif

        // Transfer the Jacobian to cisst

        // jacobianSize[1] represents the row count of the Jacobian
        // (i.e. the number of equations or the number of joints involved
        // in the IK resolution of the given kinematic chain)
        // Joints appear from tip to base.

        // jacobianSize[0] represents the column count of the Jacobian
        // (i.e. the number of constraints, e.g. x,y,z,alpha,beta,gamma,jointDependency)

        // The Jacobian data is in RowMajor order, i.e.:
        // https://en.wikipedia.org/wiki/Row-major_order

        std::string str;

        // jacobianSize[1] == eigen ColMajor "rows" , jacobianSize[0] == eigen ColMajor "cols"

        Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> > mtestjacobian(jacobian,jacobianSize[1],jacobianSize[0]);
        Eigen::MatrixXf eigentestJacobian = mtestjacobian.rowwise().reverse().transpose();

        return eigentestJacobian;
}

}} // grl::vrep

#endif // _GRL_VREP_ROBOT_ARM_JACOBIAN_HPP_


#define BOOST_TEST_MODULE "test_read_pulling_force_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/input/read_external_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_pulling_force_interaction)
{
    mjolnir::LoggerManager::set_default_logger("test_read_pulling_force_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "PullingForce"
            parameters  = [
                {index =   0, force = [ 1.0, 2.0, 10.0]},
                {index = 100, force = [-5.0, 0.0,  0.0]},
                {index = 100, force = 0.0144, direction = [1.0, 2.0, 3.0]},
            ]
            )"_toml;

        const auto base = mjolnir::read_external_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::PullingForceInteraction<traits_type>*>(base.get());
        BOOST_TEST(static_cast<bool>(derv));

        const auto& interaction = *derv;

        BOOST_TEST(interaction.parameters().at(0).first ==   0);
        BOOST_TEST(interaction.parameters().at(1).first == 100);

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(0).second) ==  1.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(0).second) ==  2.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(0).second) == 10.0, boost::test_tools::tolerance(1e-8));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(1).second) ==-5.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(1).second) == 0.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(1).second) == 0.0, boost::test_tools::tolerance(1e-8));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(2).second) == 1.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(2).second) == 2.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(2).second) == 3.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
    }
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <iotbx/error.h>
#include <cctbx/miller.h>
#include <cctbx/import_scitbx_af.h>
#include <scitbx/array_family/shared.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <fstream>
#include <vector>

namespace iotbx { namespace boost_python { namespace scalepack_ext {

  namespace miller = cctbx::miller;
  namespace af = scitbx::af;

  class read_formatted
  {
    public:
      read_formatted() {}

      void
      new_line(std::string const& line)
      {
        len_ = line.size();
        pos_ = 0;
        buffer_.resize(len_+1);
        for(std::size_t i=0;i<len_;i++) buffer_[i] = line[i];
        buffer_[len_] = '\0';
      }

      const char*
      next_field(std::size_t w, char fill_char)
      {
        field_.resize(w+1);
        std::size_t i;
        for(i=0; i < w && pos_ < len_; i++, pos_++) {
          field_[i] = buffer_[pos_];
        }
        for(; i < w; i++) {
          field_[i] = fill_char;
        }
        field_[w] = '\0';
        return &*field_.begin();
      }

      int
      next_int(std::size_t w)
      {
        return std::atoi(next_field(w, '0'));
      }

      double
      next_double(std::size_t w)
      {
        return std::atof(next_field(w, '0'));
      }

      miller::index<>
      next_miller_index(std::size_t w)
      {
        miller::index<> result;
        for(std::size_t i=0;i<3;i++) {
          result[i] = next_int(w);
        }
        return result;
      }

    protected:
      std::size_t len_;
      std::size_t pos_;
      std::vector<char> buffer_;
      std::vector<char> field_;
  };

  class no_merge_original_index_arrays
  {
    public:
      no_merge_original_index_arrays(
        std::string const& file_name,
        std::size_t n_lines_skip)
      {
        std::ifstream cin(file_name.c_str());
        std::string line;
        for (std::size_t i=0;i<n_lines_skip;i++) {
          std::getline(cin, line);
          IOTBX_ASSERT(!cin.eof());
        }
        read_formatted rf;
        while (true) {
          std::getline(cin, line);
          if (cin.eof()) break;
          rf.new_line(line);
          original_indices_.push_back(rf.next_miller_index(4));
          unique_indices_.push_back(rf.next_miller_index(4));
          batch_numbers_.push_back(rf.next_int(6));
          centric_tags_.push_back(rf.next_int(2));
          spindle_flags_.push_back(rf.next_int(2));
          asymmetric_unit_indices_.push_back(rf.next_int(3));
          i_obs_.push_back(rf.next_double(8));
          sigmas_.push_back(rf.next_double(8));
        }
      }

      af::shared<miller::index<> >
      original_indices() const { return original_indices_; }

      af::shared<miller::index<> >
      unique_indices() const { return unique_indices_; }

      af::shared<int>
      batch_numbers() const { return batch_numbers_; }

      af::shared<int>
      centric_tags() const { return centric_tags_; }

      af::shared<int>
      asymmetric_unit_indices() const { return asymmetric_unit_indices_; }

      af::shared<int>
      spindle_flags() const { return spindle_flags_; }

      af::shared<double>
      i_obs() const { return i_obs_; }

      af::shared<double>
      sigmas() const { return sigmas_; }

    private:
      af::shared<miller::index<> > original_indices_;
      af::shared<miller::index<> > unique_indices_;
      af::shared<int> batch_numbers_;
      af::shared<int> centric_tags_;
      af::shared<int> asymmetric_unit_indices_;
      af::shared<int> spindle_flags_;
      af::shared<double> i_obs_;
      af::shared<double> sigmas_;
  };

  struct no_merge_original_index_arrays_wrappers
  {
    typedef no_merge_original_index_arrays w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("no_merge_original_index_arrays", no_init)
        .def(init<std::string const&, std::size_t>())
        .def("original_indices", &w_t::original_indices)
        .def("unique_indices", &w_t::unique_indices)
        .def("batch_numbers", &w_t::batch_numbers)
        .def("centric_tags", &w_t::centric_tags)
        .def("asymmetric_unit_indices", &w_t::asymmetric_unit_indices)
        .def("spindle_flags", &w_t::spindle_flags)
        .def("i_obs", &w_t::i_obs)
        .def("sigmas", &w_t::sigmas)
      ;
    }
  };

  void init_module()
  {
    no_merge_original_index_arrays_wrappers::wrap();
  }

}}} // namespace iotbx::boost_python::scalepack_ext

BOOST_PYTHON_MODULE(iotbx_scalepack_ext)
{
  iotbx::boost_python::scalepack_ext::init_module();
}


#pragma once

#include <Eigen/Core>

#include <vector>

#include "../stateobject.hh"
#include "../../util/Maybe.hh"
#include "../../geometry/LineSegment/linesegment.hh"
#include "../../geometry/Polygon2.hh"
#include "../../LineJunctionFinder/linejunctionfinder.hh"
#include "../../OcclusionRay/occlusionray.hh"

namespace bold
{
  class AgentFrameState : public StateObject
  {
  public:
    AgentFrameState(Maybe<Eigen::Vector3d> ballObservation,
                    std::vector<Eigen::Vector3d> goalObservations,
                    std::vector<Eigen::Vector3d> teamMateObservations,
                    std::vector<LineSegment3d> observedLineSegments,
                    std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> observedLineJunctions,
                    Maybe<Polygon2d> visibleFieldPoly,
                    std::vector<OcclusionRay<double>> occlusionRays,
                    ulong thinkCycleNumber);

    Maybe<Eigen::Vector3d> getBallObservation() const { return d_ballObservation; }
    std::vector<Eigen::Vector3d> getGoalObservations() const { return d_goalObservations; }
    std::vector<Eigen::Vector3d> getTeamMateObservations() const { return d_teamMateObservations; }
    std::vector<LineSegment3d> getObservedLineSegments() const { return d_observedLineSegments; }
    std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> getObservedLineJunctions() const { return d_observedLineJunctions; }
    Maybe<Polygon2d> getVisibleFieldPoly() const { return d_visibleFieldPoly; }
    std::vector<OcclusionRay<double>> getOcclusionRays() const { return d_occlusionRays; }

    static Maybe<Polygon2d> getOcclusionPoly(std::vector<OcclusionRay<double>> const& occlusionRays);
    Maybe<Polygon2d> getOcclusionPoly() const;

    bool isBallVisible() const { return d_ballObservation.hasValue(); }
    ulong getThinkCycleNumber() const { return d_thinkCycleNumber; }

    uint goalObservationCount() const { return d_goalObservations.size(); }

    Maybe<Eigen::Vector3d> getClosestGoalObservation() const;

    double getOcclusionDistance(double angle) const;

    void writeJson(rapidjson::Writer<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::PrettyWriter<rapidjson::StringBuffer>& writer) const override { writeJsonInternal(writer); }
    void writeJson(rapidjson::Writer<WebSocketBuffer>& writer) const override { writeJsonInternal(writer); }

    bool shouldSeeAgentFrameGroundPoint(Eigen::Vector2d groundAgent) const;

    bool isNearBall(Eigen::Vector2d point, double maxDistance) const;
    bool isNearGoal(Eigen::Vector2d point, double maxDistance) const;

  private:
    template<typename TBuffer>
    void writeJsonInternal(rapidjson::Writer<TBuffer>& writer) const;

    Maybe<Eigen::Vector3d> d_ballObservation;
    std::vector<Eigen::Vector3d> d_goalObservations;
    std::vector<Eigen::Vector3d> d_teamMateObservations;
    std::vector<LineSegment3d> d_observedLineSegments;
    std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> d_observedLineJunctions;
    Maybe<Polygon2d> d_visibleFieldPoly;
    std::vector<OcclusionRay<double>> d_occlusionRays;
    ulong d_thinkCycleNumber;
  };

  template<typename TBuffer>
  inline void AgentFrameState::writeJsonInternal(rapidjson::Writer<TBuffer>& writer) const
  {
    writer.StartObject();
    {
      writer.String("thinkCycle");
      writer.Uint64(d_thinkCycleNumber);

      writer.String("ball");
      if (d_ballObservation.hasValue())
      {
        writer.StartArray();
        writer.Double(d_ballObservation->x(), "%.3f");
        writer.Double(d_ballObservation->y(), "%.3f");
        writer.Double(d_ballObservation->z(), "%.3f");
        writer.EndArray();
      }
      else
      {
        writer.Null();
      }

      writer.String("goals");
      writer.StartArray();
      {
        for (auto const& goalPos : d_goalObservations)
        {
          writer.StartArray();
          writer.Double(goalPos.x(), "%.3f");
          writer.Double(goalPos.y(), "%.3f");
          writer.Double(goalPos.z(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("teammates");
      writer.StartArray();
      {
        for (auto const& teamMatePos : d_teamMateObservations)
        {
          writer.StartArray();
          writer.Double(teamMatePos.x(), "%.3f");
          writer.Double(teamMatePos.y(), "%.3f");
          writer.Double(teamMatePos.z(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("lines");
      writer.StartArray();
      {
        for (auto const& lineSeg : d_observedLineSegments)
        {
          writer.StartArray();
          writer.Double(lineSeg.p1().x(), "%.3f");
          writer.Double(lineSeg.p1().y(), "%.3f");
          writer.Double(lineSeg.p1().z(), "%.3f");
          writer.Double(lineSeg.p2().x(), "%.3f");
          writer.Double(lineSeg.p2().y(), "%.3f");
          writer.Double(lineSeg.p2().z(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();

      writer.String("junctions");
      writer.StartArray();
      {
        for (auto const& junction : d_observedLineJunctions)
        {
          writer.StartObject();
          writer.String("p");
          writer.StartArray();
          writer.Double(junction.position(0), "%.3f");
          writer.Double(junction.position(1), "%.3f");
          writer.EndArray();
          writer.String("a");
          writer.Double(junction.angle, "%.3f");
          writer.String("t");
          writer.Uint(static_cast<unsigned>(junction.type));
          writer.EndObject();
        }
      }
      writer.EndArray();

      writer.String("visibleFieldPoly");
      writer.StartArray();
      {
        if (d_visibleFieldPoly.hasValue())
        {
          for (auto const& vertex : d_visibleFieldPoly.value())
          {
            writer.StartArray();
            writer.Double(vertex.x(), "%.3f");
            writer.Double(vertex.y(), "%.3f");
            writer.EndArray();
          }
        }
      }
      writer.EndArray();

      writer.String("occlusionRays");
      writer.StartArray();
      {
        for (auto const& ray : d_occlusionRays)
        {
          // Should be enough to check a single value for NaN
          if (std::isnan(ray.far().x()))
            continue;

          writer.StartArray();
          writer.Double(ray.near().x(), "%.3f");
          writer.Double(ray.near().y(), "%.3f");
          writer.Double(ray.far().x(), "%.3f");
          writer.Double(ray.far().y(), "%.3f");
          writer.EndArray();
        }
      }
      writer.EndArray();
    }
    writer.EndObject();
  }
}


/* 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.
*/

#include "Lagrangian2d1DR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"
#include <boost/math/quaternion.hpp>

// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "siconos_debug.h"

void Lagrangian2d1DR::initialize(Interaction& inter)
{
  LagrangianR::initialize(inter);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));

  if((inter.getSizeOfDS() !=3) and (inter.getSizeOfDS() !=6))
  {
    THROW_EXCEPTION("Lagrangian2d1DR::initialize(Interaction& inter). The size of ds must of size 3");
  }
  unsigned int qSize = 3 * (inter.getSizeOfDS() / 3);
  _jachq.reset(new SimpleMatrix(1, qSize));
}

void Lagrangian2d1DR::computeJachq(const BlockVector& q, BlockVector& z)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0 \n");

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double G1x = q.getValue(0);
  double G1y = q.getValue(1);

  _jachq->setValue(0,0,Nx);
  _jachq->setValue(0,1,Ny);
  _jachq->setValue(0,2,(G1y-Py)*Nx - (G1x-Px)*Ny);

  if(q.size() ==6)
  {
    DEBUG_PRINT("take into account second ds\n");
    double G2x = q.getValue(3);
    double G2y = q.getValue(4);

    _jachq->setValue(0,3,-Nx);
    _jachq->setValue(0,4,-Ny);
    _jachq->setValue(0,5,- ((G2y-Py)*Nx - (G2x-Px)*Ny));
  }
  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0) \n");

}

double Lagrangian2d1DR::distance() const
{
  DEBUG_BEGIN("Lagrangian2d1DR::distance(...)\n")
  SiconosVector dpc(*_Pc2 - *_Pc1);
  DEBUG_END("Lagrangian2d1DR::distance(...)\n")
  return dpc.norm2() * (inner_prod(*_Nc, dpc) >= 0 ? -1 : 1);

}

void Lagrangian2d1DR::computeh(const BlockVector& q, BlockVector& z, SiconosVector& y)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeh(...)\n");
  DEBUG_EXPR(q.display());

  LagrangianScleronomousR::computeh(q, z, y);
  y.setValue(0, distance());
  DEBUG_EXPR(y.display(););
  DEBUG_EXPR(display(););
  DEBUG_END("Lagrangian2d1DR::computeh(...)\n")
}
void Lagrangian2d1DR::display() const
{
  LagrangianR::display();

  std::cout << " _Pc1 :" << std::endl;
  if(_Pc1)
    _Pc1->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Pc2 :" << std::endl;
  if(_Pc2)
    _Pc2->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Nc :" << std::endl;
  if(_Nc)
    _Nc->display();
  else
    std::cout << " nullptr :" << std::endl;
  std::cout << " _relNc :" << std::endl;
  if(_relNc)
    _relNc->display();
  else
    std::cout << " nullptr :" << std::endl;

}


#include <boost/preprocessor/arithmetic/mul.hpp>


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_BLOCK_CYPHERS_MD5_HPP
#define BOOST_HASH_BLOCK_CYPHERS_MD5_HPP

#include <boost/hash/block_cyphers/detail/md5_policy.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

//
// Encrypt implemented directly from the RFC as found at
// http://www.faqs.org/rfcs/rfc1321.html
//
// Decrypt is a straight-forward inverse
//
// In MD5 terminology:
// - plaintext = AA, BB, CC, and DD
// - cyphertext = A, B, C, and D
// - key = M^(i) and X
//

namespace boost {
namespace hashes {
namespace block_cyphers {

class md5 {
  public:
    typedef detail::md5_policy policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef policy_type::word_type word_type;

    static unsigned const key_bits = policy_type::key_bits;
    static unsigned const key_words = policy_type::key_words;
    typedef policy_type::key_type key_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef policy_type::block_type block_type;

  public:
    md5(key_type const &k) : key(k) {
#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < key_words; ++t) {
            std::printf("X[%2d] = %.8x\n",
                        t, key[t]);
        }
#endif
    }
  private:
    key_type const key;

  public:
    block_type
    encypher(block_type const &plaintext) {
        return encypher_block(plaintext);
    }
  private:
    block_type
    encypher_block(block_type const &plaintext) {
        return encypher_block(key, plaintext);
    }
    static block_type
    encypher_block(key_type const &key,
                   block_type const &plaintext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf("%c%c = %.8x\n",
                        'A'+t, 'A'+t, plaintext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = plaintext[0], b = plaintext[1],
                  c = plaintext[2], d = plaintext[3];

        // Encypher block
#define BOOST_HASH_MD5_ENCYPHER_STEP(aa, bb, cc, dd, fun, k, s, i) \
            { \
                word_type T = aa \
                            + policy_type::fun(bb,cc,dd) \
                            + key[policy_type::key_index(k)] \
                            + policy_type::constant(i-1); \
                aa = bb + policy_type::ROTL<s>(T); \
            }
        for (unsigned t =  0; t < 16; t += 4) {
            BOOST_HASH_MD5_ENCYPHER_STEP(a, b, c, d, ff, t+0,  7, t+1)
            BOOST_HASH_MD5_ENCYPHER_STEP(d, a, b, c, ff, t+1, 12, t+2)
            BOOST_HASH_MD5_ENCYPHER_STEP(c, d, a, b, ff, t+2, 17, t+3)
            BOOST_HASH_MD5_ENCYPHER_STEP(b, c, d, a, ff, t+3, 22, t+4)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 1: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 16; t < 32; t += 4) {
            BOOST_HASH_MD5_ENCYPHER_STEP(a, b, c, d, gg, t+0,  5, t+1)
            BOOST_HASH_MD5_ENCYPHER_STEP(d, a, b, c, gg, t+1,  9, t+2)
            BOOST_HASH_MD5_ENCYPHER_STEP(c, d, a, b, gg, t+2, 14, t+3)
            BOOST_HASH_MD5_ENCYPHER_STEP(b, c, d, a, gg, t+3, 20, t+4)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 2: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 32; t < 48; t += 4) {
            BOOST_HASH_MD5_ENCYPHER_STEP(a, b, c, d, hh, t+0,  4, t+1)
            BOOST_HASH_MD5_ENCYPHER_STEP(d, a, b, c, hh, t+1, 11, t+2)
            BOOST_HASH_MD5_ENCYPHER_STEP(c, d, a, b, hh, t+2, 16, t+3)
            BOOST_HASH_MD5_ENCYPHER_STEP(b, c, d, a, hh, t+3, 23, t+4)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 3: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 48; t < 64; t += 4) {
            BOOST_HASH_MD5_ENCYPHER_STEP(a, b, c, d, ii, t+0,  6, t+1)
            BOOST_HASH_MD5_ENCYPHER_STEP(d, a, b, c, ii, t+1, 10, t+2)
            BOOST_HASH_MD5_ENCYPHER_STEP(c, d, a, b, ii, t+2, 15, t+3)
            BOOST_HASH_MD5_ENCYPHER_STEP(b, c, d, a, ii, t+3, 21, t+4)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 4: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }

        block_type cyphertext = {{a, b, c, d}};
        return cyphertext;
    }

  public:
    block_type
    decypher(block_type const &plaintext) {
        return decypher_block(plaintext);
    }
  private:
    block_type
    decypher_block(block_type const &plaintext) {
        return decypher_block(key, plaintext);
    }
    static block_type
    decypher_block(key_type const &key,
                   block_type const &cyphertext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf("%c = %.8x\n",
                        'A'+t, cyphertext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = cyphertext[0], b = cyphertext[1],
                  c = cyphertext[2], d = cyphertext[3];

        // Decypher block
#define BOOST_HASH_MD5_DECYPHER_STEP(aa, bb, cc, dd, fun, k, s, i) \
            { \
                word_type T = policy_type::ROTR<s>(aa - bb); \
                aa = T \
                   - policy_type::fun(bb,cc,dd) \
                   - key[policy_type::key_index(k)] \
                   - policy_type::constant(i-1); \
            }
        for (unsigned t = 64; t -= 4, t >= 48; ) {
            BOOST_HASH_MD5_DECYPHER_STEP(b, c, d, a, ii, t+3, 21, t+4)
            BOOST_HASH_MD5_DECYPHER_STEP(c, d, a, b, ii, t+2, 15, t+3)
            BOOST_HASH_MD5_DECYPHER_STEP(d, a, b, c, ii, t+1, 10, t+2)
            BOOST_HASH_MD5_DECYPHER_STEP(a, b, c, d, ii, t+0,  6, t+1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 4: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 48; t -= 4, t >= 32; ) {
            BOOST_HASH_MD5_DECYPHER_STEP(b, c, d, a, hh, t+3, 23, t+4)
            BOOST_HASH_MD5_DECYPHER_STEP(c, d, a, b, hh, t+2, 16, t+3)
            BOOST_HASH_MD5_DECYPHER_STEP(d, a, b, c, hh, t+1, 11, t+2)
            BOOST_HASH_MD5_DECYPHER_STEP(a, b, c, d, hh, t+0,  4, t+1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 3: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 32; t -= 4, t >= 16; ) {
            BOOST_HASH_MD5_DECYPHER_STEP(b, c, d, a, gg, t+3, 20, t+4)
            BOOST_HASH_MD5_DECYPHER_STEP(c, d, a, b, gg, t+2, 14, t+3)
            BOOST_HASH_MD5_DECYPHER_STEP(d, a, b, c, gg, t+1,  9, t+2)
            BOOST_HASH_MD5_DECYPHER_STEP(a, b, c, d, gg, t+0,  5, t+1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 2: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }
        for (unsigned t = 16; t -= 4, t < 16; ) {
            BOOST_HASH_MD5_DECYPHER_STEP(b, c, d, a, ff, t+3, 22, t+4)
            BOOST_HASH_MD5_DECYPHER_STEP(c, d, a, b, ff, t+2, 17, t+3)
            BOOST_HASH_MD5_DECYPHER_STEP(d, a, b, c, ff, t+1, 12, t+2)
            BOOST_HASH_MD5_DECYPHER_STEP(a, b, c, d, ff, t+0,  7, t+1)

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf("Round 1: %.8x %.8x %.8x %.8x\n",
                   a, b, c, d);
#endif
        }

        block_type plaintext = {{a, b, c, d}};
        return plaintext;
    }

};

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_MD5_HPP


#include <algorithm>
#include <memory>
#include <vector>

#include <apriltags/TagDetector.h>
#include <apriltags/Tag36h11.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "aslam/calibration/target-aprilgrid.h"
#include "aslam/calibration/target-observation.h"

namespace aslam {
namespace calibration {

TargetAprilGrid::TargetAprilGrid(const TargetAprilGrid::TargetConfiguration& target_config)
    : TargetBase(2u * target_config.num_tag_rows,
                 2u * target_config.num_tag_cols, // 4 points per tag.
                 createGridPoints(target_config)),
                 target_config_(target_config) {
  CHECK_GT(target_config.tag_size_meter, 0.0);
  CHECK_GT(target_config.tag_inbetween_space_meter, 0.0);
}

Eigen::Matrix3Xd TargetAprilGrid::createGridPoints(
    const TargetAprilGrid::TargetConfiguration& target_config) {
  /// \brief initialize an april grid
  ///   point ordering: (e.g. 2x2 grid)
  ///          12-----13  14-----15
  ///          | TAG 2 |  | TAG 3 |
  ///          8-------9  10-----11
  ///          4-------5  6-------7
  ///    y     | TAG 0 |  | TAG 1 |
  ///   ^      0-------1  2-------3
  ///   |-->x
  const double tag_size = target_config.tag_size_meter;
  const double tag_spacing = target_config.tag_inbetween_space_meter;
  const size_t num_point_rows = 2u * target_config.num_tag_rows;
  const size_t num_point_cols = 2u * target_config.num_tag_cols;
  CHECK_GT(tag_size, 0.0);
  CHECK_GT(tag_spacing, 0.0);

  Eigen::Matrix3Xd points_target_frame(3, num_point_rows * num_point_cols);
  for (size_t row_idx = 0u; row_idx < num_point_rows; ++row_idx) {
    for (size_t col_idx = 0u; col_idx < num_point_cols; ++col_idx) {
      Eigen::Vector3d point;
      point(0) =
          static_cast<int>(col_idx / 2) * (tag_size + tag_spacing) + (col_idx % 2) * tag_size;
      point(1) =
          static_cast<int>(row_idx / 2) * (tag_size + tag_spacing) + (row_idx % 2) * tag_size;
      point(2) = 0.0;

      points_target_frame.col(row_idx * num_point_cols + col_idx) = point;
    }
  }
  return points_target_frame;
}

DetectorAprilGrid::DetectorAprilGrid(
    const TargetAprilGrid::Ptr& target,
    const DetectorAprilGrid::DetectorConfiguration& detector_config)
    : target_(target),
      detector_config_(detector_config),
      tag_codes_(AprilTags::tagCodes36h11) {
  CHECK(target);
  tag_detector_.reset(
      new AprilTags::TagDetector(tag_codes_, target_->getConfig().black_tag_border_bits));
}

TargetObservation::Ptr DetectorAprilGrid::detectTargetInImage(const cv::Mat& image) const {
  // Detect all Apriltags in the image.
  std::vector<AprilTags::TagDetection> detections = tag_detector_->extractTags(image);

  // Remove bad tags.
  std::vector<AprilTags::TagDetection>::iterator iter = detections.begin();
  for (iter = detections.begin(); iter != detections.end();) {
    bool remove = false;

    // Enforce min. distance of corners to the image border (tag removed if violated).
    for (int tag_corner_idx = 0; tag_corner_idx < 4; ++tag_corner_idx) {
      remove |= iter->p[tag_corner_idx].first < detector_config_.min_border_distance_px;
      remove |= iter->p[tag_corner_idx].first >
        static_cast<double>(image.cols) - detector_config_.min_border_distance_px;
      remove |= iter->p[tag_corner_idx].second < detector_config_.min_border_distance_px;
      remove |= iter->p[tag_corner_idx].second >
        static_cast<double>(image.rows) - detector_config_.min_border_distance_px;
    }

    // Flag for removal if tag detection is marked as bad.
    if (iter->good != 1) {
      remove |= true;
    }

    // Flag for removal if the tag ID is out-of-range for this grid (faulty detection or wild tag).
    if (iter->id >= static_cast<int>(target_->size() / 4.0)) {
      remove |= true;
    }

    // Remove tag from the observation list.
    if (remove) {
      VLOG(200) << "Tag with ID " << iter->id << " is only partially in image (corners outside) "
                << "and will be removed from the TargetObservation.\n";

      // delete the tag and advance in list
      iter = detections.erase(iter);
    } else {
      //advance in list
      ++iter;
    }
  }

  // Check if enough tags have been found.
  if (detections.size() < detector_config_.min_visible_tags_for_valid_obs) {
    // Detection failed; return nullptr.
    return TargetObservation::Ptr();
  }

  // Sort detections by tagId.
  std::sort(detections.begin(), detections.end(), AprilTags::TagDetection::sortByIdCompare);

  // Check for duplicate tag ids that would indicate Apriltags not belonging to calibration target.
  if (detections.size() > 1) {
    for (size_t tag_idx = 0; tag_idx < detections.size() - 1; ++tag_idx)
      if (detections[tag_idx].id == detections[tag_idx + 1].id) {
        // Show image of duplicate Apriltag.
        cv::destroyAllWindows();
        cv::namedWindow("Wild Apriltag detected. Hide them!");
        cvStartWindowThread();

        cv::Mat image_copy = image.clone();
        cv::cvtColor(image_copy, image_copy, CV_GRAY2RGB);

        // Mark all duplicate tags in the image.
        for (size_t inner_tag_idx = 0; inner_tag_idx < detections.size() - 1; ++inner_tag_idx) {
          if (detections[inner_tag_idx].id == detections[inner_tag_idx + 1].id) {
            detections[inner_tag_idx].draw(image_copy);
            detections[inner_tag_idx + 1].draw(image_copy);
          }
        }

        cv::putText(image_copy, "Duplicate Apriltags detected. Hide them.", cv::Point(50, 50),
                    CV_FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255, 0, 0), 2, 8, false);
        cv::putText(image_copy, "Press enter to exit...", cv::Point(50, 80),
                    CV_FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255, 0, 0), 2, 8, false);
        cv::imshow("Duplicate Apriltags detected. Hide them", image_copy);
        cv::waitKey();

        LOG(WARNING) << "Found apriltag not belonging to calibration board. Check the image for "
                     << "the tag and hide it.";
        return TargetObservation::Ptr();
      }
  }

  // Convert corners to cv::Mat (4 consecutive corners form one tag).
  // point ordering here
  //          11-----10  15-----14
  //          | TAG 2 |  | TAG 3 |
  //          8-------9  12-----13
  //          3-------2  7-------6
  //    y     | TAG 0 |  | TAG 1 |
  //   ^      0-------1  4-------5
  //   |-->x
  cv::Mat tag_corners(4u * detections.size(), 2, CV_32F);
  for (size_t tag_idx = 0; tag_idx < detections.size(); tag_idx++) {
    for (size_t tag_corner_idx = 0; tag_corner_idx < 4; tag_corner_idx++) {
      tag_corners.at<float>(4u * tag_idx + tag_corner_idx, 0) =
          detections[tag_idx].p[tag_corner_idx].first;
      tag_corners.at<float>(4u * tag_idx + tag_corner_idx, 1) =
          detections[tag_idx].p[tag_corner_idx].second;
    }
  }

  // Store a copy of the corner list before subpix refinement.
  cv::Mat tag_corners_raw = tag_corners.clone();

  // Perform optional subpixel refinement on all tag corners (four corners each tag).
  if (detector_config_.run_subpixel_refinement) {
    cv::cornerSubPix(image, tag_corners, cv::Size(2, 2), cv::Size(-1, -1),
                     cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
  }

  // Insert the observed points into the correct location of the grid point array.
  // point ordering
  //          12-----13  14-----15
  //          | TAG 2 |  | TAG 3 |
  //          8-------9  10-----11
  //          4-------5  6-------7
  //    y     | TAG 0 |  | TAG 1 |
  //   ^      0-------1  2-------3
  //   |-->x
  Eigen::VectorXi corner_ids(target_->size());
  Eigen::Matrix2Xd image_corners(2, target_->size());
  size_t out_point_idx = 0u;

  for (size_t tag_idx = 0; tag_idx < detections.size(); ++tag_idx) {
    const unsigned int tag_id = detections[tag_idx].id;

    // Calculate the grid idx for all four tag corners given the tagId and cols.
    const size_t cols = target_->cols();
    const unsigned int base_idx =
        tag_id * 2 + static_cast<int>(tag_id / (cols / 2)) * cols;
    unsigned int point_indices_tag[] = {base_idx,
                                        base_idx + 1,
                                        base_idx + static_cast<unsigned int>(cols + 1),
                                        base_idx + static_cast<unsigned int>(cols)};

    // Add four points per tag
    for (int tag_corner_idx = 0; tag_corner_idx < 4; tag_corner_idx++) {
      const Eigen::Vector2d corner_refined(
          tag_corners.row(4 * tag_idx + tag_corner_idx).at<float>(0),
          tag_corners.row(4 * tag_idx + tag_corner_idx).at<float>(1));
      const Eigen::Vector2d corner_raw(
          tag_corners_raw.row(4 * tag_idx + tag_corner_idx).at<float>(0),
          tag_corners_raw.row(4 * tag_idx + tag_corner_idx).at<float>(1));

      // Add corner points if it has not moved too far in the subpix refinement.
      const double subpix_displacement_squarred = (corner_refined - corner_raw).squaredNorm();
      if (subpix_displacement_squarred <= detector_config_.max_subpixel_refine_displacement_px_sq) {
        corner_ids(out_point_idx) = point_indices_tag[tag_corner_idx];
        image_corners.col(out_point_idx) = corner_refined;
        ++out_point_idx;
      }
    }
  }
  corner_ids.conservativeResize(out_point_idx);
  image_corners.conservativeResize(Eigen::NoChange, out_point_idx);

  return TargetObservation::Ptr(new TargetObservation(target_, image.rows, image.cols,
                                                      corner_ids, image_corners));
}

}  // namespace calibration
}  // namespace aslam