#include <boost/python.hpp>
#include <boost/python/object.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/list.hpp>

#include <boost/scoped_ptr.hpp>
#include <boost/scoped_array.hpp>

#include <mmdb/mmdb_manager.h>
#include <ssm/ssm_align.h>
#include <ssm/ssm_malign.h>

namespace ccp4io_adaptbx { namespace boost_python {

using namespace ssm;
using namespace mmdb;

class PySSMAlign : public Align
{
  public:
    PySSMAlign() {}
    ~PySSMAlign() {}

    boost::python::tuple get_t_matrix() const
    {
      return boost::python::make_tuple(
        TMatrix[0][0], TMatrix[0][1], TMatrix[0][2], TMatrix[0][3],
        TMatrix[1][0], TMatrix[1][1], TMatrix[1][2], TMatrix[1][3],
        TMatrix[2][0], TMatrix[2][1], TMatrix[2][2], TMatrix[2][3]
        );
    }

    boost::python::list get_q_values() const
    {
        boost::python::list l;
        realtype* pqvalues = GetQvalues();

        for ( int i = 0; i < GetNMatches(); ++i )
        {
            l.append( pqvalues[ i ] );
        }

        return l;
    }
};

class ResidueData
{
  public:
    realtype hydropathy;
    boost::python::str chain_id, resname, inscode;
    int sse_type, resseq;

    ResidueData(
      const realtype& hydropathy_,
      const ChainID& chain_id_,
      const ResName& resname_,
      const InsCode& inscode_,
      int sse_type_,
      int resseq_
      )
      : hydropathy( hydropathy_ ),
        chain_id(
          chain_id_[0] == '\0' ? " " : boost::python::str( chain_id_, 1 )
          ),
        resname( boost::python::str( resname_, 3 ) ),
        inscode(
          inscode_[0] == '\0' ? " " : boost::python::str( inscode_, 1 )
          ),
        sse_type( sse_type_ ),
        resseq( resseq_ )
    {}

    ~ResidueData() {}
};

class PyXAlignText : public XAlignText
{
  private:
    int length;

  public:
    PyXAlignText() : length( 0 ) {}
    ~PyXAlignText() {}

    void py_x_align(PManager m1, PManager m2, PySSMAlign& cssm)
    {
      PPAtom Calpha1,Calpha2;
      int nat1,nat2,nr;

      m1->GetSelIndex ( cssm.selHndCa1,Calpha1,nat1 );
      m2->GetSelIndex ( cssm.selHndCa2,Calpha2,nat2 );

      align(
        cssm.G1, Calpha1, cssm.Ca1, nat1,
        cssm.G2, Calpha2, cssm.Ca2, nat2,
        cssm.dist1, length
        );
    }

    boost::python::list get_blocks()
    {
      boost::python::list l;

      PXTAlign XTA = GetTextRows();

      for( int i = 0; i < length; ++i)
      {
        boost::python::tuple equivs;
        boost::python::tuple info;

        switch ( XTA[i].alignKey )
        {
        case 0: // KEY_ALIGNED
          info = boost::python::make_tuple(
            XTA[i].dist,
            XTA[i].loopNo,
            XTA[i].simindex
            );
            // no break!

        case 1: // KEY_NOT_ALIGNED
          equivs = boost::python::make_tuple(
            ResidueData(
              XTA[i].hydropathy1,
              XTA[i].chID1,
              XTA[i].resName1,
              XTA[i].insCode1,
              XTA[i].sseType1,
              XTA[i].seqNum1
              ),
            ResidueData(
              XTA[i].hydropathy2,
              XTA[i].chID2,
              XTA[i].resName2,
              XTA[i].insCode2,
              XTA[i].sseType2,
              XTA[i].seqNum2
              )
            );
          break;

        case 2: // KEY_FIRST_GAP
          equivs = boost::python::make_tuple(
            boost::python::object(),
            ResidueData(
              XTA[i].hydropathy2,
              XTA[i].chID2,
              XTA[i].resName2,
              XTA[i].insCode2,
              XTA[i].sseType2,
              XTA[i].seqNum2
              )
            );
          break;

        case 3: // KEY_SECOND_GAP
          equivs = boost::python::make_tuple(
            ResidueData(
              XTA[i].hydropathy1,
              XTA[i].chID1,
              XTA[i].resName1,
              XTA[i].insCode1,
              XTA[i].sseType1,
              XTA[i].seqNum1
              ),
            boost::python::object()
            );
          break;
        }

        l.append( boost::python::make_tuple( equivs, info ) );
      }

      return l;
    }
};

struct MultAlignResidueData
{
  public:
    bool aligned;
    boost::python::str chain_id, resname, inscode;
    int sse_type, resseq;

    MultAlignResidueData(
      const ChainID& chain_id_,
      const ResName& resname_,
      const InsCode& inscode_,
      int sse_type_,
      int resseq_,
      bool aligned_
      )
      : chain_id( chain_id_[0] == '\0' ? "" : boost::python::str( chain_id_, 1 ) ),
        resname( resname_[0] == '\0' ? "" : boost::python::str( resname_, 3 ) ),
        inscode( inscode_[0] == '\0' ? "" : boost::python::str( inscode_, 1 ) ),
        sse_type( sse_type_ ),
        resseq( resseq_ ),
        aligned( aligned_ )
    {}

    ~MultAlignResidueData() {}
};

class MultipleAlignment
{
public:
  typedef boost::scoped_ptr< Graph > graph_ptr_type;

private:
  int rc_;
  boost::python::list result_;
  boost::python::list matrices_;

  int n_align_;
  int n_sses_;
  mmdb::realtype rmsd_;
  mmdb::realtype q_score_;

public:
  MultipleAlignment(
    boost::python::object managers,
    boost::python::object selstrings
    )
    : rc_( MALIGN_NoAlignment ), n_align_( 0 ), n_sses_( 0 ), rmsd_( 0 ),
      q_score_( 0 )
  {
    using namespace boost::python;
    using namespace boost;

    std::size_t size = extract< std::size_t >( managers.attr( "__len__" )() );
    assert ( size == extract< std::size_t >( selstrings.attr( "__len__" )() ) );

    scoped_array< PManager > p_managers( new PManager[ size ] );
    scoped_array< int > p_handles( new int[ size ] );
    scoped_array< pstr > p_strs( new pstr[ size ] );
    scoped_array< graph_ptr_type > p_graphs( new graph_ptr_type[ size ] );
    scoped_array< PGraph > p_raw_pgraphs( new PGraph[ size ] );

    for ( std::size_t i = 0; i < size; ++i )
    {
      p_managers[ i ] = extract< Manager* >( managers[ i ] );
      p_strs[ i ] = extract< char* >( selstrings[ i ] );

      p_handles[ i ] = p_managers[ i ]->NewSelection();
      p_managers[ i ]->Select( p_handles[ i ], STYPE_ATOM, p_strs[ i ], SKEY_NEW );
    }

    rc_ = MALIGN_Ok;

    for ( std::size_t i = 0; i < size; ++i )
    {
      graph_ptr_type graph_ptr( GetSSGraph( p_managers[ i ], p_handles[ i ], rc_ ) );

      if ( rc_ != MALIGN_Ok )
      {
        break;
      }

      p_graphs[ i ].swap( graph_ptr );
      p_raw_pgraphs[ i ] = p_graphs[ i ].get();
    }

    MultAlign malign;

    if ( rc_ == MALIGN_Ok )
    {
      rc_ = malign.align( p_managers.get(), p_strs.get(), p_raw_pgraphs.get(), size );
    }

    if ( rc_ == MALIGN_Ok )
    {
      get_ma_output( malign );
      get_ss_output( malign, size );
      get_scores( malign );
    }

    for ( std::size_t i = 0; i < size; ++i )
    {
      p_managers[ i ]->DeleteSelection( p_handles[ i ] );
    }
  }

  ~MultipleAlignment()
  {}

public:
  int get_return_code() const
  {
    return rc_;
  }

  boost::python::list get_alignment() const
  {
    return result_;
  }

  boost::python::list get_matrices() const
  {
    return matrices_;
  }

  int get_n_align() const
  {
    return n_align_;
  }

  int get_n_sses() const
  {
    return n_sses_;
  }

  mmdb::realtype get_rmsd() const
  {
    return rmsd_;
  }

  mmdb::realtype get_q_score() const
  {
    return q_score_;
  }

private:
  void get_ma_output(MultAlign& malign)
  {
    PPMAOutput MAOut = NULL;
    int nrows = 0;
    int ncols = 0;

    malign.GetMAOutput ( MAOut, nrows, ncols );

    for ( int i = 0; i < nrows; ++i )
    {
      boost::python::list row;

      for ( int j = 0; j < ncols; ++j )
      {
        MAOutput const& ma = MAOut[i][j];

        row.append(
          MultAlignResidueData(
            ma.chID,
            ma.name,
            ma.insCode,
            ma.sseType,
            ma.seqNum,
            ma.aligned
            )
          );
      }

      result_.append( row );
    }

    FreeMSOutput ( MAOut, nrows );
  }

  void get_ss_output(MultAlign& malign, int size)
  {
    mmdb::mat44 T;

    for ( int i=0; i< size; ++i )
    {
      malign.getTMatrix( T, i );
      matrices_.append(
        boost::python::make_tuple(
          T[0][0],T[0][1],T[0][2],T[0][3],
          T[1][0],T[1][1],T[1][2],T[1][3],
          T[2][0],T[2][1],T[2][2],T[2][3]
          )
        );
    }
  }

  void get_scores(MultAlign& malign)
  {
    malign.getAlignScores( n_align_, n_sses_, rmsd_, q_score_ );
  }
};

struct Manager_wrappers
{
  typedef Manager wt;

  static
  boost::python::object
  GetSymOp_wrapper(
    wt& O,
    int Nop)
  {
    cpstr s = O.GetSymOp(Nop);
    if (s == 0) return boost::python::object();
    return boost::python::str(s);
  }

  static void
  wrap()
  {
    using namespace boost::python;
    class_<wt>( "Manager", init<>() )
      .def( "SetFlag", &wt::SetFlag, ( arg( "flag" ) ) )
      .def("ReadPDBASCII",
        (ERROR_CODE (wt::*)(cpstr, io::GZ_MODE)) &wt::ReadPDBASCII, (
          arg( "fileName" ), arg( "gzipMode" )))
      .def( "PutPDBString",
        (ERROR_CODE (wt::*)(cpstr)) &wt::PutPDBString,
         ( arg( "pdbString" )))
      .def( "WritePDBASCII",
        (ERROR_CODE (wt::*)(cpstr, io::GZ_MODE)) &wt::WritePDBASCII, (
          arg( "fileName" ), arg( "gzipMode" )))
      .def( "NewSelection", &wt::NewSelection )
      .def("Select",
        (int (wt::*)(int, SELECTION_TYPE, cpstr, SELECTION_KEY)) &wt::Select, (
          arg( "selHnd" ), arg( "selType" ), arg( "cid" ), arg( "selKey")))
      .def( "GetSelLength", &wt::GetSelLength, ( arg( "selHnd" ) ) )
      .def( "isSpaceGroup", &wt::isSpaceGroup )
      .def( "GetNumberOfSymOps", &wt::GetNumberOfSymOps )
      .def( "GetSymOp", GetSymOp_wrapper, ( arg( "Nop" ) ) )
    ;
  }
};

void
init_module()
{
  using namespace boost::python;

  object package = scope();
  package.attr( "__path__" ) = "ccp4io_adaptbx";

  object mmdb_module((
    handle<>( borrowed( PyImport_AddModule( "ccp4io_adaptbx.mmdb" ) ) )
    ));
  object ssm_module((
    handle<>( borrowed( PyImport_AddModule( "ccp4io_adaptbx.ssm" ) ) )
    ));
  scope().attr( "mmdb" ) = mmdb_module;
  scope().attr( "ssm" ) = ssm_module;
  scope mmdb_scope = mmdb_module;
  enum_<SELECTION_TYPE>("SELECTION_TYPE")
    .value("INVALID", STYPE_INVALID)
    .value("UNDEFINED", STYPE_UNDEFINED)
    .value("ATOM", STYPE_ATOM)
    .value("RESIDUE", STYPE_RESIDUE)
    .value("CHAIN", STYPE_CHAIN)
    .value("MODEL", STYPE_MODEL);
  enum_<SELECTION_KEY>("SELECTION_KEY")
    .value("NEW", SKEY_NEW)
    .value("OR", SKEY_OR)
    .value("AND", SKEY_AND)
    .value("XOR", SKEY_XOR)
    .value("CLR", SKEY_CLR)
    .value("XAND", SKEY_XAND);
  enum_<ERROR_CODE>("ERROR_CODE")
    .value("EmptyCIF",Error_EmptyCIF)
    .value("NoError",Error_NoError)
    .value("Ok",Error_Ok);
  enum_<MMDB_READ_FLAG>("MMDB_READ_FLAG")
    .value("PrintCIFWarnings", MMDBF_PrintCIFWarnings )
    .value("IgnoreDuplSeqNum", MMDBF_IgnoreDuplSeqNum)
    .value("IgnoreNonCoorPDBErrors", MMDBF_IgnoreNonCoorPDBErrors);
  enum_<io::GZ_MODE>("IO_GZ_MODE")
    .value("NONE", io::GZM_NONE)
    .value("CHECK", io::GZM_CHECK)
    .value("ENFORCE", io::GZM_ENFORCE)
    .value("ENFORCE_GZIP", io::GZM_ENFORCE_GZIP)
    .value("ENFORCE_COMPRESS", io::GZM_ENFORCE_COMPRESS);
  enum_<io::FILE_ERROR>("IO_FILE_ERROR")
    .value("NoMemory", io::FileError_NoMemory)
    .value("ShortData", io::FileError_ShortData)
    .value("_NoDataFound", io::FileError_NoDataFound)
    .value("NoColumn", io::FileError_NoColumn)
    .value("BadData", io::FileError_BadData)
    .value("WrongMemoryAllocation", io::FileError_WrongMemoryAllocation);
  def( "GetErrorDescription", &GetErrorDescription );

  InitMatType();
  Manager_wrappers::wrap();

  scope ssm_scope = ssm_module;

  enum_<SUPERPOSITION_RESULT>("SUPERPOSITION_RESULT")
    .value("Ok", SPOSE_Ok)
    .value("BadData", SPOSE_BadData)
    .value("NoCalphas1", SPOSE_NoCalphas1)
    .value("NoCalphas2", SPOSE_NoCalphas2)
    .value("RemoteStruct", SPOSE_RemoteStruct)
    .value("SVDFail", SPOSE_SVDFail);
  enum_<RETURN_CODE>("RETURN_CODE")
    .value("Ok", RC_Ok)
    .value("NoHits", RC_NoHits)
    .value("NoSuperposition", RC_NoSuperposition)
    .value("NoGraph", RC_NoGraph)
    .value("NoVertices", RC_NoVertices)
    .value("NoGraph2", RC_NoGraph2)
    .value("NoVertices2", RC_NoVertices2)
    .value("TooFewMatches", RC_TooFewMatches);
  enum_<PRECISION>("PRECISION")
    .value("Highest", PREC_Highest)
    .value("High", PREC_High)
    .value("Normal", PREC_Normal)
    .value("Low", PREC_Low)
    .value("Lowest", PREC_Lowest);
  enum_<CONNECTIVITY>("CONNECTIVITY")
    .value("Flexible", CONNECT_Flexible)
    .value("None_", CONNECT_None)
    .value("Strict", CONNECT_Strict);
  enum_<VERTEX_TYPE>("VERTEX_TYPE")
    .value("UNKNOWN", V_UNKNOWN)
    .value("HELIX", V_HELIX)
    .value("STRAND", V_STRAND);
  enum_<MALIGN_RC>("MALIGN")
    .value("Ok", MALIGN_Ok)
    .value("BadInput", MALIGN_BadInput)
    .value("NoStructure", MALIGN_NoStructure)
    .value("NoAlignment", MALIGN_NoAlignment)
    .value("NoGraph", MALIGN_NoGraph);

  InitGraph();
  class_< PySSMAlign >( "SSMAlign", init<>() )
    .def(
      "Align",
      &PySSMAlign::align,
      ( arg( "manager1" ), arg( "manager2" ), arg( "precision" ),
        arg( "connectivity" ), arg( "selHnd1" ), arg( "selHnd2" ) )
      )
    .def( "GetQvalues", &PySSMAlign::get_q_values )
    .def(
      "AlignSelectedMatch",
      &PySSMAlign::AlignSelectedMatch,
      ( arg( "manager1" ), arg( "manager2" ), arg( "precision" ),
        arg( "connectivity" ), arg( "selHnd1" ), arg( "selHnd2" ), arg( "nselected" ) )
      )
    .def_readonly( "rmsd", &PySSMAlign::rmsd )
    .def_readonly( "n_align", &PySSMAlign::nalgn )
    .add_property( "t_matrix", &PySSMAlign::get_t_matrix )
    ;

  class_< ResidueData >( "ResidueData", no_init )
    .def_readonly( "hydropathy", &ResidueData::hydropathy )
    .def_readonly( "chain_id", &ResidueData::chain_id )
    .def_readonly( "resname", &ResidueData::resname )
    .def_readonly( "inscode", &ResidueData::inscode )
    .def_readonly( "sse_type", &ResidueData::sse_type )
    .def_readonly( "resseq", &ResidueData::resseq )
    ;

  class_< PyXAlignText >( "XAlignText", init<>() )
    .def( "XAlign", &PyXAlignText::py_x_align,
      ( arg( "manager1" ), arg( "manager2" ), arg( "ssm_align" ) )
      )
    .def( "get_blocks", &PyXAlignText::get_blocks )
    ;

  class_< MultAlignResidueData >( "MultAlignResidueData", no_init )
    .def_readonly( "chain_id", &MultAlignResidueData::chain_id )
    .def_readonly( "resname", &MultAlignResidueData::resname )
    .def_readonly( "inscode", &MultAlignResidueData::inscode )
    .def_readonly( "sse_type", &MultAlignResidueData::sse_type )
    .def_readonly( "resseq", &MultAlignResidueData::resseq )
    .def_readonly( "aligned", &MultAlignResidueData::aligned )
    ;

  class_< MultipleAlignment >( "MultipleAlignment", no_init )
    .def( init< object, object >( ( arg( "managers" ), arg( "selstrings" ) ) ) )
    .def( "get_return_code", &MultipleAlignment::get_return_code )
    .def( "get_alignment", &MultipleAlignment::get_alignment )
    .def( "get_matrices", &MultipleAlignment::get_matrices )
    .def( "get_n_align", &MultipleAlignment::get_n_align )
    .def( "get_n_sses", &MultipleAlignment::get_n_sses )
    .def( "get_rmsd", &MultipleAlignment::get_rmsd )
    .def( "get_q_score", &MultipleAlignment::get_q_score )
    ;
}

}}  // namespace ccp4io_adaptbx::boost_python

BOOST_PYTHON_MODULE( ccp4io_adaptbx_ext )
{
  ccp4io_adaptbx::boost_python::init_module();
}


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

#include "precomp.hpp"

#if CERES_FOUND

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/sfm.hpp>

#include <iostream>

using namespace cv;
using namespace cv::sfm;
using namespace std;

namespace cv
{
namespace sfm
{

  template<class T>
  void
  reconstruct_(const T &input, OutputArray Rs, OutputArray Ts, InputOutputArray K, OutputArray points3d, const bool refinement=true)
  {
    // Initial reconstruction
    const int keyframe1 = 1, keyframe2 = 2;
    const int select_keyframes = 1; // enable automatic keyframes selection
    const int verbosity_level = -1; // mute libmv logs

    // Refinement parameters
    const int refine_intrinsics = ( !refinement ) ? 0 :
        SFM_REFINE_FOCAL_LENGTH | SFM_REFINE_PRINCIPAL_POINT | SFM_REFINE_RADIAL_DISTORTION_K1 | SFM_REFINE_RADIAL_DISTORTION_K2;

    // Camera data
    Matx33d Ka = K.getMat();
    const double focal_length = Ka(0,0);
    const double principal_x = Ka(0,2), principal_y = Ka(1,2), k1 = 0, k2 = 0, k3 = 0;

    // Set reconstruction options
    libmv_ReconstructionOptions reconstruction_options(keyframe1, keyframe2, refine_intrinsics, select_keyframes, verbosity_level);

    libmv_CameraIntrinsicsOptions camera_instrinsic_options =
      libmv_CameraIntrinsicsOptions(SFM_DISTORTION_MODEL_POLYNOMIAL,
                                    focal_length, principal_x, principal_y,
                                    k1, k2, k3);

    //-- Instantiate reconstruction pipeline
    Ptr<BaseSFM> reconstruction =
      SFMLibmvEuclideanReconstruction::create(camera_instrinsic_options, reconstruction_options);

    //-- Run reconstruction pipeline
    reconstruction->run(input, K, Rs, Ts, points3d);

  }

  //  Reconstruction function for API
  void
  reconstruct(InputArrayOfArrays points2d, OutputArray Ps, OutputArray points3d, InputOutputArray K,
              bool is_projective)
  {
    const int nviews = points2d.total();
    CV_Assert( nviews >= 2 );

    // OpenCV data types
    std::vector<Mat> pts2d;
    points2d.getMatVector(pts2d);
    const int depth = pts2d[0].depth();

    Matx33d Ka = K.getMat();

    // Projective reconstruction

    if (is_projective)
    {

      if ( nviews == 2 )
      {
        // Get Projection matrices
        Matx33d F;
        Matx34d P, Pp;

        normalizedEightPointSolver(pts2d[0], pts2d[1], F);
        projectionsFromFundamental(F, P, Pp);
        Ps.create(2, 1, depth);
        Mat(P).copyTo(Ps.getMatRef(0));
        Mat(Pp).copyTo(Ps.getMatRef(1));

        // Triangulate and find 3D points using inliers
        triangulatePoints(points2d, Ps, points3d);
      }
      else
      {
        std::vector<Mat> Rs, Ts;
        reconstruct(points2d, Rs, Ts, Ka, points3d, is_projective);

        // From Rs and Ts, extract Ps
        const int nviews = Rs.size();
        Ps.create(nviews, 1, depth);

        Matx34d P;
        for (size_t i = 0; i < nviews; ++i)
        {
          projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P);
          Mat(P).copyTo(Ps.getMatRef(i));
        }

        Mat(Ka).copyTo(K.getMat());
      }

    }

    // Affine reconstruction

    else
    {
      // TODO: implement me
    }

  }

  void
  reconstruct(InputArrayOfArrays points2d, OutputArray Rs, OutputArray Ts, InputOutputArray K,
              OutputArray points3d, bool is_projective)
  {
    const int nviews = points2d.total();
    CV_Assert( nviews >= 2 );

    // Projective reconstruction

    if (is_projective)
    {

      // calls simple pipeline
      reconstruct_(points2d, Rs, Ts, K, points3d);

    }

    // Affine reconstruction

    else
    {
      // TODO: implement me
    }

  }

  void
  reconstruct(const std::vector<std::string> images, OutputArray Ps, OutputArray points3d,
              InputOutputArray K, bool is_projective)
  {
    const int nviews = static_cast<int>(images.size());
    CV_Assert( nviews >= 2 );

    Matx33d Ka = K.getMat();
    const int depth = Mat(Ka).depth();

    // Projective reconstruction

    if ( is_projective )
    {
      std::vector<Mat> Rs, Ts;
      reconstruct(images, Rs, Ts, Ka, points3d, is_projective);

      // From Rs and Ts, extract Ps

      const int nviews_est = Rs.size();
      Ps.create(nviews_est, 1, depth);

      Matx34d P;
      for (size_t i = 0; i < nviews_est; ++i)
      {
        projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P);
        Mat(P).copyTo(Ps.getMatRef(i));
      }

      Mat(Ka).copyTo(K.getMat());
      }

    // Affine reconstruction

    else
    {
      // TODO: implement me
    }

  }

  void
  reconstruct(const std::vector<std::string> images, OutputArray Rs, OutputArray Ts,
              InputOutputArray K, OutputArray points3d, bool is_projective)
  {
    const int nviews = static_cast<int>(images.size());
    CV_Assert( nviews >= 2 );

    // Projective reconstruction

    if ( is_projective )
    {
      reconstruct_(images, Rs, Ts, K, points3d, false);
    }

    // Affine reconstruction

    else
    {
      // TODO: implement me
    }

  }

} // namespace sfm
} // namespace cv

#endif /* HAVE_CERES */


#include "com_classifier.h"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <stdexcept>
#include <iostream>
#include <string>
#include <vector>
#include <boost/math/special_functions/fpclassify.hpp>
#include "pcrtypes.h"
#include "com_classifierimp.h"
#include "com_linclassifier.h"
#include "com_logclassifier.h"
#include "com_tlogclassifier.h"
#include "com_userdefinedclassifier.h"

//------------------------------------------------------------------------------
// DEFINITION OF STATIC CLASS MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF CLASS MEMBERS
//------------------------------------------------------------------------------

/*!
  \warning Doesn't set a default classification algorithm. Set classification
           parameters before calling classify() or classify(size_t)!
  \sa      com::Classifier(const com::Classifier &),
           com::Classifier(REAL8, REAL8)

  The default classification mode is AUTO.
*/
com::Classifier::Classifier()
{
  try
  {
    init();
  }
  catch(...)
  {
    clean();
    throw;
  }
}

/*!
  \param   rhs Object to copy values from.
  \sa      com::Classifier(const com::Classifier &)

  Performs a deep copy of the classification algorithm.
*/
com::Classifier &com::Classifier::operator=(const Classifier &rhs)
{
  if(this != &rhs)
  {
    clean();

    // Set d_classifier and d_algorithm.
    if(rhs.d_algorithm == LIN) {
      com_LinClassifier<REAL8> *c =
                     dynamic_cast<com_LinClassifier<REAL8> *>(rhs.d_classifier);
      assert(c);
      d_classifier = new com_LinClassifier<REAL8>(*c);
      d_algorithm  = LIN;
    }
    else if(rhs.d_algorithm == LOG) {
      com_LogClassifier<REAL8> *c =
                     dynamic_cast<com_LogClassifier<REAL8> *>(rhs.d_classifier);
      assert(c);
      d_classifier = new com_LogClassifier<REAL8>(*c);
      d_algorithm  = LOG;
    }
    else if(rhs.d_algorithm == TLOG) {
      com_TLogClassifier<REAL8> *c =
                    dynamic_cast<com_TLogClassifier<REAL8> *>(rhs.d_classifier);
      assert(c);
      d_classifier = new com_TLogClassifier<REAL8>(*c);
      d_algorithm  = TLOG;
    }
    else if(rhs.d_algorithm == USERDEFINED) {
      UserDefinedClassifier<REAL8> *c =
                   dynamic_cast<UserDefinedClassifier<REAL8> *>(rhs.d_classifier);
      assert(c);
      d_classifier = new UserDefinedClassifier<REAL8>(*c);
      d_algorithm  = USERDEFINED;
    }

    d_mode      = rhs.d_mode;
    d_borders   = rhs.d_borders;

    if(rhs.extremesAreValid()) {
      d_min = rhs.d_min;
      d_max = rhs.d_max;
    }
    else {
      pcr::setMV(d_min);
      pcr::setMV(d_max);
    }

    if(rhs.cutoffsAreValid()) {
      d_minCutoff = rhs.d_minCutoff;
      d_maxCutoff = rhs.d_maxCutoff;
    }
    else {
      pcr::setMV(d_minCutoff);
      pcr::setMV(d_maxCutoff);
    }

    d_nrClasses = rhs.d_nrClasses;
  }

  return *this;
}

/*!
  \param   rhs Object to copy values from.
  \sa      operator=(const com::Classifier &), com::Classifier(),
           com::Classifier(REAL8, REAL8)

  Performs a deep copy of the classification algorithm.
*/
com::Classifier::Classifier(const Classifier &rhs)
{
  init();

  // Set d_classifier and d_algorithm.
  if(rhs.d_algorithm == LIN) {
    com_LinClassifier<REAL8> *c =
         dynamic_cast<com_LinClassifier<REAL8> *>(rhs.d_classifier);
    assert(c);
    d_classifier = new com_LinClassifier<REAL8>(*c);
    d_algorithm  = LIN;
  }
  else if(rhs.d_algorithm == LOG) {
    com_LogClassifier<REAL8> *c =
         dynamic_cast<com_LogClassifier<REAL8> *>(rhs.d_classifier);
    assert(c);
    d_classifier = new com_LogClassifier<REAL8>(*c);
    d_algorithm  = LOG;
  }
  else if(rhs.d_algorithm == TLOG) {
    com_TLogClassifier<REAL8> *c =
         dynamic_cast<com_TLogClassifier<REAL8> *>(rhs.d_classifier);
    assert(c);
    d_classifier = new com_TLogClassifier<REAL8>(*c);
    d_algorithm  = TLOG;
  }
  else if(rhs.d_algorithm == USERDEFINED) {
    UserDefinedClassifier<REAL8> *c =
         dynamic_cast<UserDefinedClassifier<REAL8> *>(rhs.d_classifier);
    assert(c);
    d_classifier = new UserDefinedClassifier<REAL8>(*c);
    d_algorithm  = USERDEFINED;
  }

  d_mode      = rhs.d_mode;
  d_borders   = rhs.d_borders;

  if(rhs.extremesAreValid()) {
    d_min = rhs.d_min;
    d_max = rhs.d_max;
  }
  else {
    pcr::setMV(d_min);
    pcr::setMV(d_max);
  }

  if(rhs.cutoffsAreValid()) {
    d_minCutoff = rhs.d_minCutoff;
    d_maxCutoff = rhs.d_maxCutoff;
  }
  else {
    pcr::setMV(d_minCutoff);
    pcr::setMV(d_maxCutoff);
  }

  d_nrClasses = rhs.d_nrClasses;
}

/*!
  \param   min Minimum data value.
  \param   max Maximum data value.
  \sa      com::Classifier(), com::Classifier(const Classifier &)

  The extremes and the cutoff values are set to \a min and \a max. If \a max
  is smaller than \a min, than they're swapped.
*/
com::Classifier::Classifier(REAL8 min, REAL8 max)
{
  try
  {
    init();
    d_min = min;
    d_max = max;

    if(d_min > d_max)
      std::swap(d_min, d_max);

    d_minCutoff = d_min;
    d_maxCutoff = d_max;
  }
  catch(...)
  {
    clean();
    throw;
  }
}

/*!
  Calls clean().
*/
com::Classifier::~Classifier()
{
  clean();
}

/*!
  After calling this function you can't call the classify() or classify(size_t)
  functions anymore. All members are set to an initial state (empty, invalid,
  0, etc).
*/
void com::Classifier::init()
{
  d_classifier = 0;
  d_borders.erase(d_borders.begin(), d_borders.end());
  d_algorithm  = INVALID_ALGORITHM;
  d_mode       = AUTO;
  pcr::setMV(d_min);
  pcr::setMV(d_max);
  pcr::setMV(d_minCutoff);
  pcr::setMV(d_maxCutoff);
  d_nrClasses  = 0;
}

/*!
  \warning  If you saved a copy of the pointer to that algorithm you're better
            off not using it anymore!

  Deletes the classification algorithm from memory.
*/
void com::Classifier::clean()
{
  delete d_classifier; d_classifier = 0;
}

//!
/*!
  \param     .
  \return    .
  \exception .
  \warning   .
  \sa        .

  No need to check the classifier, this is determined by d_algorithm.
*/
bool com::Classifier::equals(Classifier const& rhs) const
{
  bool extremesAreEqual = false;

  if(!extremesAreValid()) {
    if(!rhs.extremesAreValid()) {
      // Both objects have invalid/unset extremes.
      extremesAreEqual = true;
    }
  }
  else {
    if(rhs.extremesAreValid()) {
      if(d_min == rhs.d_min && d_max == rhs.d_max) {
        // Both objects have valid and equal extremes.
        extremesAreEqual = true;
      }
    }
  }

  bool cutoffsAreEqual = false;

  if(!cutoffsAreValid()) {
    if(!rhs.cutoffsAreValid()) {
      // Both objects have invalid/unset cutoffs.
      cutoffsAreEqual = true;
    }
  }
  else {
    if(rhs.cutoffsAreValid()) {
      if(d_min == rhs.d_min && d_max == rhs.d_max) {
        // Both objects have valid and equal cutoffs.
        cutoffsAreEqual = true;
      }
    }
  }

  return d_algorithm == rhs.d_algorithm &&
         d_mode == rhs.d_mode &&
         d_borders == rhs.d_borders &&
         extremesAreEqual &&
         cutoffsAreEqual &&
         d_nrClasses == rhs.d_nrClasses;
}

/*!
  \warning Don't forget to set the classification parameters before calling
           this function!
  \sa      setExtremes(), setMinCutoff(), setMaxCutoff(), setCutoffs(),
           setNrClasses()

  The currently set values for minimum cutoff, maximum cutoff and number of
  classes are passed to the classification algorithm.
*/
void com::Classifier::classify()
{
  assert(d_classifier);

  if(!cutoffsAreValid() || d_minCutoff == d_maxCutoff) {
    d_borders.resize(0);
  }
  else {
    assert(d_min <= d_max);
    assert(d_minCutoff <= d_maxCutoff);
    assert(!pcr::isMV(d_min));
    assert(!pcr::isMV(d_max));
    assert(!pcr::isMV(d_minCutoff));
    assert(!pcr::isMV(d_maxCutoff));

    if(d_mode == AUTO) {
      d_classifier->autoClassify(d_borders, d_minCutoff, d_maxCutoff,
         d_nrClasses);
    }
    else if(d_mode == EXACT) {
      d_classifier->classify(d_borders, d_minCutoff, d_maxCutoff, d_nrClasses);
    }
#ifdef DEBUG_DEVELOP
    else {
      throw std::logic_error(std::string("com::Classifier::classify()"));
    }
#endif
  }

  if(!d_borders.empty()) {
    d_minCutoff = d_borders.front();
    d_maxCutoff = d_borders.back();
    d_nrClasses = d_borders.size() - 1;
  }
}

/*!
  \overload
  \param   n Number of class borders to calculate.
*/
void com::Classifier::classify(size_t n)
{
#ifdef DEBUG_DEVELOP
  assert(d_classifier);
#endif

  d_nrClasses = n;
  classify();
}

/*!
  \param   a Algorithm to install.
  \sa      installLin(), installLog(), installTLog()
*/
void com::Classifier::installAlgorithm(Algorithm a)
{
  if(a == LIN)
    (void)installLin();
  else if(a == LOG)
    (void)installLog();
  else if(a == TLOG)
    (void)installTLog();
  else if(a == USERDEFINED)
    (void)installUserDefined();
}

/*!
  \return  Classification object created.
  \warning The pointer returned by this function is for configuring the
           algorithm specific parameters. If the com::Classifier object dies,
           this pointer is not valid anymore and should not be dereferenced!
  \sa      com_LinClassifier, installLog(), installTLog()
*/
com_LinClassifier<REAL8> *com::Classifier::installLin()
{
  delete d_classifier, d_classifier = 0;
  d_borders.erase(d_borders.begin(), d_borders.end());
  d_algorithm  = INVALID_ALGORITHM;

  com_LinClassifier<REAL8> *c = 0;

  try
  {
    c = new com_LinClassifier<REAL8>();
    d_classifier = c;
    d_algorithm = LIN;
  }
  catch(...)
  {
    delete c; c = 0;
    clean();
    throw;
  }

  return c;
}

/*!
  \return  Classification object created.
  \warning The pointer returned by this function is for configuring the
           algorithm specific parameters. If the com::Classifier object dies,
           this pointer is not valid anymore and should not be dereferenced!
  \sa      com_LogClassifier, installLin(), installTLog()
*/
com_LogClassifier<REAL8> *com::Classifier::installLog()
{
  delete d_classifier, d_classifier = 0;
  d_borders.erase(d_borders.begin(), d_borders.end());
  d_algorithm  = INVALID_ALGORITHM;

  com_LogClassifier<REAL8> *c = 0;

  try
  {
    c = new com_LogClassifier<REAL8>();
    d_classifier = c;
    d_algorithm  = LOG;
  }
  catch(...)
  {
    delete c; c = 0;
    clean();
    throw;
  }

  return c;
}

/*!
  \return  Classification object created.
  \warning The pointer returned by this function is for configuring the
           algorithm specific parameters. If the com::Classifier object dies,
           this pointer is not valid anymore and should not be dereferenced!
  \sa      com_TLogClassifier, installLin(), installLog()
*/
com_TLogClassifier<REAL8> *com::Classifier::installTLog()
{
  delete d_classifier, d_classifier = 0;
  d_borders.erase(d_borders.begin(), d_borders.end());
  d_algorithm  = INVALID_ALGORITHM;

  com_TLogClassifier<REAL8> *c = 0;

  try
  {
    c = new com_TLogClassifier<REAL8>();
    d_classifier = c;
    d_algorithm  = TLOG;
  }
  catch(...)
  {
    delete c; c = 0;
    clean();
    throw;
  }

  return c;
}

com::UserDefinedClassifier<REAL8> *com::Classifier::installUserDefined()
{
  delete d_classifier, d_classifier = 0;
  d_borders.erase(d_borders.begin(), d_borders.end());
  d_algorithm  = INVALID_ALGORITHM;

  UserDefinedClassifier<REAL8>* classifier = 0;

  try {
    classifier = new UserDefinedClassifier<REAL8>();
    d_classifier = classifier;
    d_algorithm  = USERDEFINED;
  }
  catch(...) {
    delete classifier; classifier = 0;
    clean();
    throw;
  }

  return classifier;
}

/*!
  \return  The number of classes calculated.
  \warning The value returned by this function is need not have the same value
           as set with the setNrClasses() or classify(size_t) member functions.
           The requested number of classes does not need to be the same as the
           actually calculated number.
  \sa      setNrClasses()

  If classify() or classify(size_t) hasn't been called yet, this function will
  return 0.

  The number of classes is equal to the calculated number of classborders
  minus 1.

  This function returns 0 if no class borders are calculated.
*/
size_t com::Classifier::nrClasses() const
{
  return d_borders.empty() ? 0 : d_borders.size() - 1;
}

size_t com::Classifier::nrBorders() const
{
  return d_borders.size();
}

size_t com::Classifier::nrClassesRequested() const
{
  return d_nrClasses;
}

com::Classifier::const_iterator com::Classifier::begin() const
{
  return d_borders.begin();
}

com::Classifier::const_iterator com::Classifier::end() const
{
  return d_borders.end();
}

com::Classifier::Algorithm com::Classifier::algorithm() const
{
  return d_algorithm;
}

const std::vector<REAL8> &com::Classifier::borders() const
{
  return d_borders;
}

/*!
  \sa      max(), setExtremes()
*/
REAL8 com::Classifier::min() const
{
  assert(!pcr::isMV(d_min));
  return d_min;
}

/*!
  \sa      min(), setExtremes()
*/
REAL8 com::Classifier::max() const
{
  assert(!pcr::isMV(d_max));
  return d_max;
}

/*!
  \sa      maxCutoff(), setCutoffs(), setMinCutoff(), setMaxCutoff()
*/
REAL8 com::Classifier::minCutoff() const
{
  assert(!pcr::isMV(d_minCutoff));
  return d_minCutoff;
}

/*!
  \sa      minCutoff(), setCutoffs(), setMinCutoff(), setMaxCutoff()
*/
REAL8 com::Classifier::maxCutoff() const
{
  assert(!pcr::isMV(d_maxCutoff));
  return d_maxCutoff;
}

/*!
  \param   min New minimum value.
  \param   max New minimum value.
  \sa      min(), max(), setCutoffs(), setMinCutoff(), setMaxCutoff()

  If \a min > \a max they're swapped.
*/
void com::Classifier::setExtremes(REAL8 min, REAL8 max)
{
  d_min = min;
  d_max = max;

  if(d_min > d_max)
    std::swap(d_min, d_max);
}

/*!
  \param   min New minimum cutoff value.
  \param   min New maximum cutoff value.
  \sa      setMinCutoff(), setMaxCutOff(), resetCutoffs(), minCutoff(),
           maxCutoff()

  If \a min > \a max they're swapped.
*/
void com::Classifier::setCutoffs(REAL8 min, REAL8 max)
{
  d_minCutoff = min;
  d_maxCutoff = max;

  if(d_minCutoff > d_maxCutoff)
    std::swap(d_minCutoff, d_maxCutoff);
}

/*!
  \sa      setCutoffs(), setMinCutoff(), setMaxCutoff(), minCutoff(),
           maxCutoff()
*/
void com::Classifier::resetCutoffs()
{
  d_minCutoff = d_min;
  d_maxCutoff = d_max;
}

void com::Classifier::resetMinCutoff()
{
  d_minCutoff = d_min;
}

void com::Classifier::resetMaxCutoff()
{
  d_maxCutoff = d_max;
}

/*!
  \param   v Value to classify.
  \return  Class index of value \a v.
  \sa      classBorder()
  \warning The result is undefined if the number classes == 0.

  The class index returned is the index of the class whose border is greater
  of equal to \a v. Values larger than the upper class border are assigned
  to the highest class.

  Returned class indices range from 0 to nr_of_classes - 1.
*/
size_t com::Classifier::classIndex(REAL8 v) const
{
#ifdef DEBUG_DEVELOP
  assert(nrClasses() > 0);
#endif

  const_iterator it = std::upper_bound(begin() + 1, end(), v);
  if(it != end())
    return it - (begin() + 1);
  else
    return nrClasses() - 1;
}

/*!
  \param   i Class index.
  \return  Upper class border.
  \warning The returned value is undefined if \a i >= nrClasses().
  \sa      classIndex()
*/
REAL8 com::Classifier::classBorder(size_t i) const
{
  assert(i < nrBorders());

  return d_borders[i];
}

/*!
  \param   n Number of class borders to calculate.
  \warning Only after classify() of classify(size_t) has been called the value
           set here and the value returned by nrClasses() are in sync.
  \sa      nrClasses()
*/
void com::Classifier::setNrClasses(size_t n)
{
  d_nrClasses = n;
}

/*!
  \param   v New minimum cutoff value.
  \sa      setMaxCutoff(), setCutoffs(), setExtremes(), minCutoff(), maxCutoff()
*/
void com::Classifier::setMinCutoff(REAL8 v)
{
  d_minCutoff = v;
}

/*!
  \param   v New maximum cutoff value.
  \sa      setMinCutoff(), setCutoffs(), setExtremes(), minCutoff(), maxCutoff()
*/
void com::Classifier::setMaxCutoff(REAL8 v)
{
  d_maxCutoff = v;
}

void com::Classifier::setMode(Mode m)
{
  d_mode = m;
}

com::Classifier::Mode com::Classifier::mode() const
{
  return d_mode;
}

//! Merges properties of \a classifier with this classifier.
/*!
  \param     classifier Object to use properties from.
  \exception .
  \warning   Does not call classify().

  The most extreme values of the extreme values of *this and \a classifier
  are set as the extreme values of *this. Other stuff is untouched.
*/
void com::Classifier::merge(Classifier const& classifier)
{
  if(extremesAreValid()) {
    if(classifier.extremesAreValid()) {
      setExtremes(std::min(min(), classifier.min()),
                  std::max(max(), classifier.max()));
    }
  }
  else {
    if(classifier.extremesAreValid()) {
      setExtremes(classifier.min(), classifier.max());
    }
  }
}

bool com::Classifier::extremesAreValid() const
{
  return
    !(pcr::isMV(d_min) || boost::math::isnan(d_min)) &&
    !(pcr::isMV(d_max) || boost::math::isnan(d_max));
}

bool com::Classifier::cutoffsAreValid() const
{
  return
    !(pcr::isMV(d_minCutoff) || boost::math::isnan(d_minCutoff)) &&
    !(pcr::isMV(d_maxCutoff) || boost::math::isnan(d_maxCutoff));
}

bool com::operator==(Classifier const& lhs, Classifier const& rhs)
{
  return lhs.equals(rhs);
}

bool com::operator!=(Classifier const& lhs, Classifier const& rhs)
{
  return !lhs.equals(rhs);
}

//------------------------------------------------------------------------------
// DOCUMENTATION OF ENUMERATIONS
//------------------------------------------------------------------------------

/*!
  \var com::Classifier::Algorithm com::Classifier::INVALID_ALGORITHM
  No classification algorithm set.

  \warning Don't call classify() or classify(size_t) if the algorithm()
           function returns this value.
*/

/*!
  \var com::Classifier::Algorithm com::Classifier::LIN

  Classification performed by com_LinClassifier object.
*/

/*!
  \var com::Classifier::Algorithm com::Classifier::LOG

  Classification performed by com_LogClassifier object.
*/

/*!
  \var com::Classifier::Algorithm com::Classifier::TLOG

  Classification performed by com_TLogClassifier object.
*/

//------------------------------------------------------------------------------
// DOCUMENTATION OF INLINE FUNCTIONS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DOCUMENTATION OF PURE VIRTUAL FUNCTIONS
//------------------------------------------------------------------------------


//
// Created by yalez on 2018/8/21.
//

#define BOOST_TEST_MODULE miner_plugin

#ifndef CELESOS_TEST_MOUDLE_MINER_PLUGIN_ON
#define CELESOS_TEST_MOUDLE_MINER_PLUGIN_ON
#endif //CELESOS_TEST_MOUDLE_MINER_PLUGIN_ON

#include <boost/test/unit_test.hpp>
#include <boost/asio.hpp>
#include <boost/signals2.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <fc/log/logger.hpp>
#include <eosio/testing/tester.hpp>
#include <celesos/pow/ethash.hpp>
#include <celesos/miner_plugin/worker.hpp>
#include <celesos/miner_plugin/miner.hpp>
#include <celesos/miner_plugin/miner_plugin.hpp>
#include <eosio/chain/forest_bank.hpp>

using namespace celesos;
using namespace eosio;

using boost::multiprecision::uint256_t;

BOOST_AUTO_TEST_SUITE(worker_suite)

    BOOST_AUTO_TEST_CASE(worker_test) {
        const static uint32_t HASH_BYTES = 64;
        const static uint64_t CACHE_BYTES = 1024;
        const static uint64_t DATASET_BYTES = 1024 * 32;

        const static auto CACHE_COUNT = static_cast<uint32_t>(CACHE_BYTES / HASH_BYTES);
        const static auto DATASET_COUNT = static_cast<uint32_t>(DATASET_BYTES / HASH_BYTES);

        auto target_ptr = std::make_shared<uint256_t>(197);
        auto &target = *target_ptr;
        target |= uint256_t{90} << 1;
        for (int i = 2; i < 31; ++i) {
            target |= uint256_t{255} << i * 8;
        }

        auto nonce_start_ptr = std::make_shared<uint256_t>();
        miner::miner::gen_random_uint256(*nonce_start_ptr);

        auto retry_count_ptr = std::make_shared<uint256_t>(-1);

        auto io_service_ptr = std::make_shared<boost::asio::io_service>();
        auto signal_ptr = std::make_shared<miner::mine_signal_type>();
        signal_ptr->connect([](auto is_success, auto block_num, auto wood) {
            BOOST_CHECK_EQUAL(is_success, true);
        });

        auto seed_ptr = std::make_shared<std::string>("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
        auto forest_ptr = seed_ptr;

        BOOST_TEST_MESSAGE("begin prepare cache with count: " << CACHE_COUNT);
        auto cache_ptr = std::make_shared<std::vector<ethash::node>>(CACHE_COUNT);
        ethash::calc_cache(*cache_ptr, CACHE_COUNT, *seed_ptr);
        BOOST_TEST_MESSAGE("end prepare cache with count: " << CACHE_COUNT);

        BOOST_TEST_MESSAGE("begin prepare dataset with count: " << DATASET_COUNT);
        auto dataset_ptr = std::make_shared<std::vector<ethash::node>>(DATASET_COUNT);
        ethash::calc_dataset(*dataset_ptr, DATASET_COUNT, *cache_ptr);
        BOOST_TEST_MESSAGE("end prepare dataset with count: " << DATASET_COUNT);

        fc::logger a_logger{};
        miner::worker_ctx ctx{
                .logger = a_logger,
                .dataset_ptr = dataset_ptr,
                .seed_ptr = seed_ptr,
                .forest_ptr = forest_ptr,
                .nonce_start_ptr = nonce_start_ptr,
                .retry_count_ptr = retry_count_ptr,
                .target_ptr = target_ptr,
                .block_num = 1024,
                .io_service_ref = *io_service_ptr,
                .signal_ptr = signal_ptr,
                .sleep_interval_sec = 0,
                .sleep_probability = 0.0f,
        };
        miner::worker worker{std::move(ctx)};
        worker.start();

        BOOST_TEST_MESSAGE("begin solve nonce");
        boost::asio::io_service::work work{*io_service_ptr};
        io_service_ptr->run_one();
        BOOST_TEST_MESSAGE("end solve nonce");
    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(miner_suite)

    BOOST_FIXTURE_TEST_CASE(miner_test, testing::tester) {
        auto &controller_ref = *this->control;

        std::mutex mutex{};
        std::unique_lock<std::mutex> lock{mutex};
        std::condition_variable stop_signal{};
        auto bank = forest::forest_bank::getInstance(controller_ref);

        miner::miner miner{fc::logger::get(), app().get_io_service(), 1, 0, 0.0f};
        miner.connect([&stop_signal, bank](bool is_success, chain::block_num_type block_num,
                                           const boost::optional<uint256_t> &wood_opt) {
            BOOST_CHECK_EQUAL(is_success, true);
            std::string wood_hex{};
            ethash::uint256_to_hex(wood_hex, *wood_opt);
            BOOST_CHECK_EQUAL(bank->verify_wood(block_num, "yale", wood_hex.c_str()), true);
            stop_signal.notify_all();
        });

        BOOST_TEST_MESSAGE("begin solve wood by miner");
        chain::account_name relative_account{"yale"};
        miner.start(std::move(relative_account), controller_ref);
        produce_blocks(1024);
        stop_signal.wait(lock);
        BOOST_TEST_MESSAGE("after wait");
        miner.stop();
        BOOST_TEST_MESSAGE("end solve wood by miner");
    }

BOOST_AUTO_TEST_SUITE_END()

//BOOST_AUTO_TEST_SUITE(miner_plugin_suite)
//
//BOOST_AUTO_TEST_SUITE_END()


#ifndef RINNE_HPP
#define RINNE_HPP

#include <sys/time.h>

#include <string>

#include <boost/thread.hpp>
#include <boost/thread/barrier.hpp>

struct rn_quaternion {
    double w;
    double i, j, k;
};

struct rn_uv {
    double u, v;
};

struct rn_vec {
    double x, y, z;
};

struct rn_pos {
    double theta; // latitude, 0 ... pi
    double phi;   // longitude, 0 ... 2 pi
};

struct rn_node;

struct rn_edge {
    rn_node *src;
    rn_node *dst;
    rn_edge *next;
    rn_edge *bp_next;
};

struct rn_node {
    rn_pos   pos;
    rn_edge *edge;
    rn_edge *bp_edge;
    int      num_edge;
    int      num_bp_edge;
};

class rinne {
public:
    void read_dot(char *path);
    rinne() : m_is_mouse_down(false),
              m_is_fullscreen(false),
              m_rotate_z(0.0),
              m_rotate_x(0.0),
              m_is_blink(1),
              m_is_auto_rotate(1),
              m_max_in_degree(0),
              m_max_out_degree(0),
              m_top_n(50),
              m_top_idx(1),
              m_factor_repulse(0.01),
              m_factor_spring(0.01),
              m_factor_step(1.0),
              m_cycle(30.0),
              m_score(0.0),
              m_score_loop(0)
    {
        timeval tv;
        gettimeofday(&tv, NULL);
        m_init_sec = (double)tv.tv_sec + (double)tv.tv_usec * 0.000001;
        m_prev_sec = m_current_sec = m_init_sec;
    }

    void on_mouse_down(int button, int x, int y);
    void on_mouse_up(int button, int x, int y);
    void on_mouse_move(int x, int y);
    void on_keyboard(unsigned char key, int x, int y);
    void on_resize(int w, int h);
    void on_menu(int id);
    void force_directed(int id);
    void reduce_step() { m_factor_step *= 0.5; }
    void inc_loop() { m_score_loop++; }
    int  get_num_node() { return m_num_node; }
    int  get_num_edge() { return m_num_edge; }
    int  get_window_h() { return m_window_h; }
    int  get_window_w() { return m_window_w; }
    void set_score(double score) { m_score = score; }

    void display();

private:
    int m_num_thread;
    boost::thread  *m_thread;
    boost::barrier *m_barrier;
    rn_pos *m_pos_tmp;

    bool m_is_mouse_down;
    bool m_is_fullscreen;
    int  m_mouse_x;
    int  m_mouse_y;
    int  m_window_w;
    int  m_window_h;

    double m_rotate_z;
    double m_rotate_x;

    int m_is_blink;
    int m_is_auto_rotate;

    int m_num_node;
    int m_num_edge;

    int m_max_in_degree;
    int m_max_out_degree;

    rn_node *m_node;
    rn_edge *m_edge;
    std::string *m_label;

    rn_node **m_node_top;
    int    m_top_n;
    int    m_top_idx;

    double m_factor_repulse;
    double m_factor_spring;
    double m_factor_step;
    double m_init_sec;
    double m_current_sec;
    double m_prev_sec;
    double m_cycle;
    double m_score;
    int    m_score_loop;

    void init_pos();
    void draw_node();
    void draw_edge(double g, double b, double alpha);
    void draw_label();
    void draw_tau();
    void draw_status();
    void get_top_n();
    void update_time();
    void rotate_view();
    void get_uv_vec(rn_vec &v, const rn_pos &a, const rn_pos &b);
    void get_uv_vec_rand(rn_vec &v, const rn_pos &a);
    void get_repulse_vec(rn_vec &uv, double psi);
    void get_spring_vec(rn_vec &uv, double psi);
    void get_color(double &g, double &b, double &alpha,
                   double min_b, double max_b,
                   double min_g, double max_g,
                   double min_alpha, double max_alpha);
};

#endif // RINNE_HPP


#include "pctrackcontainer.h"
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

PCTrackContainer::PCTrackContainer(int _maxFrame)
    :TrackContainer<PCObject>::TrackContainer(_maxFrame)
{
    oldCnt = 0;
    isUpdated = 1;
}

PCTrackContainer::PCTrackContainer()
    :TrackContainer<PCObject>::TrackContainer(1000)
{
    oldCnt = 0;
    isUpdated = 1;
}

void PCTrackContainer::iros2014()
{
    // hand avg : 107, 88, 82
    // black avg : 36, 35, 42
    // white avg : 216, 218, 232
    double avg;
    int kind;
    int cnt[3]; // 0: black, 1: white, 2: hand
    int error=0;
    int total=0;

    double ref[3];
    ref[0] = (36.1661 + 35.5952 + 42.2318) / 3.;
    ref[1] = (216 + 217.127 + 231.618) / 3.;
    ref[2] = (107.212 + 88.0612 + 82.3061) / 3.;
    for(int i=0;i<numTracks();i++){
        avg = 0.;
        cnt[0] = cnt[1] = cnt[2] = 0;
        int id = tracks.at(i)->id;
        if(tracks.at(i)->lastFrame().time == currentT){
            PCObject object;
            object = tracks.at(i)->lastFrame().object;
            total += object.points.size();
            for(int j=0;j<object.points.size();j++){
                int R = object.points.at(j).rgb[0];
                int G = object.points.at(j).rgb[1];
                int B = object.points.at(j).rgb[2];

                double RGB = (R + G + B) / 3.;
                avg += RGB;
                if(fabs(RGB-ref[0]) <= fabs(RGB-ref[1]) && fabs(RGB-ref[0]) <= fabs(RGB-ref[2]))
                    cnt[0] ++;
                else if(fabs(RGB-ref[1]) <= fabs(RGB-ref[0]) && fabs(RGB-ref[1]) <= fabs(RGB-ref[2]))
                    cnt[1] ++;
                else if(fabs(RGB-ref[2]) <= fabs(RGB-ref[1]) && fabs(RGB-ref[2]) <= fabs(RGB-ref[0]))
                    cnt[2] ++;
            }

            avg /= object.points.size();
            if(fabs(avg-ref[0]) <= fabs(avg-ref[1]) && fabs(avg-ref[0]) <= fabs(avg-ref[2])){
                kind = 0;
                error += cnt[1] + cnt[2];
            }
            else if(fabs(avg-ref[1]) <= fabs(avg-ref[0]) && fabs(avg-ref[1]) <= fabs(avg-ref[2])){
                kind = 1;
                error += cnt[0] + cnt[2];
            }
            else if(fabs(avg-ref[2]) <= fabs(avg-ref[1]) && fabs(avg-ref[2]) <= fabs(avg-ref[0])){
                kind = 2;
                error += cnt[1] + cnt[0];
            }
        }
        //        cout<<"ID: "<<id<<" kind: "<< kind<<" total: "<< total<<" error: "<< error<<endl;
    }
    cout<<total<<"  "<<error<<endl;

    //    numTruePoints = 0;
    //    numFalsePoints = 0;
    //    numTotalPoints = 0;

    //    for(int i=0;i<numTracks();i++){
    //        if(tracks.at(i)->lastFrame().time == currentT){
    //            PCObject object;
    //            object = tracks.at(i)->lastFrame().object;
    //            int numBlack = 0;
    //            int numWhite = 0;
    //            for(int j=0;j<object.points.size();j++){
    //                Point point = object.points.at(j);
    //                double avgrgb = (point.rgb[0]*3000 + point.rgb[1]*3000 + point.rgb[2]*3000) / 3;

    //                if(avgrgb > 128) numWhite ++;
    //                else numBlack ++;
    //            }
    //            if(numWhite > numBlack){
    //                numTruePoints += numWhite;
    //                numFalsePoints += numBlack;
    //            }
    //            else{
    //                numTruePoints += numBlack;
    //                numFalsePoints += numWhite;
    //            }
    //            numTotalPoints += object.points.size();
    //        }
    //    }
}

void PCTrackContainer::toPointCloudXYZI(Cloud &cloudOut)
{
    for(int i=0;i<numTracks();i++){
        if(!isNewlyUpdated(i))  continue;
        PCObject object;
        object = tracks.at(i)->lastFrame().object;
        int id = tracks.at(i)->id;
        for(int j=0;j<object.points.size();j++){
            PointT point;
            point.x = object.points.at(j).pos[0];
            point.y = object.points.at(j).pos[1];
            point.z = object.points.at(j).pos[2];
            point.r = r[id];
            point.g = g[id];
            point.b = b[id];
            //                point.intensity = id;
            cloudOut.points.push_back(point);
        }
    }
}

void PCTrackContainer::toPointCloudXYZI_model(Cloud &cloudOut)
{
    for(int i=0;i<numTracks();i++){
        if(!isNewlyUpdated(i))  continue;
        PCObject object;
        object = tracks.at(i)->lastFrame().object;
        int id = tracks.at(i)->id;
        if(object.points_model.size() == 0){
            PointT point;
            point.x = point.y = point.y = 0;
            point.r = point.g = point.b = 0;
            point.a = 0;
            cloudOut.points.push_back(point);
        }
        for(int j=0;j<object.points_model.size();j++){
            PointT point;
            point.x = object.points_model.at(j).pos[0];
            point.y = object.points_model.at(j).pos[1];
            point.z = object.points_model.at(j).pos[2];
            point.r = r[id];
            point.g = g[id];
            point.b = b[id];
            //                point.intensity = id;
            cloudOut.points.push_back(point);
        }
    }
}

visualization_msgs::Marker PCTrackContainer::toMarkerEdges()
{
    visualization_msgs::Marker edgeMarker;

    edgeMarker.header.frame_id = "/origin";
    edgeMarker.header.stamp = ros::Time();
    edgeMarker.ns = "edge";
    edgeMarker.id = 1;
    edgeMarker.type = visualization_msgs::Marker::LINE_LIST;
    edgeMarker.action = visualization_msgs::Marker::ADD;
    edgeMarker.lifetime = ros::Duration(0);
    edgeMarker.scale.x = 0.003;
    edgeMarker.color.a = 1;
    edgeMarker.color.r = 1;
    edgeMarker.color.g = 0;
    edgeMarker.color.b = 0;

    for(int i=0;i<numTracks();i++){
        if(tracks.at(i)->lastFrame().time == currentT){
            PCObject object;
            object = tracks.at(i)->lastFrame().object;
            for(int j=0;j<object.edges.size();j++){
                Point p = object.edges.at(j).u;
                geometry_msgs::Point point;
                point.x = p.pos[0];
                point.y = p.pos[1];
                point.z = p.pos[2];
                edgeMarker.points.push_back(point);
                p = object.edges.at(j).v;
                point.x = p.pos[0];
                point.y = p.pos[1];
                point.z = p.pos[2];
                edgeMarker.points.push_back(point);
            }
        }
    }
    return edgeMarker;
}

visualization_msgs::MarkerArray PCTrackContainer::toMarkerGaussians()
{
    oldGaussiansId.clear();
    double margin = 0.1;
    double stepsize = 0.01;

    visualization_msgs::MarkerArray gaussianMarkers;

    int cnt = 0;
    for(int i=0;i<numTracks();i++){
        if(!isNewlyUpdated(i))  continue;
        PCObject object;
        object = tracks.at(i)->lastFrame().object;
        int id = tracks.at(i)->id;
        // make a gmm eval points of the object

        cnt ++;
        visualization_msgs::Marker gaussianMarker;
        gaussianMarker.header.frame_id = "/origin";
        gaussianMarker.header.stamp = ros::Time();
        gaussianMarker.ns = "gaussian";
        gaussianMarker.id = cnt;
        oldGaussiansId.push_back(cnt);

        Gaussian gaussian = object.gaussian;
        gaussianMarker.type = visualization_msgs::Marker::SPHERE;
        gaussianMarker.action = visualization_msgs::Marker::ADD;
        gaussianMarker.lifetime = ros::Duration(0);
        gaussianMarker.pose.position.x = gaussian.mean[0];
        gaussianMarker.pose.position.y = gaussian.mean[1];
        gaussianMarker.pose.position.z = gaussian.mean[2];

        Eigen::Matrix3d cov3d;
        for(int i=0;i<3;i++)
            for(int j=0;j<3;j++)
                cov3d(i,j)=gaussian.covariance(i,j);
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov3d);
        Eigen::Vector3d eigenvalues = eigensolver.eigenvalues();
        Eigen::Matrix3d eigenvectors = eigensolver.eigenvectors();
        Eigen::Matrix3d rotation;
        // ordering
        Eigen::Vector3d eigenvalues_ordered;
        eigenOrdering(eigenvalues, eigenvectors, eigenvalues_ordered, rotation);

        double m00 = rotation(0,0);
        double m01 = rotation(0,1);
        double m02 = rotation(0,2);
        double m10 = rotation(1,0);
        double m11 = rotation(1,1);
        double m12 = rotation(1,2);
        double m20 = rotation(2,0);
        double m21 = rotation(2,1);
        double m22 = rotation(2,2);

        // euler z-y-x sequence from orientation metrix
        double yaw = atan2(m10, m00);
        double pitch = -atan2(m20 , sqrt(m00*m00+m10*m10-m20*m20));
        double roll = atan2(sqrt(m12*m12+m02*m02), fabs(m22));
        //        if(m22<0) roll = PI-roll;

        if(m22>0 && m12<0 && m02>0) {
            if(yaw<0 && pitch>0)
                roll = -roll;
            else roll = roll;
        }
        else if (m22>0 && m12>0 && m02>0) roll = -roll;
        else if (m22<0 && m12>0 && m02<0){
            if(yaw>0 && pitch<0) roll = roll;
            else roll = -roll;
        }
        else if (m22<0 && m12<0 && m02<0) roll = -roll;

        // euler z-y-x sequence to quaternion
        double q0 = sin(yaw/2.)*sin(pitch/2.)*sin(roll/2.) + cos(yaw/2.)*cos(pitch/2.)*cos(roll/2.);
        double q1 = 0. - sin(yaw/2.)*sin(pitch/2.)*cos(roll/2.) + cos(yaw/2.)*cos(pitch/2.)*sin(roll/2.);
        double q2 = sin(yaw/2.)*cos(pitch/2.)*sin(roll/2.) + cos(yaw/2.)*sin(pitch/2.)*cos(roll/2.);
        double q3 = sin(yaw/2.)*cos(pitch/2.)*cos(roll/2.) - cos(yaw/2.)*sin(pitch/2.)*sin(roll/2.);

        gaussianMarker.pose.orientation.w = q0;
        gaussianMarker.pose.orientation.x = q1;
        gaussianMarker.pose.orientation.y = q2;
        gaussianMarker.pose.orientation.z = q3;

        gaussianMarker.frame_locked = 0;
        // confidence interval:
        // 95%: s=5.991,
        // 99%: s=9.210
        // 90%: s=4.605
//        gaussianMarker.scale.x = sqrt(eigenvalues_ordered[0]*3)*2;
//        gaussianMarker.scale.y = sqrt(eigenvalues_ordered[1]*3)*2;
//        gaussianMarker.scale.z = sqrt(eigenvalues_ordered[2]*3)*2;

        gaussianMarker.scale.x = sqrt(eigenvalues_ordered[0])*2;
        gaussianMarker.scale.y = sqrt(eigenvalues_ordered[1])*2;
        gaussianMarker.scale.z = sqrt(eigenvalues_ordered[2])*2;

//        gaussianMarker.scale.x = 1;
//        gaussianMarker.scale.y = 1;
//        gaussianMarker.scale.z = 1;

        gaussianMarker.color.a = 0.5;
        gaussianMarker.color.r = ((double)r[id])/256;
        gaussianMarker.color.g = ((double)g[id])/256;
        gaussianMarker.color.b = ((double)b[id])/256;

        gaussianMarkers.markers.push_back(gaussianMarker);
    }
    //    // delete markers
    //    for(int i=0;i<oldCnt_gaussians;i++){
    //        visualization_msgs::Marker delGauss;
    //        delGauss.header.frame_id = "/origin";
    //        delGauss.header.stamp = ros::Time();
    //        delGauss.ns = "gaussian";
    //        delGauss.id = i;
    //        delGauss.type = visualization_msgs::Marker::SPHERE;
    //        delGauss.action = visualization_msgs::Marker::DELETE;
    //        gaussianMarkers.markers.push_back(delGauss);
    //    }
    //    oldCnt_gaussians = cnt;
    return gaussianMarkers;
}

visualization_msgs::MarkerArray PCTrackContainer::oldGaussians()
{
    visualization_msgs::MarkerArray gaussianMarkers;

    // delete markers
    for(int i=0;i<oldGaussiansId.size();i++){
        visualization_msgs::Marker delGauss;
        delGauss.header.frame_id = "/origin";
        delGauss.header.stamp = ros::Time();
        delGauss.ns = "gaussian";
        delGauss.id = oldGaussiansId.at(i);
        delGauss.type = visualization_msgs::Marker::SPHERE;
        delGauss.action = visualization_msgs::Marker::DELETE;
        gaussianMarkers.markers.push_back(delGauss);
    }
    return gaussianMarkers;
}

visualization_msgs::MarkerArray PCTrackContainer::toMarkerIDs()
{
    oldTrackIDs.clear();
    visualization_msgs::MarkerArray idMarkers;

    int cnt = 0;
    for(int i=0;i<numTracks();i++){
        if(!isNewlyUpdated(i))  continue;
        Point centroid = tracks.at(i)->lastFrame().object.getCentroid();
        int id = tracks.at(i)->id;

        stringstream strm;
        string sID;
        strm << id;
        strm >> sID;

        visualization_msgs::Marker trackID;
        trackID.header.frame_id = "/origin";
        trackID.header.stamp = ros::Time();
        trackID.ns = "id";
        trackID.id = id;
        oldTrackIDs.push_back(id);
        trackID.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        trackID.action = visualization_msgs::Marker::ADD;
        trackID.lifetime = ros::Duration(0);
        trackID.pose.position.x = centroid.pos[0];
        trackID.pose.position.y = centroid.pos[1];
        trackID.pose.position.z = centroid.pos[2];
        trackID.pose.orientation.x = 0.0;
        trackID.pose.orientation.y = 0.0;
        trackID.pose.orientation.z = 0.0;
        trackID.pose.orientation.w = 1.0;
        trackID.color.a = 1.0;
        trackID.color.r = 1.0;
        trackID.color.g = 1.0;
        trackID.color.b = 1.0;
        trackID.text = sID;
        trackID.scale.z = 0.02;
        //        trackID.
        idMarkers.markers.push_back(trackID);
    }
    return idMarkers;
}

visualization_msgs::MarkerArray PCTrackContainer::oldMarkerIDs()
{
    visualization_msgs::MarkerArray idMarkers;

    // delete markers
    for(int i=0;i<oldTrackIDs.size();i++){
        visualization_msgs::Marker delId;

        delId.header.frame_id = "/origin";
        delId.header.stamp = ros::Time();
        delId.ns = "id";
        delId.id = oldTrackIDs.at(i);
        delId.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        delId.action = visualization_msgs::Marker::DELETE;

        idMarkers.markers.push_back(delId);
    }
    return idMarkers;

}

void PCTrackContainer::eigenOrdering(const Eigen::Vector3d& values, const Eigen::Matrix3d& vectors, Eigen::Vector3d& values_ordered, Eigen::Matrix3d& vectors_ordered)
{
    if(values[0] > values[1] && values[0] > values[2]){
        values_ordered[0] = values[0];
        vectors_ordered(0,0) = vectors.col(0)[0];
        vectors_ordered(1,0) = vectors.col(0)[1];
        vectors_ordered(2,0) = vectors.col(0)[2];
        if(values[1] > values[2]){
            values_ordered[1] = values[1];
            vectors_ordered(0,1) = vectors.col(1)[0];
            vectors_ordered(1,1) = vectors.col(1)[1];
            vectors_ordered(2,1) = vectors.col(1)[2];

            values_ordered[2] = values[2];
            vectors_ordered(0,2) = vectors.col(2)[0];
            vectors_ordered(1,2) = vectors.col(2)[1];
            vectors_ordered(2,2) = vectors.col(2)[2];
        }
        else{
            values_ordered[1] = values[2];
            vectors_ordered(0,1) = vectors.col(2)[0];
            vectors_ordered(1,1) = vectors.col(2)[1];
            vectors_ordered(2,1) = vectors.col(2)[2];

            values_ordered[2] = values[1];
            vectors_ordered(0,2) = vectors.col(1)[0];
            vectors_ordered(1,2) = vectors.col(1)[1];
            vectors_ordered(2,2) = vectors.col(1)[2];
        }
    }
    else if(values[1] > values[0] && values[1] > values[2]){
        values_ordered[0] = values[1];
        vectors_ordered(0,0) = vectors.col(1)[0];
        vectors_ordered(1,0) = vectors.col(1)[1];
        vectors_ordered(2,0) = vectors.col(1)[2];
        if(values[0] > values[2]){
            values_ordered[1] = values[0];
            vectors_ordered(0,1) = vectors.col(0)[0];
            vectors_ordered(1,1) = vectors.col(0)[1];
            vectors_ordered(2,1) = vectors.col(0)[2];

            values_ordered[2] = values[2];
            vectors_ordered(0,2) = vectors.col(2)[0];
            vectors_ordered(1,2) = vectors.col(2)[1];
            vectors_ordered(2,2) = vectors.col(2)[2];
        }
        else{
            values_ordered[1] = values[2];
            vectors_ordered(0,1) = vectors.col(2)[0];
            vectors_ordered(1,1) = vectors.col(2)[1];
            vectors_ordered(2,1) = vectors.col(2)[2];

            values_ordered[2] = values[0];
            vectors_ordered(0,2) = vectors.col(0)[0];
            vectors_ordered(1,2) = vectors.col(0)[1];
            vectors_ordered(2,2) = vectors.col(0)[2];
        }
    }
    else if(values[2] > values[0] && values[2] > values[1]){
        values_ordered[0] = values[2];
        vectors_ordered(0,0) = vectors.col(2)[0];
        vectors_ordered(1,0) = vectors.col(2)[1];
        vectors_ordered(2,0) = vectors.col(2)[2];
        if(values[0] > values[1]){
            values_ordered[1] = values[0];
            vectors_ordered(0,1) = vectors.col(0)[0];
            vectors_ordered(1,1) = vectors.col(0)[1];
            vectors_ordered(2,1) = vectors.col(0)[2];

            values_ordered[2] = values[1];
            vectors_ordered(0,2) = vectors.col(1)[0];
            vectors_ordered(1,2) = vectors.col(1)[1];
            vectors_ordered(2,2) = vectors.col(1)[2];
        }
        else{
            values_ordered[1] = values[1];
            vectors_ordered(0,1) = vectors.col(1)[0];
            vectors_ordered(1,1) = vectors.col(1)[1];
            vectors_ordered(2,1) = vectors.col(1)[2];

            values_ordered[2] = values[0];
            vectors_ordered(0,2) = vectors.col(0)[0];
            vectors_ordered(1,2) = vectors.col(0)[1];
            vectors_ordered(2,2) = vectors.col(0)[2];
        }
    }
}

visualization_msgs::MarkerArray PCTrackContainer::toMarkerGMMs()
{

    double margin = 0.1;
    double stepsize = 0.01;

    visualization_msgs::MarkerArray gmmMarkers;

    int cnt = 0;
    for(int i=0;i<numTracks();i++){
        if(!isNewlyUpdated(i))   continue;
        PCObject object;
        object = tracks.at(i)->lastFrame().object;
        if(object.state != NOGMM){
            int id = tracks.at(i)->id;
            // make a gmm eval points of the object

            for(int j=0;j<object.gmm.size();j++){
                cnt ++;
                visualization_msgs::Marker gmmMarker;
                gmmMarker.header.frame_id = "/origin";
                gmmMarker.header.stamp = ros::Time();
                gmmMarker.ns = "gmm";
                gmmMarker.id = cnt;

                Gaussian gmm = object.gmm.at(j);
                gmmMarker.type = visualization_msgs::Marker::SPHERE;
                gmmMarker.action = visualization_msgs::Marker::ADD;
                gmmMarker.lifetime = ros::Duration(0);
                gmmMarker.pose.position.x = gmm.mean[0];
                gmmMarker.pose.position.y = gmm.mean[1];
                gmmMarker.pose.position.z = gmm.mean[2];

                Eigen::Matrix3d cov3d;
                for(int i=0;i<3;i++)
                    for(int j=0;j<3;j++)
                        cov3d(i,j)=gmm.covariance(i,j);
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov3d);
                Eigen::Vector3d eigenvalues = eigensolver.eigenvalues();
                Eigen::Matrix3d eigenvectors = eigensolver.eigenvectors();
                // ordering
                Eigen::Vector3d eigenvalues_ordered;
                Eigen::Matrix3d rotation;
                eigenOrdering(eigenvalues, eigenvectors, eigenvalues_ordered, rotation);

                double m00 = rotation(0,0);
                double m01 = rotation(0,1);
                double m02 = rotation(0,2);
                double m10 = rotation(1,0);
                double m11 = rotation(1,1);
                double m12 = rotation(1,2);
                double m20 = rotation(2,0);
                double m21 = rotation(2,1);
                double m22 = rotation(2,2);

                // euler z-y-x sequence from orientation metrix
                double yaw = atan2(m10, m00);
                double pitch = -atan2(m20 , sqrt(m00*m00+m10*m10-m20*m20));
                double roll = atan2(sqrt(m12*m12+m02*m02), fabs(m22));
                //        if(m22<0) roll = PI-roll;

                if(m22>0 && m12<0 && m02>0) {
                    if(yaw<0 && pitch>0)
                        roll = -roll;
                    else roll = roll;
                }
                else if (m22>0 && m12>0 && m02>0) roll = -roll;
                else if (m22<0 && m12>0 && m02<0){
                    if(yaw>0 && pitch<0) roll = roll;
                    else roll = -roll;
                }
                else if (m22<0 && m12<0 && m02<0) roll = -roll;
                //
                //                cout<<id<<" m00: "<<m00<<endl;
                //                cout<<id<<" m10: "<<m10<<endl;
                //                cout<<id<<" m20: "<<m20<<endl;
                //                cout<<id<<"eigenv1: "<<eigenvalues_ordered[0]<<endl;
                //                cout<<id<<"eigenv2: "<<eigenvalues_ordered[1]<<endl;
                //                cout<<id<<"eigenv3: "<<eigenvalues_ordered[2]<<endl;
                //                cout<<id<<" yaw: "<<yaw*180./PI<<endl;
                //                cout<<id<<" pitch: "<<pitch*180./PI<<endl;
                //                cout<<id<<" roll: "<<roll*180./PI<<endl;

                // euler z-y-x sequence to quaternion
                double q0 = sin(yaw/2.)*sin(pitch/2.)*sin(roll/2.) + cos(yaw/2.)*cos(pitch/2.)*cos(roll/2.);
                double q1 = 0. - sin(yaw/2.)*sin(pitch/2.)*cos(roll/2.) + cos(yaw/2.)*cos(pitch/2.)*sin(roll/2.);
                double q2 = sin(yaw/2.)*cos(pitch/2.)*sin(roll/2.) + cos(yaw/2.)*sin(pitch/2.)*cos(roll/2.);
                double q3 = sin(yaw/2.)*cos(pitch/2.)*cos(roll/2.) - cos(yaw/2.)*sin(pitch/2.)*sin(roll/2.);

                gmmMarker.pose.orientation.w = q0;
                gmmMarker.pose.orientation.x = q1;
                gmmMarker.pose.orientation.y = q2;
                gmmMarker.pose.orientation.z = q3;

                gmmMarker.frame_locked = 0;

                // confidence interval:
                // 95%: s=5.991,
                // 99%: s=9.210
                // 90%: s=4.605
                gmmMarker.scale.x = sqrt(2*eigenvalues_ordered[0])*2;
                gmmMarker.scale.y = sqrt(2*eigenvalues_ordered[1])*2;
                gmmMarker.scale.z = sqrt(2*eigenvalues_ordered[2])*2;

                gmmMarker.color.a = gmm.weight*object.gmm.size()/2;
                gmmMarker.color.r = ((double)r[id])/256;
                gmmMarker.color.g = ((double)g[id])/256;
                gmmMarker.color.b = ((double)b[id])/256;

                gmmMarkers.markers.push_back(gmmMarker);

            }
        }
    }

    // delete markers
    for(int i=cnt+1;i<=oldCnt;i++){
        visualization_msgs::Marker delGauss;
        delGauss.header.frame_id = "/origin";
        delGauss.header.stamp = ros::Time();
        delGauss.ns = "gmm";
        delGauss.id = i;
        delGauss.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        delGauss.action = visualization_msgs::Marker::DELETE;
        gmmMarkers.markers.push_back(delGauss);
    }
    oldCnt = cnt;
    return gmmMarkers;
}

void PCTrackContainer::evaluate()
{
    numTruePoints = 0;
    numFalsePoints = 0;
    numTotalPoints = 0;

    for(int i=0;i<numTracks();i++){
        if(tracks.at(i)->lastFrame().time == currentT){
            PCObject object;
            object = tracks.at(i)->lastFrame().object;
            int numBlack = 0;
            int numWhite = 0;
            for(int j=0;j<object.points.size();j++){
                Point point = object.points.at(j);
                double avgrgb = (point.rgb[0]*3000 + point.rgb[1]*3000 + point.rgb[2]*3000) / 3;

                if(avgrgb > 128) numWhite ++;
                else numBlack ++;
            }
            if(numWhite > numBlack){
                numTruePoints += numWhite;
                numFalsePoints += numBlack;
            }
            else{
                numTruePoints += numBlack;
                numFalsePoints += numWhite;
            }
            numTotalPoints += object.points.size();
        }
    }
}