#include "tetgen.h"
#include <boost/python.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <stdexcept>
#include <iostream>
#include "foreign_array_wrap.hpp"

using namespace boost::python;
namespace py = boost::python;
using namespace std;

namespace boost {
template <>
inline tForeignArray<struct tetgenio::polygon> const volatile * get_pointer(class tForeignArray<struct tetgenio::polygon> const volatile *tF) {
  return tF;
}

template <>
inline tForeignArray<struct tetgenio::facet> const volatile * get_pointer(class tForeignArray<struct tetgenio::facet> const volatile *tF) {
  return tF;
}

template <>
inline tForeignArray<int> const volatile * get_pointer(class tForeignArray<int> const volatile *tF) {
  return tF;
}

template <>
inline tForeignArray<double> const volatile * get_pointer(class tForeignArray<double> const volatile *tF) {
  return tF;
}
}

namespace
{
  struct tMeshInfo : public tetgenio, public boost::noncopyable
  {
    private:
      typedef tetgenio super;

    public:
      tForeignArray<REAL>                       Points; // in/out
      tForeignArray<REAL>                       PointAttributes; // in/out
      tForeignArray<REAL>                       PointMetricTensors; // in/out
      tForeignArray<int>                        PointMarkers; // in/out

      tForeignArray<int>                        Elements; // in/out
      tForeignArray<REAL>                       ElementAttributes; // in/out
      tForeignArray<REAL>                       ElementVolumes; // out
      tForeignArray<int>                        Neighbors; // out

      tForeignArray<tetgenio::facet>      Facets;
      tForeignArray<int>                  FacetMarkers;

      tForeignArray<REAL>                 Holes;
      tForeignArray<REAL>                 Regions;

      tForeignArray<REAL>                 FacetConstraints;
      tForeignArray<REAL>                 SegmentConstraints;

      tForeignArray<int>                  Faces;
      tForeignArray<int>                  AdjacentElements;
      tForeignArray<int>                  FaceMarkers;

      tForeignArray<int>                  Edges;
      tForeignArray<int>                  EdgeMarkers;
      tForeignArray<int>                  EdgeAdjTetList;

    public:
      tMeshInfo()
        : Points(pointlist, numberofpoints, 3),
          PointAttributes(pointattributelist, numberofpoints, 0, &Points),
          PointMetricTensors(pointmtrlist, numberofpoints, 0, &Points),
          PointMarkers(pointmarkerlist, numberofpoints, 1, &Points),

          Elements(tetrahedronlist, numberoftetrahedra, 0),
          ElementAttributes(tetrahedronattributelist,
              numberoftetrahedra, 0, &Elements),
          ElementVolumes(tetrahedronvolumelist, numberoftetrahedra, 1, &Elements),
          Neighbors(neighborlist, numberoftetrahedra, 4, &Elements),

          Facets(facetlist, numberoffacets),
          FacetMarkers(facetmarkerlist, numberoffacets, 1, &Facets),

          Holes(holelist, numberofholes, 3),

          Regions(regionlist, numberofregions, 5),

          FacetConstraints(facetconstraintlist, numberoffacetconstraints, 2),
          SegmentConstraints(segmentconstraintlist, numberofsegmentconstraints, 3),

          Faces(trifacelist, numberoftrifaces, 3),
          AdjacentElements(adjtetlist, numberoftrifaces, 2, &Faces),
          FaceMarkers(trifacemarkerlist, numberoftrifaces, 1, &Faces),

          Edges(edgelist, numberofedges, 2),
          EdgeMarkers(edgemarkerlist, numberofedges, 1, &Edges),
          EdgeAdjTetList(edgeadjtetlist, numberofedges, 1, &Edges)
      {
        Elements.fixUnit(numberofcorners);
      }

      unsigned numberOfPointAttributes() const
      {
        return numberofpointattributes;
      }

      unsigned numberOfPointMetricTensors() const
      {
        return numberofpointmtrs;
      }

      unsigned numberOfElementVertices() const
      {
        return numberofcorners;
      }

      unsigned numberOfElementAttributes() const
      {
        return numberoftetrahedronattributes;
      }

      void setNumberOfPointAttributes(unsigned attrs)
      {
        PointAttributes.setUnit(attrs);
        numberofpointattributes = attrs;
      }

      void setNumberOfPointMetricTensors(unsigned mtrs)
      {
        PointMetricTensors.setUnit(mtrs);
        numberofpointmtrs = mtrs;
      }

      void setNumberOfElementVertices(unsigned verts)
      {
        Elements.setUnit(verts);
        numberofcorners = verts;
      }

      void setNumberOfElementAttributes(unsigned attrs)
      {
        ElementAttributes.setUnit(attrs);
        numberoftetrahedronattributes = attrs;
      }

#define OVERRIDE_LOAD_WITH_ERROR_CHECK(WHAT, POSTPROC) \
      void load_##WHAT(char* filename) \
      { \
        if (!super::load_##WHAT(filename)) \
          throw std::runtime_error("load_" #WHAT " failed"); \
        POSTPROC; \
      }

      OVERRIDE_LOAD_WITH_ERROR_CHECK(node,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(var,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(mtr,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(poly,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(off,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(ply,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(stl,);
      OVERRIDE_LOAD_WITH_ERROR_CHECK(vtk,);

      void load_plc(char* filename, int object)
      {
        if (!super::load_plc(filename, object))
          throw std::runtime_error("load_plc failed");
      }

      void load_medit(char* filename, int object)
      {
        if (!super::load_medit(filename, object))
          throw std::runtime_error("load_tetmesh failed");
      }

      void load_tetmesh(char* filename, int object)
      {
        if (!super::load_tetmesh(filename, object))
          throw std::runtime_error("load_tetmesh failed");
        Elements.fixUnit(numberofcorners);
      }

      /*
      tTriangulationParameters &operator=(const tTriangulationParameters &src)
      {
        numberofpointattributes = src.numberofpointattributes ;
        numberofcorners = src.numberofcorners;
        numberoftriangleattributes = src.numberoftriangleattributes;

        Points = src.Points;
        PointAttributes = src.PointAttributes;
        PointMarkers = src.PointMarkers;

        Triangles = src.Triangles;
        TriangleAttributes = src.TriangleAttributes;
        TriangleAreas = src.TriangleAreas;
        Neighbors = src.Neighbors;

        Segments = src.Segments;
        SegmentMarkers = src.SegmentMarkers;

        Holes = src.Holes;

        Regions = src.Regions;

        Edges = src.Edges;
        EdgeMarkers = src.EdgeMarkers;
        Normals = src.Normals;

        return *this;
      }
      */
  };

  /*
  tTriangulationParameters *copyTriangulationParameters(const tTriangulationParameters &src)
  {
    auto_ptr<tTriangulationParameters> copy(new tTriangulationParameters);
    *copy = src;
    return copy.release();
  }
  */

  void tetrahedralizeWrapper(tetgenbehavior &bhv, tMeshInfo &in, tMeshInfo &out,
      tMeshInfo *addin)
  {
    try
    {
      tetrahedralize(&bhv, &in, &out, addin);
    }
    catch (int &i)
    {
      throw runtime_error("TetGen runtime error code "+boost::lexical_cast<string>(i));
    }

    out.Elements.fixUnit(out.numberofcorners);
    out.PointAttributes.fixUnit(out.numberofpointattributes);
    out.PointMetricTensors.fixUnit(out.numberofpointmtrs);
    out.ElementAttributes.fixUnit(out.numberoftetrahedronattributes);
  }

  template <std::size_t owner_arg = 1, class Base = default_call_policies>
  struct manage_new_internal_reference
      : with_custodian_and_ward_postcall<0, owner_arg, Base>
  {
     typedef manage_new_object result_converter;
  };

  tForeignArray<tetgenio::polygon> *facet_get_polygons(tetgenio::facet &self)
  {
    return new tForeignArray<tetgenio::polygon>(
        self.polygonlist, self.numberofpolygons);
  }

  tForeignArray<REAL> *facet_get_holes(tetgenio::facet &self)
  {
    return new tForeignArray<REAL>(self.holelist, self.numberofholes, 3);
  }

  tForeignArray<int> *polygon_get_vertices(tetgenio::polygon &self)
  {
    return new tForeignArray<int>(self.vertexlist, self.numberofvertices);
  }
}

#define DEF_RW_MEMBER(NAME) \
    def_readwrite(#NAME, &cl::NAME)
#define DEF_METHOD(NAME) \
    def(#NAME, &cl::NAME)

BOOST_PYTHON_MODULE(_tetgen)
{
  def("tetrahedralize", tetrahedralizeWrapper,
      (py::args("behavior", "in", "out"), py::arg("addin")=py::object()));

  {
    typedef tMeshInfo cl;
    class_<cl, boost::noncopyable>
      ("MeshInfo", init<>())
      .def_readonly("points", &cl::Points)
      .def_readonly("point_attributes", &cl::PointAttributes)
      .def_readonly("point_metric_tensors", &cl::PointMetricTensors)
      .def_readonly("point_markers", &cl::PointMarkers)

      .def_readonly("elements", &cl::Elements)
      .def_readonly("element_attributes", &cl::ElementAttributes)
      .def_readonly("element_volumes", &cl::ElementVolumes)
      .def_readonly("neighbors", &cl::Neighbors)

      .def_readonly("facets", &cl::Facets)
      .def_readonly("facet_markers", &cl::FacetMarkers)

      .def_readonly("holes", &cl::Holes)

      .def_readonly("regions", &cl::Regions)

      .def_readonly("facet_constraints", &cl::FacetConstraints)
      .def_readonly("segment_constraints", &cl::SegmentConstraints)

      .def_readonly("faces", &cl::Faces)
      .def_readonly("adjacent_elements", &cl::AdjacentElements)
      .def_readonly("face_markers", &cl::FaceMarkers)

      .def_readonly("edges", &cl::Edges)
      .def_readonly("edge_markers", &cl::EdgeMarkers)
      .def_readonly("edge_adjacent_elements", &cl::EdgeAdjTetList)

      .add_property("number_of_point_attributes",
          &cl::numberOfPointAttributes,
          &cl::setNumberOfPointAttributes)
      .add_property("number_of_element_vertices",
          &cl::numberOfElementVertices,
          &cl::setNumberOfElementVertices)
      .add_property("number_of_element_attributes",
          &cl::numberOfElementAttributes,
          &cl::setNumberOfElementAttributes)

      .DEF_METHOD(save_nodes)
      .DEF_METHOD(save_elements)
      .DEF_METHOD(save_faces)
      .DEF_METHOD(save_edges)
      .DEF_METHOD(save_neighbors)
      .DEF_METHOD(save_poly)

      .DEF_METHOD(load_node)
      .DEF_METHOD(load_var)
      .DEF_METHOD(load_mtr)
      .DEF_METHOD(load_poly)
      .DEF_METHOD(load_off)
      .DEF_METHOD(load_ply)
      .DEF_METHOD(load_stl)
      .DEF_METHOD(load_medit)
      .DEF_METHOD(load_plc)
      .DEF_METHOD(load_tetmesh)

      /*
         .def("copy", &copyTriangulationParameters,
         return_value_policy<manage_new_object>())
         */
      //.enable_pickling()
      ;
  }

  {
    typedef tetgenio::facet cl;
    class_<cl, boost::noncopyable>("Facet", no_init)
      .add_property("polygons",
          make_function(facet_get_polygons, manage_new_internal_reference<>()))
      .add_property("holes",
          make_function(facet_get_holes, manage_new_internal_reference<>()))
      ;
  }

  {
    typedef tetgenio::polygon cl;
    class_<cl, boost::noncopyable>("Polygon", no_init)
      .add_property("vertices",
          make_function(polygon_get_vertices, manage_new_internal_reference<>()))
      ;
  }

  {
    typedef tetgenbehavior cl;
    class_<cl, boost::noncopyable>("Options", init<>())
      .DEF_RW_MEMBER(plc)
      .DEF_RW_MEMBER(psc)
      .DEF_RW_MEMBER(refine)
      .DEF_RW_MEMBER(quality)
      .DEF_RW_MEMBER(nobisect)
      .DEF_RW_MEMBER(coarsen)
      .DEF_RW_MEMBER(weighted)
      .DEF_RW_MEMBER(brio_hilbert)
      .DEF_RW_MEMBER(incrflip)
      .DEF_RW_MEMBER(flipinsert)
      .DEF_RW_MEMBER(metric)
      .DEF_RW_MEMBER(varvolume)
      .DEF_RW_MEMBER(fixedvolume)
      .DEF_RW_MEMBER(regionattrib)
      .DEF_RW_MEMBER(conforming)
      .DEF_RW_MEMBER(insertaddpoints)
      .DEF_RW_MEMBER(diagnose)
      .DEF_RW_MEMBER(convex)
      .DEF_RW_MEMBER(nomergefacet)
      .DEF_RW_MEMBER(nomergevertex)
      .DEF_RW_MEMBER(noexact)
      .DEF_RW_MEMBER(nostaticfilter)
      .DEF_RW_MEMBER(zeroindex)
      .DEF_RW_MEMBER(facesout)
      .DEF_RW_MEMBER(edgesout)
      .DEF_RW_MEMBER(neighout)
      .DEF_RW_MEMBER(voroout)
      .DEF_RW_MEMBER(meditview)
      .DEF_RW_MEMBER(vtkview)
      .DEF_RW_MEMBER(nobound)
      .DEF_RW_MEMBER(nonodewritten)
      .DEF_RW_MEMBER(noelewritten)
      .DEF_RW_MEMBER(nofacewritten)
      .DEF_RW_MEMBER(noiterationnum)
      .DEF_RW_MEMBER(nojettison)
      .DEF_RW_MEMBER(reversetetori)
      .DEF_RW_MEMBER(docheck)
      .DEF_RW_MEMBER(quiet)
      .DEF_RW_MEMBER(verbose)

      .DEF_RW_MEMBER(vertexperblock)
      .DEF_RW_MEMBER(tetrahedraperblock)
      .DEF_RW_MEMBER(shellfaceperblock)
      .DEF_RW_MEMBER(nobisect_param)
      .DEF_RW_MEMBER(addsteiner_algo)
      .DEF_RW_MEMBER(coarsen_param)
      .DEF_RW_MEMBER(weighted_param)
      .DEF_RW_MEMBER(fliplinklevel)
      .DEF_RW_MEMBER(flipstarsize)
      .DEF_RW_MEMBER(fliplinklevelinc)
      .DEF_RW_MEMBER(reflevel)
      .DEF_RW_MEMBER(optlevel)
      .DEF_RW_MEMBER(optscheme)
      .DEF_RW_MEMBER(delmaxfliplevel)
      .DEF_RW_MEMBER(order)
      .DEF_RW_MEMBER(steinerleft)
      .DEF_RW_MEMBER(no_sort)
      .DEF_RW_MEMBER(hilbert_order)
      .DEF_RW_MEMBER(hilbert_limit)
      .DEF_RW_MEMBER(brio_threshold)
      .DEF_RW_MEMBER(brio_ratio)
      .DEF_RW_MEMBER(facet_ang_tol)
      .DEF_RW_MEMBER(maxvolume)
      .DEF_RW_MEMBER(minratio)
      .DEF_RW_MEMBER(mindihedral)
      .DEF_RW_MEMBER(optmaxdihedral)
      .DEF_RW_MEMBER(optminsmtdihed)
      .DEF_RW_MEMBER(optminslidihed)
      .DEF_RW_MEMBER(epsilon)
      .DEF_RW_MEMBER(minedgelength)
      .DEF_RW_MEMBER(coarsen_percent)

      .def("parse_switches", (bool (tetgenbehavior::*)(char *)) &cl::parse_commandline)
      ;
  }

  exposePODForeignArray<REAL>("RealArray");
  exposePODForeignArray<int>("IntArray");
  exposeStructureForeignArray<tetgenio::facet>("FacetArray");
  exposeStructureForeignArray<tetgenio::polygon>("PolygonArray");
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_COMPARE_LESS_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_COMPARE_LESS_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/reduction/functions/compare_less.hpp>
#include <boost/simd/reduction/functions/simd/common/details/compare_less_helper.hpp>
#include <boost/simd/include/functions/simd/compare_equal.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_less_, boost::simd::tag::avx_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                              ((simd_<double_<A0>,boost::simd::tag::avx_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      unsigned int mask_a_lt_b =  _mm256_movemask_pd(lt(a0,a1));
      unsigned int mask_a_gt_b =  _mm256_movemask_pd(gt(a0,a1));
      return result_type(boost::simd::details::compare_less_helper(mask_a_lt_b,mask_a_gt_b));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_less_, boost::simd::tag::avx_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                              ((simd_<single_<A0>,boost::simd::tag::avx_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      unsigned int mask_a_lt_b =  _mm256_movemask_ps(lt(a0,a1));
      unsigned int mask_a_gt_b =  _mm256_movemask_ps(gt(a0,a1));
      return result_type(boost::simd::details::compare_less_helper(mask_a_lt_b,mask_a_gt_b));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::compare_less_, boost::simd::tag::avx_
                            , (A0)
                            , ((simd_<integer_<A0>,boost::simd::tag::avx_>))
                              ((simd_<integer_<A0>,boost::simd::tag::avx_>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type  sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::scalar_of<A0>::type      stype;
      typedef native < stype,  boost::simd::tag::sse_>          htype;
      htype a00 = _mm256_extractf128_si256(a0, 0);
      htype a10 = _mm256_extractf128_si256(a1, 0);
      if (compare_less(a00, a10))
      {
        return True<result_type>();
      }
      else if (compare_equal(a00, a10))
      {
        htype a01 = _mm256_extractf128_si256(a0, 1);
        htype a11 = _mm256_extractf128_si256(a1, 1);
        return  compare_less(a01, a11);
      }
      else
      {
        return False<result_type>();
      }
    }
  };
} } }

#endif
#endif


#include <iostream>
#include <string>
#include <vector>

#include "baldr/graphreader.h"
#include "baldr/rapidjson_utils.h"
#include "midgard/vector2.h"
#include <boost/property_tree/ptree.hpp>

#include "test.h"

using namespace valhalla::midgard;
using namespace valhalla::baldr;

namespace {

boost::property_tree::ptree get_conf(const std::string& tile_dir) {
  std::stringstream ss;
  ss << R"({"mjolnir":{"tile_dir":")" + tile_dir + R"(", "concurrency": 1}})";
  boost::property_tree::ptree conf;
  rapidjson::read_json(ss, conf);
  return conf;
}

struct bb_tester {
  GraphReader reader;
  AABB2<PointLL> bb;
  bb_tester(const std::string& tile_dir) : reader(get_conf(tile_dir).get_child("mjolnir")) {
    // to get the bb of the whole data set we can just look at nodes of all tiles
    bb = AABB2<PointLL>{PointLL{}, PointLL{}};
    for (const auto& id : reader.GetTileSet()) {
      const auto* t = reader.GetGraphTile(id);
      for (const auto& node : t->GetNodes()) {
        auto node_ll = node.latlng(t->header()->base_ll());
        if (!bb.minpt().IsValid())
          bb = AABB2<PointLL>{node_ll, node_ll};
        else
          bb.Expand(node_ll);
      }
    }
  }
  AABB2<PointLL> operator()(const AABB2<PointLL>& b) {
    return reader.GetMinimumBoundingBox(b);
  }
};

bool ApproxEqual(const AABB2<PointLL>& a, const AABB2<PointLL>& b) {
  return a.minpt().ApproximatelyEqual(b.minpt(), 0.000001f) &&
         a.maxpt().ApproximatelyEqual(b.maxpt(), 0.000001f);
}

TEST(MinBB, utrecht_bb) {
  bb_tester t("test/data/utrecht_tiles");

  EXPECT_TRUE(t.bb.minpt().IsValid());
  EXPECT_TRUE(t.bb.maxpt().IsValid());

  EXPECT_PRED2(ApproxEqual, t(t.bb), t.bb)
      << "Expanding the bbox from the largest bbox shouldn't change the bbox";

  AABB2<PointLL> sbb(t.bb.minpt() + Vector2(0.0001f, 0.0001f),
                     t.bb.maxpt() - Vector2(0.0001f, 0.0001f));

  EXPECT_PRED2(ApproxEqual, t(sbb), t.bb)
      << "Expanding a slightly smaller bbox shouldn't change the bbox";

  AABB2<PointLL> lbb(t.bb.minpt() - Vector2(0.0001f, 0.0001f),
                     t.bb.maxpt() + Vector2(0.0001f, 0.0001f));

  EXPECT_PRED2(ApproxEqual, t(lbb), t.bb)
      << "Expanding a slightly larger bbox shouldn't change the bbox";
}

TEST(MinBB, null_bb) {
  bb_tester t("");

  // no dataset should have an invalid bounding box
  EXPECT_FALSE(t.bb.minpt().IsValid());
  EXPECT_FALSE(t.bb.maxpt().IsValid());

  auto bb = t(t.bb);
  EXPECT_EQ(bb, t.bb) << "reader should also give back invalid bounding box";

  bb = t(AABB2<PointLL>{});
  EXPECT_EQ(bb, t.bb) << "reader should still give back invalid bounding box";
}

} // namespace

int main(int argc, char* argv[]) {
  valhalla::midgard::logging::Configure({{"type", ""}});
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/* boost nondet_random_speed.cpp performance test
 *
 * Copyright Jens Maurer 2000
 * 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)
 *
 * $Id$
 *
 */

#include <iostream>
#include <string>
#include <boost/timer.hpp>
#include <boost/nondet_random.hpp>

// set to your CPU frequency in MHz
static const double cpu_frequency = 200 * 1e6;

static void show_elapsed(double end, int iter, const std::string & name)
{
  double usec = end/iter*1e6;
  double cycles = usec * cpu_frequency/1e6;
  std::cout << name << ": "
            << usec*1e3 << " nsec/loop = "
            << cycles << " CPU cycles"
            << std::endl;
}

template<class Result, class RNG>
static void timing(RNG & rng, int iter, const std::string& name)
{
  volatile Result tmp; // make sure we're not optimizing too much
  boost::timer t;
  for(int i = 0; i < iter; i++)
    tmp = rng();
  show_elapsed(t.elapsed(), iter, name);
}

template<class RNG>
void run(int iter, const std::string & name)
{
  RNG rng;
  timing<long>(rng, iter, name);
}

int main(int argc, char*argv[])
{
  if(argc != 2) {
    std::cerr << "usage: " << argv[0] << " iterations" << std::endl;
    return 1;
  }

  int iter = std::atoi(argv[1]);

#ifdef __linux__
  boost::random_device dev;
  timing<unsigned int>(dev, iter, "random_device");
#else
#error The non-deterministic random device is currently available on Linux only.
#endif

  return 0;
}


#include <iostream>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>

#include <besiq/prior.hpp>
#include <besiq/method/besiq_method.hpp>
#include <besiq/method/besiq_fine_method.hpp>

#include "common_options.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-bayes [OPTIONS] pairs genotype_plink_prefix";
const std::string DESCRIPTION = "Bayesian inference of genetic interactions.";

int
main(int argc, char *argv[])
{
    OptionParser parser = create_common_options( USAGE, DESCRIPTION, false );

    OptionGroup group = OptionGroup( parser, "Options for bayes", "These options will change the behaviour of bayes and fine." );
    group.add_option( "-n", "--num-interactions" ).type( "int" ).help( "The number of interactions to correct for, this is used in the model prior (default: all)." );
    group.add_option( "-s", "--num-single" ).type( "int" ).help( "The number of snps to consider when correcting (default: proportional to square of the number of interactions)." );
    group.add_option( "-t", "--single-prior" ).type( "float" ).help( "The probability that a single snp is associated (default: %default)." ).set_default( 0.0 );
    group.add_option( "-i", "--mc-iterations" ).type( "int" ).help( "The number of monte carlo iterations to use in the fine method (default: %default)." ).set_default( 4000000 );
    group.add_option( "-a", "--beta-prior-param1" ).type( "float" ).help( "First shape parameter of beta prior (default: %default)." ).set_default( 2.0 );
    group.add_option( "-b", "--beta-prior-param2" ).type( "float" ).help( "Second shape parameter of beta prior (default: %default)." ).set_default( 2.0 );
    group.add_option( "-e", "--estimate-prior-params" ).action( "store_true" ).help( "Estimate prior parameters from data by permuting phenotype (default: off)." );
    parser.add_option( "--additive" ).action( "store_true" ).help( "Use an additive model (slow)." );
    parser.add_option_group( group );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }

    shared_ptr<common_options> parsed_data = parse_common_options( options, parser.args( ) );

    /* Read prior parameters */
    arma::vec alpha = arma::ones<arma::vec>( 2 );
    alpha[ 0 ] = (float) options.get( "beta_prior_param1" );
    alpha[ 1 ] = (float) options.get( "beta_prior_param2" );
    if( options.is_set( "estimate_prior_params" ) )
    {
        alpha = estimate_prior_parameters( parsed_data->genotypes, parsed_data->data->phenotype, parsed_data->data->missing, 5000 );
    }

    /* Count the number of interactions to adjust for */
    parsed_data->data->single_prior = (float) options.get( "single_prior" );
    parsed_data->data->num_single = (unsigned int) options.get( "num_single" );
    parsed_data->data->num_interactions = (unsigned int) options.get( "num_interactions" );

    method_type *m = new besiq_method( parsed_data->data, alpha );
    if( options.is_set( "additive" ) )
    {
        m = new besiq_method( parsed_data->data, alpha );
    }
    else
    {
        m = new besiq_fine_method( parsed_data->data, (int) options.get( "mc_iterations" ), alpha );
    }

    run_method( *m, parsed_data->genotypes, *parsed_data->pairs, *parsed_data->result_file );

    delete m;

    return 0;
}


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;

#include "../../../core/format_helper.h"

#include "elas.h"

void runDenseStereo(const vector<string>& imagelist, const string& source_folder, const string& output_folder)
{
  vector<int> compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(9);

  Mat img_left, img_right;
  Mat img_disp, img_disp_save, mat_disp_save;
  for(size_t i = 0; i < imagelist.size(); i+=2) {
    cout << source_folder + imagelist[i] << "\n";
    img_left = imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
    cv::GaussianBlur(img_left, img_left, cv::Size(3,3), 0.7);
    cv::GaussianBlur(img_right, img_right, cv::Size(3,3), 0.7);

    int width = img_left.cols;
    int height = img_left.rows;
    const int32_t dims[3] = {width,height,width}; // bytes per line = width
    float* D1_data = new float[width*height];
    float* D2_data = new float[width*height];
    // process
    Elas::parameters param;
    param.add_corners = 1;
    param.match_texture = 0;
    //param.postprocess_only_left = false;
    Elas elas(param);
    elas.process(img_left.data, img_right.data, D1_data, D2_data, dims);
    // find maximum disparity for scaling output disparity images to [0..255]
    float disp_max = 0;
    for (int32_t i=0; i<width*height; i++) {
      //std::cout << D1_data[i] << "\n";
      if(D1_data[i] > disp_max) disp_max = D1_data[i];
    }
    std::cout << "Max disp = " << disp_max << "\n";
    // copy float to uchar
    img_disp = cv::Mat::zeros(height, width, CV_8U);
    mat_disp_save = cv::Mat::zeros(height, width, CV_32F);
    for(int i = 0; i < height; i++) {
      for(int j = 0; j < width; j++) {
        //img_disp.at<uint8_t>(i,j) = (uint8_t)max(255.0*D1_data[i*width + j]/disp_max, 0.0);
        img_disp.at<uint8_t>(i,j) = (uint8_t)std::max(D1_data[i*width + j], 0.0f);
        mat_disp_save.at<float>(i,j) = D1_data[i*width + j];
      }
    }

    string img_fname = output_folder + "img/" + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
    string mat_fname = output_folder + "mat/" + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.yml";
    cout << "Saving: " << img_fname << "\n" << mat_fname << "\n----------------------\n";
    cv::equalizeHist(img_disp, img_disp);
    cv::imwrite(img_fname, img_disp, compression_params);
    cv::FileStorage mat_file(mat_fname, cv::FileStorage::WRITE);
    mat_file << "disparity_matrix" << mat_disp_save;

    //cout << img_disp << "\n\n";
    //imshow("disparity", img_disp);
    //waitKey(0);
    delete[] D1_data;
    delete[] D2_data;
  }
}

int main(int argc, char** argv)
{
  string config_file;
  string imagelistfn;
  string cam_params_file;
  string source_folder;
  string output_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"),
       "camera params file")
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("disp_folder,d", po::value<string>(&output_folder), "folder for output")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
      ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).allow_unregistered().run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      cout << "can not open config file: " << config_file << "\n";
      cout << generic;
      cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options, true), vm);
      notify(vm);
    }
    cout << "Configuring done, using:" << endl;

    if(vm.count("camera_params")) {
      cout << "Camera params: ";
      cout << cam_params_file << endl;
    }
    if(vm.count("source_folder")) {
      cout << "Source folder: ";
      cout << source_folder << endl;
    }
    if(vm.count("disp_folder")) {
      cout << "Disparity folder: ";
      cout << output_folder << endl;
    }
    if(vm.count("imglist")) {
      cout << "Image list file: ";
      cout << imagelistfn << endl;
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return 1;
  }

  if(imagelistfn == "")
  {
    cout << "error: no xml image list given." << endl;
    return -1;
  }
  if(output_folder == "" || source_folder == "")
  {
    cout << "error: no output or source folder given." << endl;
    return -1;
  }

  vector<string> imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }

  runDenseStereo(imagelist, source_folder, output_folder);

  return 0;
}


//
// Created by LI YANZHE on 18-4-13.
//

#include "FireRuneSplitter.h"
#include <numeric>
#include <Eigen/Core>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudafilters.hpp>
#include <opencv2/cudawarping.hpp>
#include <opencv2/cudaarithm.hpp>
//#include <opencv2/cudafeatures2d.hpp>
#include "../Utils.h"

using namespace std;
using namespace cv;
using cuda::GpuMat;
using namespace runerec;

void FireRuneSplitter::split(const cv::cuda::GpuMat &frame, std::vector<cv::cuda::GpuMat> &roi,
                             vector<RotatedRect> &roi_rects) {
  roi_rects.clear();
  float hw_ratio = (float) frame.cols / frame.rows;
  resize_ratio = (float) frame.rows / 1080;
  GpuMat src(frame), dst, dilate_binary;
  cuda::resize(src, src, Size((int) 1080 * hw_ratio, 1080));
  vector<vector<Point2i>> contours;
//    getContours(src,dst,contours);
  if (frame.channels() != 1)
    cuda::cvtColor(src, src, COLOR_BGR2GRAY);
  {
//        cuda::threshold(src, dst, 127, 255, THRESH_BINARY);
    cpuThreshold(src, dst, 127, 255, THRESH_OTSU);
    Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
    Ptr<cuda::Filter> erode = cuda::createMorphologyFilter(MORPH_ERODE, dst.type(), element);
    erode->apply(dst, dst);
    Mat element2 = getStructuringElement(MORPH_RECT, Size(4, 4));
    Ptr<cuda::Filter> dilate = cuda::createMorphologyFilter(cv::MORPH_DILATE, dst.type(), element2);
    dilate->apply(dst, dilate_binary);
    vector<Vec4i> hierarchy;
    Mat dld_dst;
    dilate_binary.download(dld_dst);
    findContours(dld_dst, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE);//只检测最外测轮廓
  }
  checkSudoku(contours, roi_rects);
//    double parameter_1 = 70;
//    double parameter_2 = 0.05;
//    Mat drawing = Mat::zeros(dst.size(), CV_8UC3);
//    for (int i = 0; i < contours.size(); i++) {
//        Scalar color = Scalar(255, 255, 255);//rng.uniform 随机数生成
//        drawContours(drawing, contours, i, color, 2, 8, hierarchy, 0, Point());
//    }
//    imshow("ct",drawing);

//    Mat show1;
//    dst.download(show1);
//    cout << "sudoku_rects len = " << roi_rects.size() << endl;
//    drawRects(show1, roi_rects);
//    imshow("dst", show1);
//    waitKey(10);
  if (filterValidSideBoxes(roi_rects)) {
//        Mat show2;
//        dst.download(show2);
//        drawRects(show2, roi_rects);
//        imshow("filtered", show2);
//        waitKey(10);
    Spilt(roi_rects); //Separate ten rectangles into five left ones and right five ones.
    if (ChooseFourPoints()) {
      roi_rects.clear();
      rectSplit(TopLeft, TopRight, BottomLeft, BottomRight, roi_rects, 3);
      filterRoi(dst, roi_rects);
      if (roi_rects.size() != 9)
        return;
      GpuMat perspective, inverse_perspective;
//            showGpuMat("src_persp", src);
      PerspectiveTransform(src, perspective); // Implement perspective transformation.

//            perspective.convertTo(perspective, perspective.type(),3);
//            showGpuMat("perspective", perspective);
      InversePerspectiveTransform(); // inverse transformation
      vector<GpuMat> numbers;
      vector<GpuMat> cropped_numbers;
      for (int i = 0; i < 9; i++) {
        GpuMat number, cropped_number;
//                GpuMat processed_number;
        ExtractNineImage(perspective, number, i); // Separate the whole image into nine subimages.
        numbers.push_back(number);
        shrinkImg(number, cropped_number, 0.4, 0.2);
//                Crop(number, cropped_number, 3500); // Crop the digits in the image.
//                PostProcessing(cropped_number, processed_number, parameter_1, parameter_2);
//                cropped_number.convertTo(cropped_number, cropped_number.type(), 1.0 / 255.0);
//                showGpuMat("cropped_number", cropped_number);
        cropped_numbers.push_back(cropped_number);
//                cuda::threshold(processed_number, processed_number, 127, 1, THRESH_BINARY);
        roi.push_back(cropped_number);
      }
    }
  }
  for (int i = 0; i < roi_rects.size(); ++i)
    roi_rects[i] = resizeRect(roi_rects[i], resize_ratio, resize_ratio);

}

bool FireRuneSplitter::ChooseFourPoints() {
  TopLeftCorner_x.clear();
  for (size_t i = 0; i < LeftFiveRec.size(); i++) {//画矩形
    const RotatedRect &rect = LeftFiveRec[i];
    Point2f vecs[4];
    rect.points(vecs);
    TopLeftCorner_x.push_back(vecs[0].x);
    TopLeftCorner_y.push_back(vecs[1].y);
  }
  Index.clear();
  Index = AscendingSort_Indexes(TopLeftCorner_y);
  sort(TopLeftCorner_x.begin(), TopLeftCorner_x.end(), AscendingSort);
  sort(TopLeftCorner_y.begin(), TopLeftCorner_y.end(), DescendingSort);
  if (Filter())
    return false;
  TopLeftRec = LeftFiveRec[Index[0]];
  BottomLeftRec = LeftFiveRec[Index[4]];
  const RotatedRect &topleftrec = TopLeftRec;
  Point2f topleftrec_points[4];
  topleftrec.points(topleftrec_points);
  const RotatedRect &bottomleftrec = BottomLeftRec;
  Point2f bottomleftrec_points[4];
  bottomleftrec.points(bottomleftrec_points);
  TopLeftCorner_x.clear();
  TopLeftCorner_y.clear();
  for (size_t i = 0; i < RightFiveRec.size(); i++) {//画矩形
    const RotatedRect &rect = RightFiveRec[i];
    Point2f vecs[4];
    rect.points(vecs);
    TopLeftCorner_x.push_back(vecs[0].x);
    TopLeftCorner_y.push_back(vecs[1].y);
  }
  Index.clear();
  Index = AscendingSort_Indexes(TopLeftCorner_y);
  sort(TopLeftCorner_x.begin(), TopLeftCorner_x.end(), AscendingSort);
  sort(TopLeftCorner_y.begin(), TopLeftCorner_y.end(), DescendingSort);
  if (Filter())
    return false;
  TopRightRec = RightFiveRec[Index[0]];
  BottomRightRec = RightFiveRec[Index[4]];
  const RotatedRect &toprightrec = TopRightRec;
  Point2f toprightrec_points[4];
  toprightrec.points(toprightrec_points);
  const RotatedRect &bottomrightrec = BottomRightRec;
  Point2f bottomrightrec_points[4];
  bottomrightrec.points(bottomrightrec_points);
  height = abs(bottomrightrec_points[1].y - bottomrightrec_points[0].y);
  Point_1 = Point2f(topleftrec_points[2].x, topleftrec_points[2].y);
  Point_2 = Point2f(toprightrec_points[1].x, toprightrec_points[1].y);
  Point_3 = Point2f(bottomleftrec_points[3].x, bottomleftrec_points[3].y);
  Point_4 = Point2f(bottomrightrec_points[0].x, bottomrightrec_points[0].y);
  TopLeft = Point2f(topleftrec_points[2].x, topleftrec_points[2].y - height);
  TopRight = Point2f(toprightrec_points[1].x, toprightrec_points[1].y - height);
  BottomLeft = Point2f(bottomleftrec_points[3].x, bottomleftrec_points[3].y + height);
  BottomRight = Point2f(bottomrightrec_points[0].x, bottomrightrec_points[0].y + height);
  Top_Middle_First = Point2f(TopLeft.x + 1 / 3.0 * abs(TopLeft.x - TopRight.x),
                             TopLeft.y - 1 / 3.0 * abs(TopLeft.y - TopRight.y));
  Top_Middle_Sec = Point2f(TopLeft.x + 2 / 3.0 * abs(TopLeft.x - TopRight.x),
                           TopLeft.y - 2 / 3.0 * abs(TopLeft.y - TopRight.y));
  Left_Middle_First = Point2f(BottomLeft.x + 1 / 3.0 * abs(BottomLeft.x - TopLeft.x),
                              BottomLeft.y - 1 / 3.0 * abs(BottomLeft.y - TopLeft.y));
  Left_Middle_Sec = Point2f(BottomLeft.x + 2 / 3.0 * abs(BottomLeft.x - TopLeft.x),
                            BottomLeft.y - 2 / 3.0 * abs(BottomLeft.y - TopLeft.y));

  Bottom_Middle_First = Point2f(BottomLeft.x + 1 / 3.0 * abs(BottomLeft.x - BottomRight.x),
                                BottomLeft.y - 1 / 3.0 * abs(BottomLeft.y - BottomRight.y));
  Bottom_Middle_Sec = Point2f(BottomLeft.x + 2 / 3.0 * abs(BottomLeft.x - BottomRight.x),
                              BottomLeft.y - 2 / 3.0 * abs(BottomLeft.y - BottomRight.y));

  Right_Middle_First = Point2f(BottomRight.x - 1 / 3.0 * abs(BottomRight.x - TopRight.x),
                               BottomRight.y - 1 / 3.0 * abs(BottomRight.y - TopRight.y));
  Right_Middle_Sec = Point2f(BottomRight.x - 2 / 3.0 * abs(BottomRight.x - TopRight.x),
                             BottomRight.y - 2 / 3.0 * abs(BottomRight.y - TopRight.y));
  return true;
}

bool FireRuneSplitter::Filter() {
  return abs(TopLeftCorner_x[0] - TopLeftCorner_x[4]) > 50.0;
}

void FireRuneSplitter::PerspectiveTransform(cv::cuda::GpuMat &src, GpuMat &dst) {
  Point2f pts1[] = {TopLeft, BottomLeft, BottomRight, TopRight};
  Point2f pts2[] = {Point2f(0, 0), Point2f(0, HEIGHT), Point2f(WIDTH, HEIGHT), Point2f(WIDTH, 0)};
//    Point2f pts2[] = {Point2f(0, 0), Point2f(0, final.rows), Point2f(final.cols, final.rows), Point2f(final.cols, 0)};
  perspective_matrix = getPerspectiveTransform(pts1, pts2);
  inverse_per_matrix = getPerspectiveTransform(pts2, pts1);
  cuda::warpPerspective(src, dst, perspective_matrix, src.size(), INTER_LINEAR);
}

void FireRuneSplitter::InversePerspectiveTransform() {
  inverse_perspective_matrix << inverse_per_matrix.at<double>(0, 0),
      inverse_per_matrix.at<double>(0, 1),
      inverse_per_matrix.at<double>(0, 2),
      inverse_per_matrix.at<double>(1, 0),
      inverse_per_matrix.at<double>(1, 1),
      inverse_per_matrix.at<double>(1, 2),
      inverse_per_matrix.at<double>(2, 0),
      inverse_per_matrix.at<double>(2, 1),
      inverse_per_matrix.at<double>(2, 2);
//    cout << "Inverse_perspective_matrix\n" << inverse_perspective_matrix << endl;
  vector<Point2f> points;
  for (int i = 0; i < 16; i++) {
    Eigen::Vector3f point_result, point_origin;
    point_result << (WIDTH / 3.0) * (i % 4), (HEIGHT / 3.0) * (i / 4), 1;
    point_origin = inverse_perspective_matrix * point_result;
    Point2f result = Point2f(point_origin(0) / point_origin(2), point_origin(1) / point_origin(2));
    points.push_back(result);
  }

//    for(int i=0;i<9;++i){
//        int tl=i;
//        int tr=tl+1;
//        int bl=i+4;
//        int br=bl+1;
//        vector<Point2f> quad={points[tl],points[tr],points[bl],points[br]};
//        digit_rects.push_back(minAreaRect(quad));
//    }

}

void FireRuneSplitter::ExtractNineImage(cv::cuda::GpuMat &image, cv::cuda::GpuMat &dst, int number) {
  Rect Rectangle;
  Rectangle.x = (number % 3) * (WIDTH / 3);
  Rectangle.y = (number / 3) * (HEIGHT / 3);
  Rectangle.width = WIDTH / 3;
  Rectangle.height = HEIGHT / 3;
  dst = image(Rectangle);
}

void FireRuneSplitter::Crop(cv::cuda::GpuMat &image, cv::cuda::GpuMat &dst, float thresholds) {
  cuda::GpuMat threshold_output;
  vector<vector<Point> > contours;
  vector<Vec4i> hierarchy;
  cuda::threshold(image, threshold_output, 50, 255, THRESH_BINARY);
  Mat dld_src;
  threshold_output.download(dld_src);
  findContours(dld_src, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
  double min_contour_area = thresholds;
  for (auto it = contours.begin(); it != contours.end();) {
    if (contourArea(*it) < min_contour_area)
      it = contours.erase(it);
    else
      ++it;
  }
  if (contours.size() > 0) {
    vector<vector<Point> > contours_poly(contours.size());
    vector<Rect> boundRect(contours.size());
    for (size_t i = 0; i < contours.size(); i++) {
      approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);
      boundRect[i] = boundingRect(Mat(contours_poly[i]));
    }
    Mat drawing = Mat::zeros(threshold_output.size(), CV_8UC3);
    for (size_t i = 0; i < contours.size(); i++) {
      Scalar color = Scalar(rng_sec.uniform(0, 255), rng_sec.uniform(0, 255), rng_sec.uniform(0, 255));
      drawContours(drawing, contours_poly, (int) i, color, 1, 8, vector<Vec4i>(), 0, Point());
      rectangle(drawing, boundRect[i].tl(), boundRect[i].br(), color, 2, 8, 0);
    }
    if (contours.size() == 1) {
      Rect Number;
      Number.x = boundRect[0].tl().x;
      Number.y = boundRect[0].tl().y;
      Number.width = boundRect[0].br().x - boundRect[0].tl().x;
      Number.height = boundRect[0].br().y - boundRect[0].tl().y;
      dst = image(Number);
      return;
    } else {
//      cout << "dst = drawing" << endl;
      dst = GpuMat(drawing);
      return;
    }

  }
  dst = image;
}

void FireRuneSplitter::Spilt(vector<RotatedRect> &sudoku_rects) {
  for (size_t i = 0; i < sudoku_rects.size(); i++) {//画矩形
    const RotatedRect &rect = sudoku_rects[i];
    Point2f vecs[4];
    rect.points(vecs);
    TopLeftCorner_x.push_back(vecs[0].x);
  }
  // Sort rectangles along with their indexes
  Index = AscendingSort_Indexes(TopLeftCorner_x);
  sort(TopLeftCorner_x.begin(), TopLeftCorner_x.end(), AscendingSort);
  for (size_t i = 0; i < sudoku_rects.size(); i++) {
    if (i < 5)
      LeftFiveRec.push_back(sudoku_rects[Index[i]]);
    else
      RightFiveRec.push_back(sudoku_rects[Index[i]]);
  }
}

template<typename T>
std::vector<int> FireRuneSplitter::AscendingSort_Indexes(const std::vector<T> &v) {

  // initialize original index locations
  vector<int> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
            [&v](int i1, int i2) { return v[i1] < v[i2]; });

  return idx;
}

template<typename T>
vector<int> FireRuneSplitter::DescendingSort_Indexes(const vector<T> &v) {

  // initialize original index locations
  vector<int> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
            [&v](int i1, int i2) { return v[i1] > v[i2]; });

  return idx;
}

bool FireRuneSplitter::checkSudoku(const vector<vector<Point2i>> &contours,
                                   vector<RotatedRect> &sudoku_rects) {

//  cout << "----------------------------contours size = " << contours.size() << endl;
  if (contours.size() < 10)
    return false;
  float ratio = 140.0 / 69.4;
  int sudoku = 0;
  float low_threshold = 0.6;
  float high_threshold = 1.4;
//    float low_threshold = 0.1;
//    float high_threshold = 200000;
  size_t i = 0;
  RotatedRect rect;
  for (; i < contours.size(); i++) {
    rect = minAreaRect(contours[i]);
    rect = adjustRRect(rect);
    const Size2f &s = rect.size;
//        cout << "width = " << s.width << "    height = " << s.height << endl;
    float ratio_cur = s.width / s.height;
//        cout << "ratio cur = " << ratio_cur << endl;
    if (ratio_cur > 0.4 * ratio
        && ratio_cur < 1.6 * ratio
        && s.width > low_threshold * roi_width
        && s.width < high_threshold * roi_width
        && s.height > low_threshold * roi_height
        && s.height < high_threshold * roi_height
        && ((rect.angle > -10 && rect.angle < 10) || rect.angle < -170 || rect.angle > 170)
//               && ((rect.angle > -20 && rect.angle < 20) || rect.angle < -170 || rect.angle > 170)
        ) {
//            cout << "Bingo" << endl;
      sudoku_rects.push_back(rect);
      ++sudoku;
    }

  }
//  cout << "sudoku_rects =  " << sudoku << "  i" << i << endl;
  return sudoku >= 9;
}

void FireRuneSplitter::rectSplit(cv::Point2f &tl, cv::Point2f &tr, cv::Point2f &bl, cv::Point2f &br,
                                 std::vector<cv::RotatedRect> &rects, int dim) {
  vector<Point2f> tls, trs, bls, brs;
  vector<Point2f> left_split_pts, right_split_pts;
  splitLine(tl, bl, dim, left_split_pts);
  splitLine(tr, br, dim, right_split_pts);
  //horizon split
  for (int i = 0; i <= dim; ++i) {
    vector<Point2f> splits;
    splitLine(left_split_pts[i], right_split_pts[i], dim, splits);
    const size_t split_size = splits.size();
    for (int j = 0; j < split_size; ++j) {
      Point2f p = splits[j];
      if (i != 0) {
        if (j != 0)
          brs.push_back(p);
        if (j != split_size - 1)
          bls.push_back(p);
      }
      if (i != dim && j != split_size - 1)
        tls.push_back(p);

      if (i != dim && j != 0)
        trs.push_back(p);
    }
  }
//    cout<<"tls size = "<<tls.size()<<endl;
//    cout<<"trs size = "<<trs.size()<<endl;
//    cout<<"bls size = "<<bls.size()<<endl;
//    cout<<"brs size = "<<brs.size()<<endl;
  for (int i = 0; i < dim * dim; ++i) {
    vector<Point2f> quadrilateral = {tls[i], trs[i], brs[i], bls[i]};
    rects.push_back(minAreaRect(quadrilateral));
  }
}

void
FireRuneSplitter::splitLine(cv::Point2f &begin, cv::Point2f &end, int n_part, std::vector<cv::Point2f> &split_points) {
  float step_x = (end.x - begin.x) / n_part;
  float step_y = (end.y - begin.y) / n_part;
  for (int i = 0; i < n_part; ++i)
    split_points.push_back({begin.x + i * step_x, begin.y + i * step_y});
  split_points.push_back(end);
}

RotatedRect FireRuneSplitter::resizeRect(cv::RotatedRect &rect, float factor_x, float factor_y) {
  Point2f pts[4];
  rect.points(pts);
  vector<Point2f> n_pts;
  for (int i = 0; i < 4; ++i) {
    Point2f tp;
    tp.x = pts[i].x * factor_x;
    tp.y = pts[i].y * factor_y;
    n_pts.push_back(tp);
  }
  return minAreaRect(n_pts);
}

bool FireRuneSplitter::filterValidSideBoxes(std::vector<cv::RotatedRect> &rects) {
  if (rects.size() < 10)
    return false;
  sort(rects.begin(), rects.end(),
       [](const RotatedRect &r1, const RotatedRect &r2) { return r1.boundingRect().x <= r2.boundingRect().x; });
  if (rects.size() == 10)
    return true;

  vector<pair<float, int>> dist;
  for (int i = 0; i < rects.size() - 1; ++i) {
    dist.push_back(make_pair(abs(rects[i + 1].boundingRect().x - rects[i].boundingRect().x), i));
  }
  sort(dist.begin(), dist.end(),
       [](const pair<float, int> &o1, const pair<float, int> &o2) { return o1.first <= o2.first; });
  vector<pair<float, int>> candidates;
  candidates.assign(dist.begin(), dist.begin() + 9);
  sort(candidates.begin(), candidates.end(),
       [](const pair<float, int> &o1, const pair<float, int> &o2) { return o1.second <= o2.second; });
  vector<RotatedRect> selected;
  for (const auto &c:candidates) {
    selected.push_back(rects[c.second]);
  }
  rects.clear();
  rects.assign(selected.begin(), selected.end());
  return false;

}

void FireRuneSplitter::shrinkImg(const GpuMat &src, cv::cuda::GpuMat &dst, float ratio_x, float ratio_y) {
  float w = src.cols * (1 - ratio_x);
  float h = src.rows * (1 - ratio_y);
  float x1 = (src.cols - w) / 2.0;
  float y1 = (src.rows - h) / 2.0;
  Rect window(x1, y1, w, h);
  dst = src(window);
}


/*
 * Newton simulator file
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#include "Framework/Physics/Newton/spNewtonSimulator.hpp"

#ifdef SP_COMPILE_WITH_NEWTON

#include "Framework/Physics/Newton/spNewtonRigidBody.hpp"
#include "Framework/Physics/Newton/spNewtonStaticObject.hpp"
#include "Framework/Physics/Newton/spNewtonJoint.hpp"
#include "Base/spMemoryManagement.hpp"

#include <newton.h>
#include <boost/foreach.hpp>

namespace sp
{
namespace physics
{

/*
 * Static callbacks
 */

void DefCallbackGenericContactProcess(const NewtonJoint* Contact, f32 TimeStep, s32 ThreadIndex)
{
    const NewtonMaterial* Material = NewtonContactGetMaterial(NewtonContactJointGetFirstContact(Contact));

    const f32 Velocity = NewtonMaterialGetContactNormalSpeed(Material);

    dim::vector3df Point, Normal;
    NewtonMaterialGetContactPositionAndNormal(Material, &Point.X, &Normal.X);

    (PhysicsSimulator::getContactCallback())(Point, Normal, Velocity);
}

/*
 * NewtonSimulator class
 */

NewtonWorld* NewtonSimulator::NtWorld_ = 0;

NewtonSimulator::NewtonSimulator() :
    PhysicsSimulator(SIMULATOR_NEWTON)
{
    /* Print newton library information */
    io::Log::message(getVersion(), 0);
    io::Log::message("Copyright (c) <2003-2011> - Julio Jerez and Alain Suero", 0);
    io::Log::message("", 0);

    /* Create the newton world */
    NtWorld_ = NewtonCreate();

    s32 MaterialGroupID = NewtonMaterialGetDefaultGroupID(NtWorld_);
    NewtonMaterialSetDefaultFriction    (NtWorld_, MaterialGroupID, MaterialGroupID, 0.8f, 0.4f);
    NewtonMaterialSetDefaultElasticity  (NtWorld_, MaterialGroupID, MaterialGroupID, 0.3f);
    NewtonMaterialSetDefaultSoftness    (NtWorld_, MaterialGroupID, MaterialGroupID, 0.05f);
}
NewtonSimulator::~NewtonSimulator()
{
    clearScene();
    NewtonDestroy(NtWorld_);
}

io::stringc NewtonSimulator::getVersion() const
{
    io::stringc Version(NewtonWorldGetVersion());
    Version = Version.left(1) + "." + Version.right(2);
    return "NewtonGameDynamics - v." + Version;
}

void NewtonSimulator::updateSimulation(const f32 StepTime)
{
    NewtonUpdate(NtWorld_, StepTime);
}

void NewtonSimulator::setGravity(const dim::vector3df &Gravity)
{
    Gravity_ = Gravity;
    foreach (RigidBody* Body, RigidBodyList_)
        Body->setGravity(Gravity);
}

PhysicsMaterial* NewtonSimulator::createMaterial(
    f32 StaticFriction, f32 DynamicFriction, f32 Restitution)
{
    return 0; //todo
}

StaticPhysicsObject* NewtonSimulator::createStaticObject(PhysicsMaterial* Material, scene::Mesh* Mesh)
{
    if (!Mesh)
    {
        io::Log::error("Invalid mesh object for static physics object");
        return 0;
    }

    StaticPhysicsObject* NewBody = new NewtonStaticObject(Mesh);
    StaticBodyList_.push_back(NewBody);

    return NewBody;
}

RigidBody* NewtonSimulator::createRigidBody(
    PhysicsMaterial* Material, const ERigidBodies Type, scene::SceneNode* RootNode, const SRigidBodyConstruction &Construct)
{
    if (!RootNode)
    {
        io::Log::error("Invalid root node for rigid body");
        return 0;
    }

    RigidBody* NewBody = 0;

    if (Type == RIGIDBODY_MESH)
    {
        if (RootNode->getType() != scene::NODE_MESH)
        {
            io::Log::error("Create mesh rigid body with no valid mesh object");
            return 0;
        }
        else
            NewBody = allocRigidBody(static_cast<scene::Mesh*>(RootNode));
    }
    else
    {
        NewBody = allocRigidBody(Type, Construct);
        NewBody->setRootNode(RootNode);
    }

    RootNode->setParent(0);
    NewBody->setMass(25.0f);
    NewBody->setGravity(Gravity_);
    NewBody->setTransformation(RootNode->getPositionMatrix() * RootNode->getRotationMatrix());

    RigidBodyList_.push_back(NewBody);

    return NewBody;
}

RigidBody* NewtonSimulator::createRigidBody(PhysicsMaterial* Material, scene::Mesh* Mesh)
{
    if (!Mesh)
    {
        io::Log::error("Invalid mesh for rigid body");
        return 0;
    }

    RigidBody* NewBody = allocRigidBody(Mesh);

    Mesh->setParent(0);
    NewBody->setMass(25.0f);
    NewBody->setGravity(Gravity_);
    NewBody->setTransformation(Mesh->getPositionMatrix() * Mesh->getRotationMatrix());

    RigidBodyList_.push_back(NewBody);

    return NewBody;
}

PhysicsJoint* NewtonSimulator::createJoint(
    const EPhysicsJoints Type, PhysicsBaseObject* Object, const SPhysicsJointConstruct &Construct)
{
    return createJoint(Type, 0, Object, Construct);
}

PhysicsJoint* NewtonSimulator::createJoint(
    const EPhysicsJoints Type, PhysicsBaseObject* ObjectA, PhysicsBaseObject* ObjectB, const SPhysicsJointConstruct &Construct)
{
    PhysicsJoint* NewJoint = new NewtonDynamicsJoint(Type, ObjectA, ObjectB, Construct);
    JointList_.push_back(NewJoint);
    return NewJoint;
}

void NewtonSimulator::setThreadCount(s32 Count)
{
    NewtonSetThreadsCount(NtWorld_, Count);
}
s32 NewtonSimulator::getThreadCount() const
{
    return NewtonGetThreadsCount(NtWorld_);
}

void NewtonSimulator::setSolverModel(s32 Model)
{
    NewtonSetSolverModel(NtWorld_, Model);
}

void NewtonSimulator::setContactCallback(const PhysicsContactCallback &Callback)
{
    PhysicsSimulator::setContactCallback(Callback);

    s32 MaterialGroupID = NewtonMaterialGetDefaultGroupID(NtWorld_);

    if (ContactCallback_)
        NewtonMaterialSetCollisionCallback(NtWorld_, MaterialGroupID, MaterialGroupID, 0, 0, DefCallbackGenericContactProcess);
    else
        NewtonMaterialSetCollisionCallback(NtWorld_, MaterialGroupID, MaterialGroupID, 0, 0, 0);
}

NewtonWorld* NewtonSimulator::getNewtonWorld()
{
    return NtWorld_;
}

/*
 * ======= Protected: =======
 */

RigidBody* NewtonSimulator::allocRigidBody(const ERigidBodies Type, const SRigidBodyConstruction &Construct)
{
    return new NewtonRigidBody(Type, Construct);
}
RigidBody* NewtonSimulator::allocRigidBody(scene::Mesh* MeshGeom)
{
    return new NewtonRigidBody(MeshGeom);
}

/*
 * ======= Physics callbacks =======
 */

void PhysicsTransformCallback(const NewtonBody* NtBody, const f32* Transformation, s32 ThreadIndex)
{
    /* Get the rigid body object */
    RigidBody* PhysicsObject = static_cast<RigidBody*>(NewtonBodyGetUserData(NtBody));

    if (!PhysicsObject)
        return;

    scene::SceneNode* RootNode = PhysicsObject->getRootNode();

    if (!RootNode)
        return;

    /* Apply the transformation */
    const dim::matrix4f* Matrix = (const dim::matrix4f*)Transformation;

    RootNode->setPosition(Matrix->getPosition());
    RootNode->setRotationMatrix(Matrix->getRotationMatrix());
}

void PhysicsForceAndTorqueCallback(const NewtonBody* NtBody, f32 TimeStep, s32 ThreadIndex)
{
    /* Get the rigid body object */
    RigidBody* PhysicsObject = static_cast<RigidBody*>(NewtonBodyGetUserData(NtBody));

    if (!PhysicsObject)
        return;

    /* Apply the forces */
    if (PhysicsObject->getGravityCallback())
        PhysicsObject->getGravityCallback()(PhysicsObject);
    else
        PhysicsObject->setForce(PhysicsObject->getGravity() * PhysicsObject->getMass());
}

} // /namespace physics

} // /namespace sp

#endif

// ================================================================================


// ROS includes
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

// Third-party includes
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <std_srvs/Trigger.h>

// Non-standard datra messages
#include <deepdive_ros/Light.h>
#include <deepdive_ros/Lighthouses.h>
#include <deepdive_ros/Trackers.h>

// Ceres and logging
#include <ceres/ceres.h>
#include <ceres/rotation.h>

// We will use OpenCV to bootstrap solution
#include <opencv2/calib3d/calib3d.hpp>

// Ceres and logging
#include <Eigen/Core>
#include <Eigen/Geometry>

// C++ libraries
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>

// Shared local code
#include "deepdive.hh"

// GLOBAL PARAMETERS

// List of lighthouses
LighthouseMap lighthouses_;
TrackerMap trackers_;
MeasurementMap measurements_;
CorrectionMap corrections_;

// Global strings
std::string calfile_ = "deepdive.tf2";
std::string perfile_ = "/tmp/performance.csv";
std::string frame_world_ = "world";     // World frame
std::string frame_vive_ = "vive";       // Vive frame
std::string frame_body_ = "body";       // EKF / external solution
std::string frame_truth_ = "truth";     // Vive solution

// Whether to apply corrections
bool correct_ = false;

// Reesolution of the graph
double res_ = 0.1;

// Rejection thresholds
int thresh_count_ = 4;
double thresh_angle_ = 60.0;
double thresh_duration_ = 1.0;

// Solver parameters
ceres::Solver::Options options_;

// What to solve for
bool refine_trajectory_ = true;
bool refine_registration_ = true;
bool refine_lighthouses_ = false;
bool refine_extrinsics_ = false;
bool refine_sensors_ = false;
bool refine_head_ = false;
bool refine_params_ = false;

// Are we running in "offline" mode
bool offline_ = false;

// Should we publish rviz markers
bool visualize_ = true;

// Are we recording right now?
bool recording_ = false;

// Force the body frame to move on a plane
bool force2d_ = false;

// World -> vive registration
double wTv_[6];

// Smoothing factor
double smoothing_ = 10.0;

// Sensor visualization publisher
ros::Publisher pub_sensors_;
ros::Publisher pub_path_;
ros::Publisher pub_ekf_;

// Timer for managing offline
ros::Timer timer_;

// CERES SOLVER

// Helper function to apply a transform b = Ra + t
template <typename T> inline
void TransformInPlace(const T transform[6], T x[3]) {
  T tmp[3];
  ceres::AngleAxisRotatePoint(&transform[3], x, tmp);
  x[0] = tmp[0] + transform[0];
  x[1] = tmp[1] + transform[1];
  x[2] = tmp[2] + transform[2];
}

// Helper function to invert a transform a = R'(b - t)
template <typename T> inline
void InverseTransformInPlace(const T transform[6], T x[3]) {
  T aa[3], tmp[3];
  tmp[0] = x[0] - transform[0];
  tmp[1] = x[1] - transform[1];
  tmp[2] = x[2] - transform[2];
  aa[0] = -transform[3];
  aa[1] = -transform[4];
  aa[2] = -transform[5];
  ceres::AngleAxisRotatePoint(aa, tmp, x);
}

// Group of light measurements -- essential for accuracy
typedef std::map<std::pair<uint16_t, uint8_t>, double> Group;

// Residual error between predicted angles to a lighthouse
struct GroupCost {
  explicit GroupCost(Group const& group) : group_(group) {}
  // Called by ceres-solver to calculate error
  template <typename T>
  bool operator()(const T* const wTv,         // Vive -> World
                  const T* const vTl,         // Lighthouse -> vive
                  const T* const wTb_pos_xy,  // Body -> world (pos xy)
                  const T* const wTb_pos_z,   // Body -> world (pos z)
                  const T* const wTb_rot_xy,  // Body -> world (rot xy)
                  const T* const wTb_rot_z,   // Body -> world (rot z)
                  const T* const bTh,         // Head -> body
                  const T* const tTh,         // Head -> tracking (light)
                  const T* const sensors,     // Lighthouse calibration
                  const T* const params,      // Tracker extrinsics
                  T* residual) const {
    // The position of the sensor
    T x[3], angle[2], wTb[6];
    // Reconstruct a transform from the components
    wTb[0] = wTb_pos_xy[0];
    wTb[1] = wTb_pos_xy[1];
    wTb[2] = wTb_pos_z[0];
    wTb[3] = wTb_rot_xy[0];
    wTb[4] = wTb_rot_xy[1];
    wTb[5] = wTb_rot_z[0];
    // Used to index the residual
    size_t cnt = 0;
    // Iterate over all measurements
    Group::const_iterator gt;
    for (gt = group_.begin(); gt != group_.end(); gt++) {
      // Get the sensor and axis for this group
      uint16_t const& s = gt->first.first;
      uint8_t const& a = gt->first.second;
      // Get the sensor position in the tracking frame
      x[0] = sensors[6*s+0];
      x[1] = sensors[6*s+1];
      x[2] = sensors[6*s+2];
      // Project the sensor position into the lighthouse frame
      InverseTransformInPlace(tTh, x);    // light -> head
      TransformInPlace(bTh, x);           // head -> body
      TransformInPlace(wTb, x);           // body -> world
      InverseTransformInPlace(wTv, x);    // world -> vive
      InverseTransformInPlace(vTl, x);    // vive -> lighthouse
      // Predict the angles - Note that the
      Predict(params, x, angle, correct_);
      // The residual angle error for the specific axis
      residual[cnt++] = angle[a] - T(gt->second);
    }
    return true;
  }
 // Internal variables
 private:
  Group group_;
};

// Residual error between sequential poses
struct MotionCost {
  explicit MotionCost() {}
  // Called by ceres-solver to calculate error
  template <typename T>
  bool operator()(const T* const prev_pos_xy,  // PREV Body -> world (pos xy)
                  const T* const prev_pos_z,   // PREV Body -> world (pos z)
                  const T* const prev_rot_xy,  // PREV Body -> world (rot xy)
                  const T* const prev_rot_z,   // PREV Body -> world (rot z)
                  const T* const next_pos_xy,  // NEXT Body -> world (pos xy)
                  const T* const next_pos_z,   // NEXT Body -> world (pos z)
                  const T* const next_rot_xy,  // NEXT Body -> world (rot xy)
                  const T* const next_rot_z,   // NEXT Body -> world (rot z)
                  T* residual) const {
    residual[0] =  prev_pos_xy[0] - next_pos_xy[0];
    residual[1] =  prev_pos_xy[1] - next_pos_xy[1];
    residual[2] =  prev_pos_z[0] - next_pos_z[0];
    residual[3] =  prev_rot_xy[0] - next_rot_xy[0];
    residual[4] =  prev_rot_xy[1] - next_rot_xy[1];
    residual[5] =  prev_rot_z[0] - next_rot_z[0];
    for (size_t i = 0; i < 6; i++)
      residual[i] *= T(smoothing_);
    return true;
  }
};

// Solve the problem
bool Solve() {
  // Create the ceres problem
  ceres::Problem problem;

  // BASIC SANITY CHECKS

  // Check measurements
  if (measurements_.empty()) {
    ROS_WARN("No measurements received, so cannot solve the problem.");
    return false;
  } else {
    double t = (measurements_.rbegin()->first - measurements_.begin()->first).toSec();
    ROS_INFO_STREAM("Processing " << measurements_.size()
      << " measurements running for " << t << " seconds from "
      << measurements_.begin()->first << " to "
      << measurements_.rbegin()->first);
  }

  // Check corrections
  if (corrections_.empty()) {
    ROS_INFO("No corrections in dataset. Assuming first body pose at origin.");
  } else {
    double t = (corrections_.rbegin()->first - corrections_.begin()->first).toSec();
    ROS_INFO_STREAM("Processing " << corrections_.size()
      << " corrections running for " << t << " seconds from "
      << corrections_.begin()->first << " to "
      << corrections_.rbegin()->first);
  }

  // BUNDLE DATA AND CORRECTIONS

  typedef std::map<ros::Time,             // Time
            std::map<uint8_t,             // Sensor
              std::map<uint8_t,           // Axis
                std::vector<double>       // Sample
              >
            >
          > Bundle;
  std::map<std::string,                   // Tracker
    std::map<std::string,                 // Lighthouse
      Bundle
    >
  > bundle;

  std::map<ros::Time, double[6]> corr;

  // We bundle measurements into into bins of width "resolution". This allows
  // us to take the average of the measurements to improve accuracy
  {
    ROS_INFO("Bundling measurements into larger discrete time units.");
    MeasurementMap::iterator mt;
    for (mt = measurements_.begin(); mt != measurements_.end(); mt++) {
      std::string const& tserial = mt->second.light.header.frame_id;
      std::string const& lserial = mt->second.light.lighthouse;
      size_t const& a = mt->second.light.axis;
      ros::Time t = ros::Time(round(mt->first.toSec() / res_) * res_);
      std::vector<deepdive_ros::Pulse>::iterator pt;
      for (pt = mt->second.light.pulses.begin(); pt != mt->second.light.pulses.end(); pt++)
        bundle[tserial][lserial][t][pt->sensor][a].push_back(pt->angle);
    }
    ROS_INFO("Bundling corrections into larger discrete time units.");
    CorrectionMap::iterator ct;
    for (ct = corrections_.begin(); ct != corrections_.end(); ct++) {
      ros::Time t = ros::Time(round(ct->first.toSec() / res_) * res_);
      Eigen::Quaterniond q(
        ct->second.transform.rotation.w,
        ct->second.transform.rotation.x,
        ct->second.transform.rotation.y,
        ct->second.transform.rotation.z);
      Eigen::AngleAxisd aa(q.toRotationMatrix());
      corr[t][0] = ct->second.transform.translation.x;
      corr[t][1] = ct->second.transform.translation.y;
      corr[t][2] = ct->second.transform.translation.z;
      corr[t][3] = aa.angle() * aa.axis()[0];
      corr[t][4] = aa.angle() * aa.axis()[1];
      corr[t][5] = aa.angle() * aa.axis()[2];
    }
  }

  // The ultimate quantity we are solving for
  std::map<ros::Time, double[6]> wTb;

  // We are going to estimate the pose of each slave lighthouse in the frame
  // of the master lighthouse (vive frame) using PNP. We can think of the
  // two lighthouses as a stereo pair that are looking at a set of corresp-
  // ondences (photosensors). We want to calibrate this stereo pair.
  {
    ROS_INFO("Using P3P to estimate tracker pose in light frame.");
    // Create a new ceres problem to solve
    ceres::Problem problem;
    // Various lighjthouse parameters
    double fov = 2.0944;                          // 120deg FOV
    double w = 1.0;                               // 1m synthetic image plane
    double z = w / (2.0 * std::tan(fov / 2.0));   // Principle distance
    uint32_t count = 0;                           // Track num transforms
    // This recursively calculates the mean, std dev for a variable
    Statistic height;
    // Iterate over lighthouses
    LighthouseMap::iterator lt;
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
      // Iterate over trackers
      TrackerMap::iterator tt;
      for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
        ROS_INFO_STREAM("- Slave " << lt->first << " and tracker " << tt->first);
        // Iterate over time epochs
        Bundle::iterator bt;
        for (bt = bundle[tt->first][lt->first].begin();
          bt != bundle[tt->first][lt->first].end(); bt++) {
          // One for each time instance
          std::vector<cv::Point3f> obj;
          std::vector<cv::Point2f> img;
          Group group;
          // Try and find correspondences for every possible sensor
          for (uint8_t s = 0; s < NUM_SENSORS; s++) {
            // Mean angles for the <lighthouse, axis>
            double angles[2];
            // Check that we have azimuth/elevation for both lighthouses
            if (!Mean(bundle[tt->first][lt->first][bt->first][s][0], angles[0]) ||
                !Mean(bundle[tt->first][lt->first][bt->first][s][1], angles[1]))
              continue;
            // Add the pre-corrected angles to the light group
            group[std::pair<uint16_t, uint8_t>(s, 0)] = angles[0];
            group[std::pair<uint16_t, uint8_t>(s, 1)] = angles[1];
            // Correct the angles using the lighthouse parameters
            Correct(lt->second.params, angles, correct_);
            // Push on the correct world sensor position
            obj.push_back(cv::Point3f(
              trackers_[tt->first].sensors[s * 6 + 0],
              trackers_[tt->first].sensors[s * 6 + 1],
              trackers_[tt->first].sensors[s * 6 + 2]));
            // Push on the coordinate in the slave image plane
            img.push_back(cv::Point2f(z * tan(angles[0]), z * tan(angles[1])));
          }
          // In the case that we have 4 or more measurements, then we can try
          // and estimate the trackers location in the lighthouse frame.
          if (obj.size() > 3) {
            // If we do't want to refine the trajectory, just use the corrections
            // as estimates of the sensor trajectory. This is mainly to help
            // solve for extrinsics and lighthouse prameters.
            if (!refine_trajectory_) {
              std::map<ros::Time, double[6]>::iterator ct = corr.find(bt->first);
              if (ct == corr.end())
                continue;
              for (size_t i = 0; i < 6; i++)
                wTb[bt->first][i] = ct->second[i];
            // If we are solving for trajectory, get a nice initial estimate
            // using PNP. Otherwise, the majority of the solvers effort goes
            // into moving each pose in the trajectory.
            } else {
              cv::Mat cam = cv::Mat::eye(3, 3, cv::DataType<double>::type);
              cv::Mat dist;
              cam.at<double>(0, 0) = z;
              cam.at<double>(1, 1) = z;
              cv::Mat R(3, 1, cv::DataType<double>::type);
              cv::Mat T(3, 1, cv::DataType<double>::type);
              cv::Mat C(3, 3, cv::DataType<double>::type);
              if (!cv::solvePnPRansac(obj, img, cam, dist, R, T,  false,
                100, 8.0, 0.99, cv::noArray(), cv::SOLVEPNP_UPNP)) continue;
              cv::Rodrigues(R, C);
              Eigen::Matrix3d rot;
              for (size_t r = 0; r < 3; r++)
                for (size_t c = 0; c < 3; c++)
                  rot(r, c) = C.at<double>(r, c);
              // Get the transform from the trackng to lighthouse frame
              Eigen::Affine3d lTt;
              lTt.translation()[0] = T.at<double>(0, 0);
              lTt.translation()[1] = T.at<double>(1, 0);
              lTt.translation()[2] = T.at<double>(2, 0);
              lTt.linear() = rot;
              // This is a great initial estimate of the true location
              Eigen::Affine3d obs;
              obs = CeresToEigen(wTv_)                  // vive -> world
                  * CeresToEigen(lt->second.vTl)        // lighthouse -> vive
                  * lTt                                 // tracking -> lighthouse
                  * CeresToEigen(tt->second.tTh)        // head -> tracking
                  * CeresToEigen(tt->second.bTh, true); // body -> head
              // Set the initial estimate to this pose
              Eigen::AngleAxisd aa(obs.linear());
              wTb[bt->first][0] = obs.translation()[0];
              wTb[bt->first][1] = obs.translation()[1];
              wTb[bt->first][2] = obs.translation()[2];
              wTb[bt->first][3] = aa.angle() * aa.axis()[0];
              wTb[bt->first][4] = aa.angle() * aa.axis()[1];
              wTb[bt->first][5] = aa.angle() * aa.axis()[2];
            }
            // Recursive calculation of mean
            height.Feed(wTb[bt->first][2]);
            // Add the cost function
            ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<GroupCost,
              ceres::DYNAMIC, 6, 6, 2, 1, 2, 1, 6, 6, NUM_SENSORS * 6,
                NUM_PARAMS * 2>(new GroupCost(group), group.size());
            // Add the residual block
            problem.AddResidualBlock(cost, new ceres::HuberLoss(1.0),
              reinterpret_cast<double*>(wTv_),
              reinterpret_cast<double*>(lt->second.vTl),
              reinterpret_cast<double*>(&wTb[bt->first][0]),
              reinterpret_cast<double*>(&wTb[bt->first][2]),
              reinterpret_cast<double*>(&wTb[bt->first][3]),
              reinterpret_cast<double*>(&wTb[bt->first][5]),
              reinterpret_cast<double*>(tt->second.bTh),
              reinterpret_cast<double*>(tt->second.tTh),
              reinterpret_cast<double*>(tt->second.sensors),
              reinterpret_cast<double*>(lt->second.params));
            // If we do not want the trajectory refined then mark all parts of
            // the trajectory as constant blocks
            if (!refine_trajectory_) {
              problem.SetParameterBlockConstant(&wTb[bt->first][0]);
              problem.SetParameterBlockConstant(&wTb[bt->first][2]);
              problem.SetParameterBlockConstant(&wTb[bt->first][3]);
              problem.SetParameterBlockConstant(&wTb[bt->first][5]);
            }
            // If we are forcing 3D, then set the pitch and roll
            if (force2d_) {
              wTb[bt->first][3] = 0.0;    // Pitch
              wTb[bt->first][4] = 0.0;    // Roll
              problem.SetParameterBlockConstant(&wTb[bt->first][2]);
              problem.SetParameterBlockConstant(&wTb[bt->first][3]);
            }
            // If we have a previous node, then link with a motion cost
            if (smoothing_ > 0) {
              std::map<ros::Time, double[6]>::iterator c = wTb.find(bt->first);
              std::map<ros::Time, double[6]>::iterator p = std::prev(c);
              if (c != wTb.end() && p != c) {
                // Create a cost function to represent motion
                ceres::CostFunction* cost = new ceres::AutoDiffCostFunction
                      <MotionCost, 6, 2, 1, 2, 1, 2, 1, 2, 1>(new MotionCost());
                // Add a residual block for error
                problem.AddResidualBlock(cost, new ceres::HuberLoss(1.0),
                  reinterpret_cast<double*>(&p->second[0]),  // pos: xy
                  reinterpret_cast<double*>(&p->second[2]),  // pos: z
                  reinterpret_cast<double*>(&p->second[3]),  // rot: xy
                  reinterpret_cast<double*>(&p->second[5]),  // rot: z
                  reinterpret_cast<double*>(&c->second[0]),  // pos: xy
                  reinterpret_cast<double*>(&c->second[2]),  // pos: z
                  reinterpret_cast<double*>(&c->second[3]),  // rot: xy
                  reinterpret_cast<double*>(&c->second[5])); // rot: z
              }
            }
            count++;
          }
        }
        // Fix tracker parameters
        if (!refine_extrinsics_)
          problem.SetParameterBlockConstant(tt->second.bTh);
        if (!refine_head_)
          problem.SetParameterBlockConstant(tt->second.tTh);
        if (!refine_sensors_)
          problem.SetParameterBlockConstant(tt->second.sensors);
      }
       // Fix lighthouse parameters
      if (!refine_lighthouses_ || lt == lighthouses_.begin())
        problem.SetParameterBlockConstant(lt->second.vTl);
      if (!refine_params_)
        problem.SetParameterBlockConstant(lt->second.params);
    }
    if (!refine_registration_)
      problem.SetParameterBlockConstant(wTv_);

    // If we have a fixed the height use the mean height estimate
    if (force2d_) {
      std::map<ros::Time, double[6]>::iterator it;
      for (it = wTb.begin(); it != wTb.end(); it++)
        it->second[2] = height.Mean();
    }

    // Lighthouse after solving
    {
      LighthouseMap::iterator lt;
      for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
        ROS_INFO_STREAM("Lighthouse BEFORE solving: " << lt->first);
        ROS_INFO_STREAM("- px: " << lt->second.vTl[0]);
        ROS_INFO_STREAM("- py: " << lt->second.vTl[1]);
        ROS_INFO_STREAM("- pz: " << lt->second.vTl[2]);
        ROS_INFO_STREAM("- rx: " << lt->second.vTl[3]);
        ROS_INFO_STREAM("- ry: " << lt->second.vTl[4]);
        ROS_INFO_STREAM("- rz: " << lt->second.vTl[5]);
      }
    }

    // Extrinsocs before solving
    {
      TrackerMap::iterator tt;
      for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
        ROS_INFO_STREAM("Extrinsics BEFORE solving: " << tt->first);
        ROS_INFO_STREAM("- px: " << tt->second.bTh[0]);
        ROS_INFO_STREAM("- py: " << tt->second.bTh[1]);
        ROS_INFO_STREAM("- pz: " << tt->second.bTh[2]);
        ROS_INFO_STREAM("- rx: " << tt->second.bTh[3]);
        ROS_INFO_STREAM("- ry: " << tt->second.bTh[4]);
        ROS_INFO_STREAM("- rz: " << tt->second.bTh[5]);
      }
    }

    // Parameters before solving
    {
      LighthouseMap::iterator lt;
      for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
        ROS_INFO_STREAM("Parameters BEFORE solving: " << lt->first);
        for (uint8_t a = 0; a < 2; a++) {
          ROS_INFO_STREAM("AXIS " << a);
          ROS_INFO_STREAM("- phase: " << lt->second.params[a*NUM_PARAMS + PARAM_PHASE]);
          ROS_INFO_STREAM("- tilt: " << lt->second.params[a*NUM_PARAMS + PARAM_TILT]);
          ROS_INFO_STREAM("- gib phase: " << lt->second.params[a*NUM_PARAMS + PARAM_GIB_PHASE]);
          ROS_INFO_STREAM("- git mag: " << lt->second.params[a*NUM_PARAMS + PARAM_GIB_MAG]);
          ROS_INFO_STREAM("- curve: " << lt->second.params[a*NUM_PARAMS + PARAM_CURVE]);
        }
      }
    }

    // Now solve the problem
    ROS_INFO_STREAM("Solving optimization problem with " << count << " obs");
    ceres::Solver::Summary summary;
    ceres::Solve(options_, &problem, &summary);
    if (summary.IsSolutionUsable()) {
      ROS_INFO("Usable solution found.");
      if (visualize_) {
        ROS_INFO("- Visualizing");
        nav_msgs::Path msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = frame_world_;
        std::map<ros::Time, double[6]>::iterator it;
        for (it = wTb.begin(); it != wTb.end(); it++) {
          geometry_msgs::PoseStamped ps;
          Eigen::Vector3d v(it->second[3], it->second[4], it->second[5]);
          Eigen::AngleAxisd aa;
          if (v.norm() > 0) {
            aa.angle() = v.norm();
            aa.axis() = v.normalized();
          }
          Eigen::Quaterniond q(aa);
          ps.header.stamp = it->first;
          ps.header.frame_id = frame_world_;
          ps.pose.position.x = it->second[0];
          ps.pose.position.y = it->second[1];
          ps.pose.position.z = it->second[2];
          ps.pose.orientation.w = q.w();
          ps.pose.orientation.x = q.x();
          ps.pose.orientation.y = q.y();
          ps.pose.orientation.z = q.z();
          msg.poses.push_back(ps);
        }
        pub_path_.publish(msg);
      }
      ROS_INFO("- Writing performance to file");
      std::ofstream outfile(perfile_);
      if (outfile.is_open()) {
        nav_msgs::Path msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = frame_world_;
        std::map<ros::Time, double[6]>::iterator it, ct;
        for (it = wTb.begin(); it != wTb.end(); it++) {
          ct = corr.find(it->first);
          if (ct == corr.end())
            continue;
          outfile << (it->first.toSec() - wTb.begin()->first.toSec()) << ","
                  << it->second[0] << ","
                  << it->second[1] << ","
                  << it->second[2] << ","
                  << it->second[3] << ","
                  << it->second[4] << ","
                  << it->second[5] << ","
                  << ct->second[0] << ","
                  << ct->second[1] << ","
                  << ct->second[2] << ","
                  << ct->second[3] << ","
                  << ct->second[4] << ","
                  << ct->second[5] << std::endl;
          // Add the pose
          geometry_msgs::PoseStamped ps;
          Eigen::Vector3d v(ct->second[3], ct->second[4], ct->second[5]);
          Eigen::AngleAxisd aa;
          if (v.norm() > 0) {
            aa.angle() = v.norm();
            aa.axis() = v.normalized();
          }
          Eigen::Quaterniond q(aa);
          ps.header.stamp = ct->first;
          ps.header.frame_id = frame_world_;
          ps.pose.position.x = ct->second[0];
          ps.pose.position.y = ct->second[1];
          ps.pose.position.z = ct->second[2];
          ps.pose.orientation.w = q.w();
          ps.pose.orientation.x = q.x();
          ps.pose.orientation.y = q.y();
          ps.pose.orientation.z = q.z();
          msg.poses.push_back(ps);
        }
        outfile.close();
        pub_ekf_.publish(msg);
      }
      // Update transforms so we can see the solution iun rviz
      SendTransforms(frame_world_, frame_vive_, frame_body_,
        wTv_, lighthouses_, trackers_);
    } else {
      ROS_WARN("Solution is not usable.");
      return false;
    }
  }

  // Lighthouse after solving
  {
    LighthouseMap::iterator lt;
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
      ROS_INFO_STREAM("Lighthouse AFTER solving: " << lt->first);
      ROS_INFO_STREAM("- px: " << lt->second.vTl[0]);
      ROS_INFO_STREAM("- py: " << lt->second.vTl[1]);
      ROS_INFO_STREAM("- pz: " << lt->second.vTl[2]);
      ROS_INFO_STREAM("- rx: " << lt->second.vTl[3]);
      ROS_INFO_STREAM("- ry: " << lt->second.vTl[4]);
      ROS_INFO_STREAM("- rz: " << lt->second.vTl[5]);
    }
  }

  // Extrinsics after solving
  {
    TrackerMap::iterator tt;
    for (tt = trackers_.begin(); tt != trackers_.end(); tt++) {
      ROS_INFO_STREAM("Extrinsics AFTER solving: " << tt->first);
      ROS_INFO_STREAM("- px: " << tt->second.bTh[0]);
      ROS_INFO_STREAM("- py: " << tt->second.bTh[1]);
      ROS_INFO_STREAM("- pz: " << tt->second.bTh[2]);
      ROS_INFO_STREAM("- rx: " << tt->second.bTh[3]);
      ROS_INFO_STREAM("- ry: " << tt->second.bTh[4]);
      ROS_INFO_STREAM("- rz: " << tt->second.bTh[5]);
    }
  }

  // Parameters before solving
  {
    LighthouseMap::iterator lt;
    for (lt = lighthouses_.begin(); lt != lighthouses_.end(); lt++) {
      ROS_INFO_STREAM("Parameters AFTER solving: " << lt->first);
      for (uint8_t a = 0; a < 2; a++) {
        ROS_INFO_STREAM("AXIS " << a);
        ROS_INFO_STREAM("- phase: " << lt->second.params[a*NUM_PARAMS + PARAM_PHASE]);
        ROS_INFO_STREAM("- tilt: " << lt->second.params[a*NUM_PARAMS + PARAM_TILT]);
        ROS_INFO_STREAM("- gib phase: " << lt->second.params[a*NUM_PARAMS + PARAM_GIB_PHASE]);
        ROS_INFO_STREAM("- git mag: " << lt->second.params[a*NUM_PARAMS + PARAM_GIB_MAG]);
        ROS_INFO_STREAM("- curve: " << lt->second.params[a*NUM_PARAMS + PARAM_CURVE]);
      }
    }
  }

  return true;
}

// MESSAGE CALLBACKS

void LightCallback(deepdive_ros::Light::ConstPtr const& msg) {
  // Reset the timer use din offline mode to determine the end of experiment
  timer_.stop();
  timer_.start();
  // Check that we are recording and that the tracker/lighthouse is ready
  if (!recording_ ||
    trackers_.find(msg->header.frame_id) == trackers_.end() ||
    lighthouses_.find(msg->lighthouse) == lighthouses_.end() ||
    !trackers_[msg->header.frame_id].ready ||
    !lighthouses_[msg->lighthouse].ready) return;
  // Copy over the data
  size_t deleted = 0;
  deepdive_ros::Light data = *msg;
  std::vector<deepdive_ros::Pulse>::iterator it = data.pulses.end();
  while (it-- > data.pulses.begin()) {
    if (it->angle > thresh_angle_ / 57.2958 &&    // Check angle
        it->duration < thresh_duration_ / 1e-6) {  // Check duration
      it = data.pulses.erase(it);
      deleted++;
    }
  }
  if (data.pulses.size() < thresh_count_)
    return;
  // Add the data
  measurements_[ros::Time::now()].light = data;
}

void CorrectionCallback(tf2_msgs::TFMessage::ConstPtr const& msg) {
  // Check that we are recording and that the tracker/lighthouse is ready
  if (!recording_)
    return;
  std::vector<geometry_msgs::TransformStamped>::const_iterator it;
  for (it = msg->transforms.begin(); it != msg->transforms.end(); it++) {
    if (it->header.frame_id == frame_world_ &&
        it->child_frame_id == frame_body_) {
      corrections_[ros::Time::now()] = *it;
    }
  }
}

bool TriggerCallback(std_srvs::Trigger::Request  &req,
                     std_srvs::Trigger::Response &res)
{
  if (!recording_) {
    res.success = true;
    res.message = "Recording started.";
  }
  if (recording_) {
    // Solve the problem
    res.success = Solve();
    if (res.success)
      res.message = "Recording stopped. Solution found.";
    else
      res.message = "Recording stopped. Solution not found.";
    // Clear all the data and corrections
    measurements_.clear();
    corrections_.clear();
  }
  // Toggle recording state
  recording_ = !recording_;
  // Success
  return true;
}

// Fake a trigger when the timer expires
void TimerCallback(ros::TimerEvent const& event) {
  std_srvs::Trigger::Request req;
  std_srvs::Trigger::Response res;
  TriggerCallback(req, res);
}

// Called when a new lighthouse appears
void NewLighthouseCallback(LighthouseMap::iterator lighthouse) {
  ROS_INFO_STREAM("Found lighthouse " << lighthouse->first);
}

// Called when a new tracker appears
void NewTrackerCallback(TrackerMap::iterator tracker) {
  ROS_INFO_STREAM("Found tracker " << tracker->first);
  if (visualize_) {
    visualization_msgs::MarkerArray msg;
    std::map<std::string, Tracker>::iterator jt;
    size_t n = 0;
    for (jt = trackers_.begin(); jt != trackers_.end(); jt++, n++)  {
      for (uint16_t i = 0; i < NUM_SENSORS; i++) {
        // All this code just to convert a normal to a quaternion
        Eigen::Vector3d vfwd(jt->second.sensors[6*i+3],
          jt->second.sensors[6*i+4], jt->second.sensors[6*i+5]);
        if (vfwd.norm() > 0) {
          Eigen::Vector3d vdown(0.0, 0.0, 1.0);
          Eigen::Vector3d vright = vdown.cross(vfwd);
          vfwd = vfwd.normalized();
          vright = vright.normalized();
          vdown = vdown.normalized();
          Eigen::Matrix3d dcm;
          dcm << vfwd.x(), vright.x(), vdown.x(),
                 vfwd.y(), vright.y(), vdown.y(),
                 vfwd.z(), vright.z(), vdown.z();
          Eigen::Quaterniond q(dcm);
          // Now plot an arrow representing the normal
          static visualization_msgs::Marker marker;
          marker.header.frame_id = jt->first + "/light";
          marker.header.stamp = ros::Time::now();
          marker.ns = "sensors";
          marker.id = NUM_SENSORS * n + i;
          marker.type = visualization_msgs::Marker::ARROW;
          marker.action = visualization_msgs::Marker::ADD;
          marker.pose.position.x = jt->second.sensors[6*i+0];
          marker.pose.position.y = jt->second.sensors[6*i+1];
          marker.pose.position.z = jt->second.sensors[6*i+2];
          marker.pose.orientation.w = q.w();
          marker.pose.orientation.x = q.x();
          marker.pose.orientation.y = q.y();
          marker.pose.orientation.z = q.z();
          marker.scale.x = 0.010;
          marker.scale.y = 0.001;
          marker.scale.z = 0.001;
          marker.color.a = 1.0;
          marker.color.r = 1.0;
          marker.color.g = 0.0;
          marker.color.b = 0.0;
          msg.markers.push_back(marker);
        }
      }
    }
    pub_sensors_.publish(msg);
  }
}

// MAIN ENTRY POINT

int main(int argc, char **argv) {
  // Initialize ROS and create node handle
  ros::init(argc, argv, "deepdive_registration");
  ros::NodeHandle nh("~");

  // If we are in offline mode when we will replay the data back at 10x the
  // speed, using it all to find a calibration solution for both the body
  // as a function of  time and the lighthouse positions.
  if (!nh.getParam("offline", offline_))
    ROS_FATAL("Failed to get if we are running in offline mode.");
  if (offline_) {
    ROS_INFO("We are in offline mode. Speed-up is possible.");
    recording_ = true;
  }

  // Get the calibration file
  if (!nh.getParam("calfile", calfile_))
    ROS_FATAL("Failed to get the calfile file.");

  // Get the performance file
  if (!nh.getParam("perfile", perfile_))
    ROS_FATAL("Failed to get the perfile file.");

  // Get some global information
  if (!nh.getParam("frames/world", frame_world_))
    ROS_FATAL("Failed to get frames/world parameter.");
  if (!nh.getParam("frames/vive", frame_vive_))
    ROS_FATAL("Failed to get frames/vive parameter.");
  if (!nh.getParam("frames/body", frame_body_))
    ROS_FATAL("Failed to get frames/body parameter.");
  if (!nh.getParam("frames/truth", frame_truth_))
    ROS_FATAL("Failed to get frames/truth parameter.");

  // Get the pose graph resolution
  if (!nh.getParam("resolution", res_))
    ROS_FATAL("Failed to get resolution parameter.");

  // Get the thresholds
  if (!nh.getParam("thresholds/count", thresh_count_))
    ROS_FATAL("Failed to get threshods/count parameter.");
  if (!nh.getParam("thresholds/angle", thresh_angle_))
    ROS_FATAL("Failed to get thresholds/angle parameter.");
  if (!nh.getParam("thresholds/duration", thresh_duration_))
    ROS_FATAL("Failed to get thresholds/duration parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("correct", correct_))
    ROS_FATAL("Failed to get correct parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("force2d", force2d_))
    ROS_FATAL("Failed to get force2d parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("smoothing", smoothing_))
    ROS_FATAL("Failed to get smoothing parameter.");

  // What to refine
  if (!nh.getParam("refine/registration", refine_registration_))
    ROS_FATAL("Failed to get refine/registration parameter.");
  if (!nh.getParam("refine/trajectory", refine_trajectory_))
    ROS_FATAL("Failed to get refine/trajectory parameter.");
  if (!nh.getParam("refine/lighthouses", refine_lighthouses_))
    ROS_FATAL("Failed to get refine/lighthouses parameter.");
  if (!nh.getParam("refine/extrinsics", refine_extrinsics_))
    ROS_FATAL("Failed to get refine/extrinsics parameter.");
  if (!nh.getParam("refine/sensors", refine_sensors_))
    ROS_FATAL("Failed to get refine/sensors parameter.");
  if (!nh.getParam("refine/head", refine_head_))
    ROS_FATAL("Failed to get refine/head parameter.");
  if (!nh.getParam("refine/params", refine_params_))
    ROS_FATAL("Failed to get refine/params parameter.");

  // Define the ceres problem
  options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  if (!nh.getParam("solver/max_time", options_.max_solver_time_in_seconds))
    ROS_FATAL("Failed to get the solver/max_time parameter.");
  if (!nh.getParam("solver/max_iterations", options_.max_num_iterations))
    ROS_FATAL("Failed to get the solver/max_iterations parameter.");
  if (!nh.getParam("solver/threads", options_.num_threads))
    ROS_FATAL("Failed to get the solver/threads parameter.");
  if (!nh.getParam("solver/debug", options_.minimizer_progress_to_stdout))
    ROS_FATAL("Failed to get the solver/debug parameter.");

  // Visualization option
  if (!nh.getParam("visualize", visualize_))
    ROS_FATAL("Failed to get the visualize parameter.");

  // Get the parent information
  std::vector<std::string> lighthouses;
  if (!nh.getParam("lighthouses", lighthouses))
    ROS_FATAL("Failed to get the lighthouse list.");
  std::vector<std::string>::iterator it;
  for (it = lighthouses.begin(); it != lighthouses.end(); it++) {
    std::string serial;
    if (!nh.getParam(*it + "/serial", serial))
      ROS_FATAL("Failed to get the lighthouse serial.");
    std::vector<double> transform;
    if (!nh.getParam(*it + "/transform", transform))
      ROS_FATAL("Failed to get the lighthouse transform.");
    if (transform.size() != 7) {
      ROS_FATAL("Failed to parse lighthouse transform.");
      continue;
    }
    Eigen::Quaterniond q(
      transform[6],  // qw
      transform[3],  // qx
      transform[4],  // qy
      transform[5]); // qz
    Eigen::AngleAxisd aa(q);
    lighthouses_[serial].vTl[0] = transform[0];
    lighthouses_[serial].vTl[1] = transform[1];
    lighthouses_[serial].vTl[2] = transform[2];
    lighthouses_[serial].vTl[3] = aa.angle() * aa.axis()[0];
    lighthouses_[serial].vTl[4] = aa.angle() * aa.axis()[1];
    lighthouses_[serial].vTl[5] = aa.angle() * aa.axis()[2];
    lighthouses_[serial].ready = false;
  }

  // Get the parent information
  std::vector<std::string> trackers;
  if (!nh.getParam("trackers", trackers))
    ROS_FATAL("Failed to get the tracker list.");
  std::vector<std::string>::iterator jt;
  for (jt = trackers.begin(); jt != trackers.end(); jt++) {
    std::string serial;
    if (!nh.getParam(*jt + "/serial", serial))
      ROS_FATAL("Failed to get the tracker serial.");
    std::vector<double> extrinsics;
    if (!nh.getParam(*jt + "/extrinsics", extrinsics))
      ROS_FATAL("Failed to get the tracker extrinsics.");
    if (extrinsics.size() != 7) {
      ROS_FATAL("Failed to parse tracker extrinsics.");
      continue;
    }
    Eigen::Quaterniond q(
      extrinsics[6],  // qw
      extrinsics[3],  // qx
      extrinsics[4],  // qy
      extrinsics[5]); // qz
    Eigen::AngleAxisd aa(q);
    trackers_[serial].bTh[0] = extrinsics[0];
    trackers_[serial].bTh[1] = extrinsics[1];
    trackers_[serial].bTh[2] = extrinsics[2];
    trackers_[serial].bTh[3] = aa.angle() * aa.axis()[0];
    trackers_[serial].bTh[4] = aa.angle() * aa.axis()[1];
    trackers_[serial].bTh[5] = aa.angle() * aa.axis()[2];
    trackers_[serial].ready = false;
  }

  // If reading the configuration file results in inserting the correct
  // number of static transforms into the problem, then we can publish
  // the solution for use by other entities in the system.
  if (ReadConfig(calfile_, frame_world_, frame_vive_, frame_body_,
    wTv_, lighthouses_, trackers_)) {
    ROS_INFO("Read transforms from calibration");
  } else {
    ROS_INFO("Could not read calibration file");
  }
  SendTransforms(frame_world_, frame_vive_, frame_body_,
    wTv_, lighthouses_, trackers_);

  // Subscribe to tracker and lighthouse updates
  ros::Subscriber sub_tracker  =
    nh.subscribe<deepdive_ros::Trackers>("/trackers", 1000, std::bind(
      TrackerCallback, std::placeholders::_1, std::ref(trackers_),
        NewTrackerCallback));
  ros::Subscriber sub_lighthouse =
    nh.subscribe<deepdive_ros::Lighthouses>("/lighthouses", 1000, std::bind(
      LighthouseCallback, std::placeholders::_1, std::ref(lighthouses_),
        NewLighthouseCallback));
  ros::Subscriber sub_light =
    nh.subscribe("/light", 1000, LightCallback);
  ros::Subscriber sub_corrections =
    nh.subscribe("/tf", 1000, CorrectionCallback);
  ros::ServiceServer service =
    nh.advertiseService("/trigger", TriggerCallback);

  // Publish sensor location and body trajectory
  pub_sensors_ =
    nh.advertise<visualization_msgs::MarkerArray>("/sensors", 10, true);
  pub_path_ =
    nh.advertise<nav_msgs::Path>("/path", 10, true);
  pub_ekf_ =
    nh.advertise<nav_msgs::Path>("/truth", 10, true);

  // Setup a timer to automatically trigger solution on end of experiment
  timer_ = nh.createTimer(ros::Duration(1.0), TimerCallback, true, false);

  // Block until safe shutdown
  ros::spin();

  // Success!
  return 0;
}