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

#ifndef BOOST_MP_EIGEN_HPP
#define BOOST_MP_EIGEN_HPP

#include <boost/multiprecision/number.hpp>
#include <Eigen/Core>

//
// Generic Eigen support code:
//
namespace Eigen {
template <class Backend, boost::multiprecision::expression_template_option ExpressionTemplates>
struct NumTraits<boost::multiprecision::number<Backend, ExpressionTemplates> >
{
   using self_type = boost::multiprecision::number<Backend, ExpressionTemplates>                         ;
   using Real = typename boost::multiprecision::scalar_result_from_possible_complex<self_type>::type;
   using NonInteger = self_type                                                                           ; // Not correct but we can't do much better??
   using Literal = double                                                                              ;
   using Nested = self_type                                                                           ;
   enum
   {
      IsComplex             = boost::multiprecision::number_category<self_type>::value == boost::multiprecision::number_kind_complex,
      IsInteger             = boost::multiprecision::number_category<self_type>::value == boost::multiprecision::number_kind_integer,
      ReadCost              = 1,
      AddCost               = 4,
      MulCost               = 8,
      IsSigned              = std::numeric_limits<self_type>::is_specialized ? std::numeric_limits<self_type>::is_signed : true,
      RequireInitialization = 1,
   };
   static Real epsilon()
   {
      return std::numeric_limits<Real>::epsilon();
   }
   static Real dummy_precision()
   {
      return 1000 * epsilon();
   }
   static Real highest()
   {
      return (std::numeric_limits<Real>::max)();
   }
   static Real lowest()
   {
      return (std::numeric_limits<Real>::min)();
   }
   static int digits10_imp(const std::integral_constant<bool, true>&)
   {
      return std::numeric_limits<Real>::digits10;
   }
   template <bool B>
   static int digits10_imp(const std::integral_constant<bool, B>&)
   {
      return Real::default_precision();
   }
   static int digits10()
   {
      return digits10_imp(std::integral_constant<bool, std::numeric_limits<Real>::digits10 && (std::numeric_limits<Real>::digits10 != INT_MAX) ? true : false > ());
   }
};
template <class tag, class Arg1, class Arg2, class Arg3, class Arg4>
struct NumTraits<boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4> > : public NumTraits<typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type>
{
};

#define BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(A)                                                                                \
   template <class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>                                                                                \
   struct ScalarBinaryOpTraits<boost::multiprecision::number<Backend, ExpressionTemplates>, A, BinaryOp>                                                                                \
   {                                                                                \
      /*static_assert(boost::multiprecision::is_compatible_arithmetic_type<A, boost::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this arithmetic type is not supported.");*/ \
      using ReturnType = boost::multiprecision::number<Backend, ExpressionTemplates>;                                                                                \
   };                                                                                \
   template <class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>                                                                                \
   struct ScalarBinaryOpTraits<A, boost::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp>                                                                                \
   {                                                                                \
      /*static_assert(boost::multiprecision::is_compatible_arithmetic_type<A, boost::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this arithmetic type is not supported.");*/ \
      using ReturnType = boost::multiprecision::number<Backend, ExpressionTemplates>;                                                                                \
   };

BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(float)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(double)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(long double)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(char)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned char)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(signed char)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(short)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned short)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(int)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned int)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(long)
BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned long)

#if 0
      template<class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, class Backend2, boost::multiprecision::expression_template_option ExpressionTemplates2, typename BinaryOp>
   struct ScalarBinaryOpTraits<boost::multiprecision::number<Backend, ExpressionTemplates>, boost::multiprecision::number<Backend2, ExpressionTemplates2>, BinaryOp>
   {
      static_assert(
         boost::multiprecision::is_compatible_arithmetic_type<boost::multiprecision::number<Backend2, ExpressionTemplates2>, boost::multiprecision::number<Backend, ExpressionTemplates> >::value
         || boost::multiprecision::is_compatible_arithmetic_type<boost::multiprecision::number<Backend, ExpressionTemplates>, boost::multiprecision::number<Backend2, ExpressionTemplates2> >::value, "Interoperability with this arithmetic type is not supported.");
      using ReturnType = typename std::conditional<std::is_convertible<boost::multiprecision::number<Backend2, ExpressionTemplates2>, boost::multiprecision::number<Backend, ExpressionTemplates> >::value,
         boost::multiprecision::number<Backend, ExpressionTemplates>, boost::multiprecision::number<Backend2, ExpressionTemplates2> >::type;
   };

   template<unsigned D, typename BinaryOp>
   struct ScalarBinaryOpTraits<boost::multiprecision::number<boost::multiprecision::backends::mpc_complex_backend<D>, boost::multiprecision::et_on>, boost::multiprecision::mpfr_float, BinaryOp>
   {
      using ReturnType = boost::multiprecision::number<boost::multiprecision::backends::mpc_complex_backend<D>, boost::multiprecision::et_on>;
   };

   template<typename BinaryOp>
   struct ScalarBinaryOpTraits<boost::multiprecision::mpfr_float, boost::multiprecision::mpc_complex, BinaryOp>
   {
      using ReturnType = boost::multiprecision::number<boost::multiprecision::backends::mpc_complex_backend<0>, boost::multiprecision::et_on>;
   };

   template<class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>
   struct ScalarBinaryOpTraits<boost::multiprecision::number<Backend, ExpressionTemplates>, boost::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp>
   {
      using ReturnType = boost::multiprecision::number<Backend, ExpressionTemplates>;
   };
#endif

template <class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, class tag, class Arg1, class Arg2, class Arg3, class Arg4, typename BinaryOp>
struct ScalarBinaryOpTraits<boost::multiprecision::number<Backend, ExpressionTemplates>, boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>, BinaryOp>
{
   static_assert(std::is_convertible<typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type, boost::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this arithmetic type is not supported.");
   using ReturnType = boost::multiprecision::number<Backend, ExpressionTemplates>;
};

template <class tag, class Arg1, class Arg2, class Arg3, class Arg4, class Backend, boost::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>
struct ScalarBinaryOpTraits<boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>, boost::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp>
{
   static_assert(std::is_convertible<typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type, boost::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this arithmetic type is not supported.");
   using ReturnType = boost::multiprecision::number<Backend, ExpressionTemplates>;
};

namespace internal {
template <typename Scalar>
struct conj_retval;

template <typename Scalar, bool IsComplex>
struct conj_impl;

template <class tag, class Arg1, class Arg2, class Arg3, class Arg4>
struct conj_retval<boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4> >
{
   using type = typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type;
};

template <class tag, class Arg1, class Arg2, class Arg3, class Arg4>
struct conj_impl<boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>, true>
{
   EIGEN_DEVICE_FUNC
   static inline typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type run(const typename boost::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>& x)
   {
      return conj(x);
   }
};

} // namespace internal

} // namespace Eigen

#endif


/*
 * Copyright (c) 2016, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Michael X. Grey <mxgrey@gatech.edu>
 *
 * Georgia Tech Graphics Lab and AMBER Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Aaron D. Ames
 * <karenliu@cc.gatech.edu> <ames@gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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 <vector>
#include <Eigen/Core>
#include <dart/dynamics/Skeleton.h>
#include <dart/dynamics/EndEffector.h>
#include <dart/dynamics/DegreeOfFreedom.h>
#include <dart/simulation/World.h>
#include <osgDart/osgDart.h>
#include <hubo/DrcModel.hpp>
#include <hubo/RelaxedPosture.hpp>

#include <HuboPath/Operator.hpp>

using Trajectory = std::vector<Eigen::VectorXd>;
using namespace dart::dynamics;
using namespace dart::simulation;

//const double DefaultStepLength = 0.13;
const double DefaultStepLength = 0.155;
const double frequency = 200.0;

//==============================================================================
struct StepContext
{
  StepContext(EndEffector* stance, EndEffector* swing,
              size_t index, double t0, double dist)
    : stance_foot(stance),
      swing_foot(swing),
      route_index(index),
      start_t(t0),
      distance(dist),
      attempts(0)
  {
    // Do nothing
  }

  EndEffector* stance_foot;
  EndEffector* swing_foot;
  size_t route_index;
  double start_t;

  Eigen::VectorXd start_q;
  Eigen::VectorXd end_q;

  double distance;
  size_t attempts;
};

//==============================================================================
std::tuple<size_t, double> getNextRouteLocation(
    const std::vector<Eigen::VectorXd>& route,
    double stepDist,
    size_t start_index,
    double start_t)
{
  assert(start_t < 1.0);

  double remaining_D = stepDist;
  while(remaining_D > 0)
  {
    if(start_index == route.size()-1)
    {
      return std::tuple<size_t, double>(start_index, 0);
    }

    Eigen::VectorXd q0 = route[start_index];
    Eigen::VectorXd q1 = route[start_index+1];

    Eigen::Vector2d p0 = q0.block<2,1>(3,0);
    Eigen::Vector2d p1 = q1.block<2,1>(3,0);

    double D = (p1-p0).norm();
    double t = start_t + remaining_D/D;
    if(t >= 1)
    {
      remaining_D = remaining_D - D*(1.0-start_t);
      ++start_index;
      start_t = 0.0;
    }
    else
    {
      start_t = t;
      remaining_D = 0.0;
    }
  }

  return std::tuple<size_t, double>(start_index, start_t);
}

//==============================================================================
Eigen::VectorXd getStepConfig(const std::vector<Eigen::VectorXd>& route,
                              const StepContext& step)
{
  Eigen::VectorXd q = route[step.route_index];
  if(step.route_index < route.size()-1 && step.start_t > 0.0)
    q += step.start_t*(route[step.route_index+1]-route[step.route_index]);

  return q;
}

//==============================================================================
bool confirmStepReachability(const dart::dynamics::SkeletonPtr& robot,
                             const std::vector<Eigen::VectorXd>& route,
                             const StepContext& step0,
                             StepContext& step1)
{
//  std::cout << step0.route_index << ":" << step0.start_t
//            << " | " << step1.route_index << ":" << step1.start_t << std::endl;

  std::shared_ptr<hubo::RelaxedPosture> posture =
      std::dynamic_pointer_cast<hubo::RelaxedPosture>(robot->getIK(true)->getObjective());

  Eigen::VectorXd q0 = getStepConfig(route, step0);
  robot->setPositions(q0);
  const Eigen::Isometry3d tf_foot0 = step0.stance_foot->getWorldTransform();
  step0.stance_foot->getIK(true)->getTarget()->setTransform(tf_foot0);
  step0.stance_foot->getIK(true)->setActive(true);

//  std::cout << "\n------------------\n"
//            << step0.stance_foot->getName() <<"\n"
//            << tf_foot0.matrix() << std::endl;

  Eigen::VectorXd q1 = getStepConfig(route, step1);
  robot->setPositions(q1);
  const Eigen::Isometry3d tf_foot1 = step1.stance_foot->getWorldTransform();
  step1.stance_foot->getIK(true)->getTarget()->setTransform(tf_foot1);
  step1.stance_foot->getIK(true)->setActive(true);

//  std::cout << "\n" << step1.stance_foot->getName() << "\n"
//            << tf_foot1.matrix() << std::endl;

  assert(step0.stance_foot != step1.stance_foot);

  // Solve while leaning on first foot
  step0.stance_foot->getSupport()->setActive(true);
  step1.stance_foot->getSupport()->setActive(false);
  robot->setPositions(q0);

  if(posture)
    posture->enforceIdealPosture = true;

  robot->getIK(true)->solve();
//  if(posture)
//    posture->enforceIdealPosture = false;
//  if(!robot->getIK(true)->solve())
//    return false;

  step1.start_q = robot->getPositions();

  // Solve while leaning on second foot
  step1.stance_foot->getSupport()->setActive(true);
  step0.stance_foot->getSupport()->setActive(false);
  robot->setPositions(q1);

  if(posture)
    posture->enforceIdealPosture = true;

  robot->getIK(true)->solve();
//  if(posture)
//    posture->enforceIdealPosture = false;

//  if(!robot->getIK(true)->solve())
//    return false;

  step1.end_q = robot->getPositions();

  return true;
}

//==============================================================================
bool testNextStepContext(const dart::dynamics::SkeletonPtr& robot,
                         const std::vector<Eigen::VectorXd>& route,
                         const StepContext& lastStep,
                         StepContext& nextStep)
{
  size_t start_index = lastStep.route_index;

  if(start_index >= route.size())
    return true;

  std::tie(nextStep.route_index, nextStep.start_t) =
      getNextRouteLocation(route, nextStep.distance,
                           start_index, lastStep.start_t);

  return confirmStepReachability(robot, route, lastStep, nextStep);
}

//==============================================================================
bool getNextStepContext(const dart::dynamics::SkeletonPtr& robot,
                        const std::vector<Eigen::VectorXd>& route,
                        const StepContext& lastStep,
                        StepContext& nextStep)
{
  const size_t maxAttempts = 20;
  size_t attempts = 0;
  while(!testNextStepContext(robot, route, lastStep, nextStep))
  {
    nextStep.distance /= 2.0;

    ++attempts;
//    std::cout << "Attempt #" << attempts << " out of " << maxAttempts << std::endl;
    if(attempts > maxAttempts)
    {
//      std::cout << "[getNextStepContext] ERROR: Tried " << attempts << " times "
//                << "to find the next step, but it keeps failing! Reverting!"
//                << std::endl;

      return false;
    }
  }

  return true;
}

//==============================================================================
std::vector<StepContext> generateSteps(
    const dart::dynamics::SkeletonPtr& robot,
    const std::vector<Eigen::VectorXd>& route,
    double stepDistance = DefaultStepLength)
{
  robot->setPositions(route.front());

  StepContext lastStep(robot->getEndEffector("l_foot"),
                       robot->getEndEffector("r_foot"),
                       0, 0, stepDistance);
  lastStep.start_q = robot->getPositions();
  lastStep.end_q = lastStep.start_q;

  std::vector<StepContext> steps;
  steps.push_back(lastStep);

  std::swap(lastStep.stance_foot, lastStep.swing_foot);
  steps.push_back(lastStep);

  const size_t maxAttempts = 3;
  while(steps.back().route_index < route.size()-1
        && steps.size() > 1)
  {
    std::cout << "Steps: " << steps.size() << " (" <<
              100*steps.back().route_index/route.size() << "%)" << std::endl;
    StepContext& previousStep = steps[steps.size()-2];
    StepContext& currentStep = steps.back();
    if(!getNextStepContext(robot, route, previousStep, currentStep))
    {
      previousStep.distance /= 2.0;
      ++previousStep.attempts;
      steps.pop_back();
//      std::cout << " === stepping failed " << std::endl;
//      return steps;

      if(previousStep.attempts >= maxAttempts)
      {
//        steps.pop_back();
//        steps.back().distance /= 2.0;
//        ++steps.back().attempts;

        std::cout << " ==== Failing to generate steps!" << std::endl;

//        osgDart::Viewer viewer;
//        viewer.addWorldNode(new osgDart::WorldNode(world));
//        viewer.setUpViewInWindow(0, 0, 640, 480);
//        viewer.run();

        return steps;
      }
      continue;
    }

    StepContext nextStep(currentStep);
    std::swap(nextStep.stance_foot, nextStep.swing_foot);
    nextStep.distance = stepDistance;

    steps.push_back(nextStep);
  }

  if(steps.size() < 2)
  {
//    std::cout << " ======= Failed to generate steps! ===== " << std::endl;
    return steps;
  }

  // The last step is extraneous
  steps.pop_back();

//  std::cout << " --- Generating finish --- " << std::endl;
//  std::cout << "Last " << steps.back().route_index << " : " << steps.back().start_t << std::endl;
  // One last finishing step
  StepContext nextStep(steps.back());
  std::swap(nextStep.stance_foot, nextStep.swing_foot);
  nextStep.distance = stepDistance;
  getNextStepContext(robot, route, steps.back(), nextStep);
  nextStep.end_q = route.back();
  steps.push_back(nextStep);

  return steps;
}

//==============================================================================
void interpolateSteps(const dart::dynamics::SkeletonPtr& robot,
                      std::vector<Eigen::VectorXd>& walk,
                      std::vector<size_t>& stepRanges,
                      const StepContext& stepFrom,
                      const StepContext& stepTo,
                      bool excludeStep = false)
{
  std::shared_ptr<hubo::RelaxedPosture> posture =
      std::dynamic_pointer_cast<hubo::RelaxedPosture>(robot->getIK(true)->getObjective());

  dart::dynamics::EndEffector* stance = stepFrom.stance_foot;
  dart::dynamics::EndEffector* swing = stepFrom.swing_foot;

  if(!excludeStep)
  {
    stance->getSupport()->setActive(true);
    swing->getSupport()->setActive(false);

    robot->setPositions(stepFrom.end_q);
    stance->getIK(true)->getTarget()->setTransform(
          stance->getWorldTransform());
    const Eigen::Isometry3d swingStart = swing->getWorldTransform();

    robot->setPositions(stepTo.start_q);
    const Eigen::Isometry3d swingGoal = swing->getWorldTransform();

    const Eigen::Vector2d v =
        (swingGoal.translation()-swingStart.translation()).block<2,1>(0,0)/(2*M_PI);
    const double D = v.norm();

    Eigen::AngleAxisd aa(swingStart.linear().transpose()*swingGoal.linear());
    const double R = aa.angle()/(2*M_PI);
    const Eigen::Vector3d axis = aa.axis();

    const size_t res = 10;
    stepRanges.push_back(walk.size());
    for(size_t i=0; i < res+1; ++i)
    {
      const double t = (double)(i)/(double)(res)*2*M_PI;
      const Eigen::Vector2d x = v*(t - sin(t));
      const double z = D*(1 - cos(t));
      const double theta = t*R;

      Eigen::Isometry3d swingTarget = swingStart;
      swingTarget.translation().block<2,1>(0,0) += x;
      swingTarget.translation()[2] += z;
      swingTarget.rotate(Eigen::AngleAxisd(theta, axis));

      swing->getIK(true)->getTarget()->setTransform(swingTarget);

      if(posture)
        posture->enforceIdealPosture = true;

      robot->getIK(true)->solve();

      if(posture)
        posture->enforceIdealPosture = false;

      if(!robot->getIK(true)->solve())
      {
        walk.push_back(robot->getPositions());
        std::cout <<" ===== COULD NOT SOLVE FOOT STEP AT INDEX " << walk.size()-1 << std::endl;
        return;
      }

      walk.push_back(robot->getPositions());
    }
  }

  const Eigen::VectorXd& start_q = stepTo.start_q;
  const Eigen::VectorXd& end_q = stepTo.end_q;

  if(excludeStep)
  {
    robot->setPositions(start_q);
    stance->getIK(true)->getTarget()->setTransform(
          stance->getWorldTransform());

    swing->getIK(true)->getTarget()->setTransform(
          swing->getWorldTransform());
  }

  const Eigen::Vector6d x0 = start_q.block<6,1>(0,0);
  const Eigen::Vector6d xf = end_q.block<6,1>(0,0);
  const Eigen::Vector6d dx = xf - x0;

  // Consider disabling the IK from moving the hips during this stage
  stance->getSupport()->setActive(true);
  swing->getSupport()->setActive(true);

  const size_t res = 10;
  stepRanges.push_back(walk.size());
  for(size_t i=0; i < res+1; ++i)
  {
    const double t = (double)(i)/(double)(res);

    const Eigen::Vector6d x = x0 + t*dx;
    robot->getJoint(0)->setPositions(x);

    if(!robot->getIK(true)->solve())
    {
      walk.push_back(robot->getPositions());
      std::cout <<" ===== COULD NOT SOLVE FOOT STEP AT INDEX " << walk.size()-1 << std::endl;
      return;
    }

    walk.push_back(robot->getPositions());
  }
}

//==============================================================================
std::vector<Eigen::VectorXd> convertRouteToWalk(
    const dart::dynamics::SkeletonPtr& robot,
    const Trajectory& trajectory,
    std::vector<size_t>& stepRanges)
{
  const std::vector<Eigen::VectorXd>& route = trajectory;

  auto originalBoundsLeft =
      robot->getEndEffector("l_foot")->getIK(true)->getErrorMethod().getBounds();
  auto originalBoundsRight =
      robot->getEndEffector("r_foot")->getIK(true)->getErrorMethod().getBounds();

  Eigen::Vector6d bounds(Eigen::Vector6d::Constant(
                          dart::dynamics::DefaultIKTolerance));
  robot->getEndEffector("l_foot")->getIK(true)->getErrorMethod().setBounds(-bounds, bounds);
  robot->getEndEffector("r_foot")->getIK(true)->getErrorMethod().setBounds(-bounds, bounds);

  std::shared_ptr<hubo::RelaxedPosture> posture =
      std::dynamic_pointer_cast<hubo::RelaxedPosture>(
        robot->getIK(true)->getObjective());
  if(posture)
    posture->enforceIdealPosture = true;

//  std::cout << "Generate steps" << std::endl;
  std::vector<StepContext> steps = generateSteps(robot, route);

  std::vector<Eigen::VectorXd> walk;

  StepContext firstStep = steps[0];
  firstStep.start_q = route[0];
  firstStep.end_q = steps[1].start_q;
  interpolateSteps(robot, walk, stepRanges, firstStep, firstStep, true);
  stepRanges.clear();
  for(size_t i=0; i < steps.size()-1; ++i)
  {
    const StepContext& stepFrom = steps[i];
    const StepContext& stepTo = i < steps.size()-1? steps[i+1] : steps[i];
    interpolateSteps(robot, walk, stepRanges, stepFrom, stepTo);
  }

  robot->getEndEffector("l_foot")->getIK(true)->getErrorMethod().setBounds(originalBoundsLeft);
  robot->getEndEffector("r_foot")->getIK(true)->getErrorMethod().setBounds(originalBoundsRight);

  return walk;
}

//==============================================================================
class TrajectoryDisplayWorld : public osgDart::WorldNode
{
public:

  TrajectoryDisplayWorld(dart::simulation::WorldPtr world,
                         const std::vector<Eigen::VectorXd>& traj)
    : osgDart::WorldNode(world), mTrajectory(traj)
  {
    hubo = world->getSkeleton(0);
    LSR = hubo->getDof("LSR")->getIndexInSkeleton();
    RSR = hubo->getDof("RSR")->getIndexInSkeleton();

    firstLoop = true;
  }

  void customPreRefresh() override
  {
    if(mTrajectory.size() == 0)
    {
      if(firstLoop)
      {
        std::cerr << "No trajectory was generated!" << std::endl;
        firstLoop = false;
      }
      return;
    }

    if(firstLoop)
    {
      mTimer.setStartTick();
      firstLoop = false;
    }

    double time = mTimer.time_s();
    size_t count = (size_t)(floor(frequency*time)) % mTrajectory.size();

    double dt = 1.0/frequency;

    size_t last_1 = count <= 0? 0 : count - 1;
    size_t last_2 = count <= 1? 0 : count - 2;
    size_t last_3 = count <= 2? 0 : count - 3;
    size_t last_4 = count <= 3? 0 : count - 4;

    size_t next_1 = count < mTrajectory.size() - 1? count + 1 : count;
    size_t next_2 = count < mTrajectory.size() - 2? count + 2 : count;
    size_t next_3 = count < mTrajectory.size() - 3? count + 3 : count;
    size_t next_4 = count < mTrajectory.size() - 4? count + 4 : count;

    Eigen::VectorXd velocities =
        (0.5*mTrajectory[next_1] - 0.5*mTrajectory[last_1])/dt;

    Eigen::Matrix<double, 5, 1> c;
    c << -205.0/72.0, 8.0/5.0, -1.0/5.0, 8.0/315.0, -1.0/560.0;

    Eigen::VectorXd accelerations =
        (  c[1]*mTrajectory[next_1] + c[2]*mTrajectory[next_2] + c[3]*mTrajectory[next_3] + c[4]*mTrajectory[next_4]
         + c[0]*mTrajectory[count]
         + c[1]*mTrajectory[last_1] + c[2]*mTrajectory[last_2] + c[3]*mTrajectory[last_3] + c[4]*mTrajectory[last_4])/pow(dt,2);

//    accelerations.head<6>().setZero();

    hubo->setPositions(mTrajectory[count]);
    hubo->setVelocities(velocities);
    hubo->setAccelerations(accelerations);

//    hubo->setPositions(mMapping, positions);
//    clone->setPositions(mMapping, mRaw[count]);
  }

protected:

  SkeletonPtr hubo;
  std::vector<Eigen::VectorXd> mTrajectory;
  size_t LSR;
  size_t RSR;

  bool firstLoop;
  osg::Timer mTimer;
};

//==============================================================================
int main()
{
  SkeletonPtr robot = hubo::DrcModel::create();
  robot->getIK(true)->getSolver()->setNumMaxIterations(1000);
//  std::cout << robot->getIK(true)->getSolver()->getNumMaxIterations() << std::endl;
  std::vector<Eigen::VectorXd> locations;
  locations.push_back(robot->getPositions());
  locations.push_back(robot->getPositions());

//  locations[1][3] += 3*DefaultStepLength;
  locations[1][3] += 6*DefaultStepLength;
//  locations[1][3] += 2.0;

  std::vector<size_t> stepRanges;
  std::vector<Eigen::VectorXd> walk = convertRouteToWalk(robot, locations, stepRanges);
  std::cout << "waypoints: " << walk.size() << std::endl;
  std::cout << "Step Ranges: ";
  for(size_t i=0; i < stepRanges.size(); ++i)
    std::cout << stepRanges[i] << ", ";
  std::cout << std::endl;

  HuboPath::Operator mOperator;
  std::vector<size_t> opIndices;
  std::vector<std::string> indexNames;
  for(size_t i=6; i < robot->getNumDofs(); ++i)
  {
    DegreeOfFreedom* dof = robot->getDof(i);
    opIndices.push_back(i);
    indexNames.push_back(dof->getName());
  }
  mOperator.setJointIndices(indexNames);

  HuboPath::Trajectory trajectory;
  for(size_t r=0; r < stepRanges.size()+1; ++r)
  {
    size_t start_i = r==0? 0 : stepRanges[r-1]-1;
    size_t end_i = r==stepRanges.size()? walk.size() : stepRanges[r];
    std::cout << "step from " << start_i << " to " << end_i << std::endl;

    mOperator.clearWaypoints();
    for(size_t i=start_i; i < end_i; ++i)
    {
      robot->setPositions(walk[i]);
      mOperator.addWaypoint(robot->getPositions(opIndices));
    }

    HuboPath::Trajectory partial_trajectory = mOperator.getCurrentTrajectory();

    if(r > 0)
      partial_trajectory.elements.erase(partial_trajectory.elements.begin());

    HuboCan::HuboDescription& desc = partial_trajectory.desc;
    for(size_t i=0; i < desc.getJointCount(); ++i)
    {
      hubo_joint_info_t& info = desc.joints[i]->info;
      hubo_joint_limits_t& limits = info.limits;

      limits.nominal_speed /= 5.0;
      limits.nominal_accel /= 5.0;
    }

    std::cout << "Initial size of partial: " << partial_trajectory.elements.size() << std::endl;
    std::cout << "Interpolating #" << r << "..." << std::endl;
    if(!partial_trajectory.interpolate(HUBO_PATH_OPTIMAL))
    {
      break;
    }
    std::cout << "Size of partial: " << partial_trajectory.elements.size() << std::endl;

    if(r==0)
    {
      trajectory = partial_trajectory;
    }
    else
    {
      for(size_t k=0; k < partial_trajectory.size(); ++k)
        trajectory.elements.push_back(partial_trajectory.elements[k]);
    }
  }

  bool operate = true;
//  operate = false;

  if(operate)
  {
    if(!trajectory.check_limits())
    {
      std::cout << "Not sending walk trajectory, because it violates limits!" << std::endl;
      return 1;
    }

    mOperator.sendNewTrajectory(trajectory);
  }
  else
  {
    if(!trajectory.check_limits())
    {
      std::cout << "Walk trajectory violates limits!" << std::endl;
    }

    std::vector<Eigen::VectorXd> fullwalk;

    std::cout << "Converting back to trajectory (" << trajectory.size() << ")..."
              << std::endl;
    for(size_t i=0; i < trajectory.size(); ++i)
    {
      Eigen::VectorXd q(robot->getNumDofs());
      for(size_t j=0; j < 6; ++j)
        q[j] = 0.0;

      const IndexArray& indexMap = mOperator.getIndexMap();
      for(size_t j=6; j < robot->getNumDofs(); ++j)
        q[j] = trajectory[i].references[indexMap[j-6]];

      fullwalk.push_back(q);
    }
    std::cout << "... converted!" << std::endl;

    auto world = std::make_shared<World>();
    world->addSkeleton(robot);

    osg::ref_ptr<TrajectoryDisplayWorld> display =
        new TrajectoryDisplayWorld(world, fullwalk);

    osgDart::Viewer viewer;
    viewer.addWorldNode(display);
    viewer.allowSimulation(false);

    viewer.addAttachment(new osgDart::SupportPolygonVisual(robot, -0.97+0.02));

    viewer.setUpViewInWindow(0, 0, 1280, 960);

    // Set up the default viewing position
    viewer.getCameraManipulator()->setHomePosition(osg::Vec3( 5.34,  3.00, 1.00),
                                                   osg::Vec3( 0.00,  0.00, 0.00),
                                                   osg::Vec3(-0.20, -0.08, 0.98));

    // Reset the camera manipulator so that it starts in the new viewing position
    viewer.setCameraManipulator(viewer.getCameraManipulator());
    std::cout << "Launching viewer" << std::endl;
    viewer.run();
  }
}


// Copyright (c) 2017 - 2019 - The SmartCash Developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "consensus/consensus.h"
#include "init.h"
#include "smartrewards/rewards.h"
#include "smartrewards/rewardspayments.h"
#include "smarthive/hive.h"
#include "smartnode/spork.h"
#include "smartnode/smartnodepayments.h"
#include "ui_interface.h"
#include "validation.h"

#include <boost/thread.hpp>
#include <boost/range/irange.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>

CSmartRewards *prewards = NULL;

CCriticalSection cs_rewardsdb;
CCriticalSection cs_rewardrounds;

// Used for time conversions.
boost::posix_time::ptime epoch(boost::gregorian::date(1970, 1, 1));

// Estimate or return the current block height.
int GetBlockHeight(const CBlockIndex *index)
{
    int64_t syncDiff = std::time(0) - index->GetBlockTime();
    int64_t firstTxDiff;
    if( MainNet() ) firstTxDiff = std::time(0) - nStartRewardTime; // Diff from the reward blocks start till now on mainnet.
    else firstTxDiff = std::time(0) - nFirstTxTimestamp_Testnet; // Diff from the first transaction till now on testnet.
    return syncDiff > 1200 ? firstTxDiff / 55 : index->nHeight; // If we are 20 minutes near now use the current height.
}

int ParseScript(const CScript &script, std::vector<CSmartAddress> &ids){

    std::vector<CTxDestination> addresses;
    txnouttype type;
    int nRequired;

    if (!ExtractDestinations(script, type, addresses, nRequired)) {
        return 0;
    }

    BOOST_FOREACH(const CTxDestination &d, addresses)
    {
        ids.push_back(CSmartAddress(d));
    }

    return nRequired;
}

void CalculateRewardRatio(CSmartRewardRound &round)
{
    int64_t time = GetTime();
    int64_t start = round.startBlockHeight;
    round.rewards = 0;

    while( start <= round.endBlockHeight) round.rewards += GetBlockValue(start++,0,time) * 0.15;

    CAmount nCalcSmart = round.eligibleSmart - round.disqualifiedSmart;

    if( nCalcSmart ){
        round.percent = double(round.rewards) / nCalcSmart;
    }else{
        round.percent = 0;
    }
}

bool CSmartRewards::Verify()
{
    LOCK(cs_rewardsdb);
    return pdb->Verify(rewardHeight);
}

void CSmartRewards::UpdatePayoutParameter(CSmartRewardRound &round)
{

    int64_t nPayeeCount = round.eligibleEntries - round.disqualifiedEntries;
    int nFirst_1_3_Round = Params().GetConsensus().nRewardsFirst_1_3_Round;

    if( round.number < nFirst_1_3_Round ){
        round.nBlockPayees  = Params().GetConsensus().nRewardsPayouts_1_2_BlockPayees;
        round.nBlockInterval = Params().GetConsensus().nRewardsPayouts_1_2_BlockInterval;
    }else if( nPayeeCount ){

        int64_t nBlockStretch = Params().GetConsensus().nRewardsPayouts_1_3_BlockStretch;
        int64_t nBlocksPerRound = Params().GetConsensus().nRewardsBlocksPerRound_1_3;
        int64_t nBlockPayees = Params().GetConsensus().nRewardsPayouts_1_3_BlockPayees;

        round.nBlockPayees = std::max<int>(nBlockPayees, (nPayeeCount / nBlockStretch * nBlockPayees) + 1);

        if( nPayeeCount > round.nBlockPayees ){

            int64_t nStartDelayBlocks = Params().GetConsensus().nRewardsPayoutStartDelay;
            int64_t nBlocksTarget = nStartDelayBlocks + nBlocksPerRound;
            round.nBlockInterval = ((nBlockStretch * round.nBlockPayees) / nPayeeCount) + 1;
            int64_t nStretchedLength = nPayeeCount / round.nBlockPayees * (round.nBlockInterval);

            if( nStretchedLength > nBlocksTarget ){
                round.nBlockInterval--;
            }else if( nStretchedLength < nBlockStretch ){
                round.nBlockInterval++;
            }

        }else{
            // If its only one block to pay
            round.nBlockInterval = 1;
        }

    }else{
        // If there are no eligible smartreward entries
        round.nBlockPayees = 0;
        round.nBlockInterval = 0;
    }

}

void CSmartRewards::EvaluateRound(CSmartRewardRound &current, CSmartRewardRound &next, CSmartRewardEntryList &entries, CSmartRewardRoundResultList &results)
{
    LOCK(cs_rewardsdb);
    results.clear();

    UpdatePayoutParameter(current);

    int nFirst_1_3_Round = Params().GetConsensus().nRewardsFirst_1_3_Round;

    CAmount nReward;

    BOOST_FOREACH(CSmartRewardEntry &entry, entries) {

        if( current.number ){

            if( current.number < nFirst_1_3_Round ){
                nReward = entry.balanceEligible > 0 && entry.disqualifyingTx.IsNull() ? CAmount(entry.balanceEligible * current.percent) : 0;
            }else{
                nReward = entry.IsEligible() ? CAmount(entry.balanceEligible * current.percent) : 0;
            }

            results.push_back(CSmartRewardRoundResult(entry, nReward));
        }

        entry.balanceAtStart = entry.balance;

        if( entry.balance >= SMART_REWARDS_MIN_BALANCE && !SmartHive::IsHive(entry.id) ){
            entry.balanceEligible = entry.balance;
        }

        // Reset outgoing transaction with every cycle.
        entry.disqualifyingTx.SetNull();
        // Reset SmartNode payment tx with every cycle in case a node was shut down during the cycle.
        entry.smartnodePaymentTx.SetNull();
        // Reset the vote proof tx with every cycle to force a new vote for eligibility
        entry.voteProof.SetNull();

        if( next.number < nFirst_1_3_Round && entry.balanceEligible ){
            ++next.eligibleEntries;
            next.eligibleSmart += entry.balanceEligible;
        }
    }
}

bool CSmartRewards::StartFirstRound(const CSmartRewardRound &first, const CSmartRewardEntryList &entries)
{
    LOCK(cs_rewardsdb);
    return pdb->StartFirstRound(first,entries);
}

bool CSmartRewards::FinalizeRound(const CSmartRewardRound &current, const CSmartRewardRound &next, const CSmartRewardEntryList &entries, const CSmartRewardRoundResultList &results)
{
    LOCK(cs_rewardsdb);
    return pdb->FinalizeRound(current, next, entries, results);
}

bool CSmartRewards::UndoFinalizeRound(const CSmartRewardRound &current, const CSmartRewardRoundResultList &results)
{
    LOCK(cs_rewardsdb);
    return pdb->UndoFinalizeRound(current, results);
}

bool CSmartRewards::GetRewardRoundResults(const int16_t round, CSmartRewardRoundResultList &results)
{
    LOCK(cs_rewardsdb);
    return pdb->ReadRewardRoundResults(round, results);
}

bool CSmartRewards::GetRewardPayouts(const int16_t round, CSmartRewardRoundResultList &payouts)
{
    LOCK(cs_rewardsdb);
    return pdb->ReadRewardPayouts(round, payouts);
}

bool CSmartRewards::GetRewardPayouts(const int16_t round, CSmartRewardRoundResultPtrList &payouts)
{
    LOCK(cs_rewardsdb);
    return pdb->ReadRewardPayouts(round, payouts);
}

bool CSmartRewards::GetCachedRewardEntry(const CSmartAddress &id, CSmartRewardEntry *&entry)
{
    LOCK(cs_rewardsdb);

    // Return the entry if its already in cache.
    auto findResult = rewardEntries.find(id);

    if( findResult != rewardEntries.end() ){
        entry = findResult->second;
        return true;
    }

    return false;
}

bool CSmartRewards::ReadRewardEntry(const CSmartAddress &id, CSmartRewardEntry &entry)
{
    LOCK(cs_rewardsdb);
    return pdb->ReadRewardEntry(id,entry);
}

bool CSmartRewards::GetRewardEntry(const CSmartAddress &id, CSmartRewardEntry &entry)
{
    LOCK(cs_rewardsdb);

    CSmartRewardEntry * pReadEntry;

    if( GetCachedRewardEntry(id,pReadEntry) ){
        entry = *pReadEntry;
        return true;
    }

    return ReadRewardEntry(id,entry);
}

bool CSmartRewards::GetRewardEntries(CSmartRewardEntryList &entries)
{
    LOCK(cs_rewardsdb);
    return pdb->ReadRewardEntries(entries);
}

bool CSmartRewards::SyncCached(bool fUndo)
{
    return SyncCached(CSmartRewardBlock(), fUndo);
}

bool CSmartRewards::SyncCached(const CSmartRewardBlock &block, bool fUndo)
{
    LOCK2(cs_rewardsdb, cs_rewardrounds);

    bool ret =  pdb->SyncCached(block, currentRound, rewardEntries, transactionEntries, fUndo);

    for( std::pair<CSmartAddress, CSmartRewardEntry*> it : rewardEntries ){
        delete it.second;
    }

    rewardEntries.clear();
    transactionEntries.clear();

    return ret;
}

bool CSmartRewards::IsSynced()
{
    int nSyncDistance = MainNet() ? nRewardsSyncDistance : nRewardsSyncDistance_Testnet;
    return (chainHeight - rewardHeight) <= nSyncDistance - 1;
}

double CSmartRewards::GetProgress()
{
    int nSyncDistance = MainNet() ? nRewardsSyncDistance : nRewardsSyncDistance_Testnet;
    double progress = chainHeight > nSyncDistance ? double(rewardHeight) / double(chainHeight - nSyncDistance) : 0.0;
    return progress > 1 ? 1 : progress;
}

int CSmartRewards::GetLastHeight()
{
    return rewardHeight;
}

int CSmartRewards::GetBlocksPerRound(const int nRound)
{
    LOCK(cs_rewardrounds);

    const CChainParams& chainparams = Params();

    if( nRound < chainparams.GetConsensus().nRewardsFirst_1_3_Round ){
        return chainparams.GetConsensus().nRewardsBlocksPerRound_1_2;
    }else{
        return chainparams.GetConsensus().nRewardsBlocksPerRound_1_3;
    }

}

void CSmartRewards::AddTransaction(const CSmartRewardTransaction &transaction)
{
    LOCK(cs_rewardsdb);
    transactionEntries.push_back(transaction);
}

CSmartRewards::CSmartRewards(CSmartRewardsDB *prewardsdb)  : pdb(prewardsdb)
{
    LOCK(cs_rewardsdb);

    // Get the last written block of the rewards database.
    if(!pdb->ReadLastBlock(currentBlock)){
        // If there is no one available yet
        // Use 0 to get 1 as start below.
        currentBlock.nHeight = 0;
        currentBlock.blockHash = uint256();
        currentBlock.blockTime = 0;
    }

    pdb->ReadRounds(finishedRounds);

    std::sort(finishedRounds.begin(), finishedRounds.end());

    if( finishedRounds.size() ){
        lastRound = finishedRounds.back();
    }

    pdb->ReadCurrentRound(currentRound);
}

void CSmartRewards::Lock()
{
    LOCK(cs_rewardsdb);
    pdb->Lock();
}

bool CSmartRewards::IsLocked()
{
    LOCK(cs_rewardsdb);
    return pdb->IsLocked();
}

bool CSmartRewards::GetLastBlock(CSmartRewardBlock &block)
{
    LOCK(cs_rewardsdb);
    // Read the last block stored in the rewards database.
    return pdb->ReadLastBlock(block);
}

bool CSmartRewards::GetTransaction(const uint256 hash, CSmartRewardTransaction &transaction)
{
    // If the transaction is already in the cache use this one.
    BOOST_FOREACH(CSmartRewardTransaction t, transactionEntries) {
        if(t.hash == hash){
            transaction = t;
            return true;
        }
    }

    return pdb->ReadTransaction(hash, transaction);
}

const CSmartRewardRound& CSmartRewards::GetCurrentRound()
{
    return currentRound;
}

const CSmartRewardRound& CSmartRewards::GetLastRound()
{
    return lastRound;
}

const CSmartRewardRoundList& CSmartRewards::GetRewardRounds()
{
    return finishedRounds;
}

void CSmartRewards::UpdateHeights(const int nHeight, const int nRewardHeight)
{
    chainHeight = nHeight;
    rewardHeight = nRewardHeight;
}

void CSmartRewards::StartBlock()
{
    rewardEntries.clear();
    transactionEntries.clear();
}

void CSmartRewards::ProcessTransaction(CBlockIndex* pIndex, const CTransaction& tx, CCoinsViewCache& coins, const CChainParams& chainparams, CSmartRewardsUpdateResult &result)
{
    LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - %s", tx.GetHash().ToString());

    CSmartRewardEntry *rEntry = nullptr;
    int nFirst_1_3_Round = chainparams.GetConsensus().nRewardsFirst_1_3_Round;

    int nCurrentRound;

    {
        LOCK(cs_rewardrounds);
        nCurrentRound = currentRound.number;
    }

    int nHeight = pIndex->nHeight;

    if(nHeight > sporkManager.GetSporkValue(SPORK_15_SMARTREWARDS_BLOCKS_ENABLED)){
        return;
    }

    int nTime1 = GetTimeMicros();

    CSmartRewardTransaction testTx;

    // First check if the transaction hash did already come up in the past.
    if( GetTransaction(tx.GetHash(), testTx) ){

        // If yes we want to ignore it! There are some double appearing transactions in the history due to zerocoin exploits.
        LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - [%s] Double appearance! First in %d - Now in %d\n", testTx.hash.ToString(), testTx.blockHeight, pIndex->nHeight);
        return;

    }else{
        // If not save add it to the database.
        AddTransaction(CSmartRewardTransaction(pIndex->nHeight, tx.GetHash()));
    }

    CSmartAddress *voteProofCheck = nullptr;
    CAmount nVoteProofIn = 0;
    unsigned char cProofOption = 0;

    // No reason to check the input here for new coins.
    if( !tx.IsCoinBase() ){

        BOOST_FOREACH(const CTxIn &in, tx.vin) {

            if( in.scriptSig.IsZerocoinSpend() ) continue;

            const Coin &coin = coins.AccessCoin(in.prevout);
            const CTxOut &rOut = coin.out;

            std::vector<CSmartAddress> ids;

            int required = ParseScript(rOut.scriptPubKey ,ids);

            if( !required || required > 1 || ids.size() > 1 ){
                LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - Process Inputs: Could't parse CSmartAddress: %s\n",rOut.ToString());
                continue;
            }

            if(!GetCachedRewardEntry(ids.at(0),rEntry)){

                rEntry = new CSmartRewardEntry(ids.at(0));

                if(!ReadRewardEntry(rEntry->id, *rEntry)){
                    delete rEntry;
                    LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - Spend without previous receive - %s", tx.ToString());
                    continue;
                }

                rewardEntries.insert(make_pair(ids.at(0), rEntry));
            }

            // If its a voteproof transaction not instantly make the
            // balance ineligible. First check if the change is sent back
            // to the address or not to avoid exploiting fund sending
            // with voteproof transactions
            if( nCurrentRound >= nFirst_1_3_Round && tx.IsVoteProof() && voteProofCheck == nullptr ){
                voteProofCheck = new CSmartAddress(rEntry->id);
                nVoteProofIn += rOut.nValue;
            }

            rEntry->balance -= rOut.nValue;

            // If its a voteproof transaction not instantly make the
            // balance ineligible. First check if the change is sent back
            // to the address or not to avoid exploiting fund sending
            // with voteproof transactions
            if( nCurrentRound >= nFirst_1_3_Round && voteProofCheck == nullptr && rEntry->disqualifyingTx.IsNull() ){

                if( rEntry->IsEligible() ){
                    result.disqualifiedEntries++;
                    result.disqualifiedSmart += rEntry->balanceEligible;
                }

                rEntry->disqualifyingTx = tx.GetHash();

            }else if( nCurrentRound < nFirst_1_3_Round && rEntry->disqualifyingTx.IsNull() ){

                rEntry->disqualifyingTx = tx.GetHash();

                if( rEntry->balanceEligible ){
                    result.disqualifiedEntries++;
                    result.disqualifiedSmart += rEntry->balanceEligible;
                }

            }

            if(rEntry->balance < 0 ){
                LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - Negative amount?! - %s", rEntry->ToString());
                rEntry->balance = 0;
            }

        }
    }

    int nTime2 = GetTimeMicros();

    BOOST_FOREACH(const CTxOut &out, tx.vout) {

        if(out.scriptPubKey.IsZerocoinMint() ) continue;

        std::vector<CSmartAddress> ids;
        int required = ParseScript(out.scriptPubKey ,ids);

        if( !required || required > 1 || ids.size() > 1 ){
            LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - Process Outputs: Could't parse CSmartAddress: %s\n",out.ToString());
            continue;
        }else{

            if(!GetCachedRewardEntry(ids.at(0),rEntry)){
                rEntry = new CSmartRewardEntry(ids.at(0));
                ReadRewardEntry(rEntry->id, *rEntry);
                rewardEntries.insert(make_pair(ids.at(0), rEntry));
            }

            if( voteProofCheck ){

                if( !out.IsVoteProofData() &&
                    !(*voteProofCheck == rEntry->id) ){

                    CSmartRewardEntry *vkEntry = nullptr;

                    if(!GetCachedRewardEntry(*voteProofCheck,vkEntry)){
                        vkEntry = new CSmartRewardEntry(*voteProofCheck);
                        ReadRewardEntry(vkEntry->id, *vkEntry);
                        rewardEntries.insert(make_pair(vkEntry->id, vkEntry));
                    }

                    if( vkEntry->IsEligible() ){
                        result.disqualifiedEntries++;
                        result.disqualifiedSmart += vkEntry->balanceEligible;
                    }

                    // Finally invalidate the balance since the change was not sent
                    // back to the sender! We don't want to allow
                    // a exploit to send around funds withouht breaking smartrewards.
                    vkEntry->disqualifyingTx = tx.GetHash();

                }else if( !out.IsVoteProofData() &&
                          (*voteProofCheck == rEntry->id) ){

                    CSmartRewardEntry *proofEntry = nullptr;
                    unsigned char cAddressType = 0;
                    uint32_t nProofRound;
                    uint160 addressHash;
                    uint256 nProposalHash; // Placeholder only for now
                    CSmartAddress proofAddress;

                    BOOST_FOREACH(const CTxOut &outData, tx.vout) {

                        if( outData.IsVoteProofData() ){

                            std::vector<unsigned char> scriptData;
                            scriptData.insert(scriptData.end(), outData.scriptPubKey.begin() + 3, outData.scriptPubKey.end());
                            CDataStream ss(scriptData, SER_NETWORK, 0);

                            ss >> cProofOption;
                            ss >> nProofRound;
                            ss >> nProposalHash;

                            if( cProofOption == 0x01 &&
                                voteProofCheck->ToString(false) != Params().GetConsensus().strRewardsGlobalVoteProofAddress){
                                proofAddress = *voteProofCheck;
                                proofEntry = rEntry;
                            }else if( cProofOption == 0x02 &&
                                      voteProofCheck->ToString(false) == Params().GetConsensus().strRewardsGlobalVoteProofAddress ){

                                ss >> cAddressType;
                                ss >> addressHash;

                                if( cAddressType == 0x01 ){
                                    proofAddress = CSmartAddress(CKeyID(addressHash));
                                }else if( cAddressType == 0x02){
                                    proofAddress = CSmartAddress(CScriptID(addressHash));
                                }else{
                                    proofAddress = CSmartAddress(); // Invalid address type
                                }

                                if(!GetCachedRewardEntry(proofAddress, proofEntry)){
                                    proofEntry = new CSmartRewardEntry(proofAddress);
                                    ReadRewardEntry(proofEntry->id, *proofEntry);
                                    rewardEntries.insert(make_pair(proofEntry->id, proofEntry));
                                }

                            }else{
                                proofAddress = CSmartAddress(); // Invalid option
                            }
                        }
                    }

                    if( proofAddress.IsValid() && proofEntry != nullptr && !SmartHive::IsHive(*voteProofCheck) ){

                        if( cProofOption == 0x01 && proofEntry->balanceEligible ){
                            proofEntry->balanceEligible -= nVoteProofIn - tx.GetValueOut();

                            if( proofEntry->balanceEligible < 0 ){
                                proofEntry->balanceEligible = 0;
                            }
                        }

                        if( proofEntry->voteProof.IsNull() ){

                            if( nProofRound == currentRound.number ){
                                proofEntry->voteProof = tx.GetHash();
                            }

                            // If the entry is eligible now after the vote proof update the results
                            if( proofEntry->IsEligible() ){
                                result.qualifiedEntries++;
                                result.qualifiedSmart += proofEntry->balanceEligible;
                            }

                        }
                    }
                }

                delete voteProofCheck;
            }

            rEntry->balance += out.nValue;

            // If we are in the 1.3 cycles check for node rewards to remove node addresses from lists
            if( nCurrentRound >= nFirst_1_3_Round && tx.IsCoinBase() ){

                int nInterval = SmartNodePayments::PayoutInterval(nHeight);
                int nPayoutsPerBlock = SmartNodePayments::PayoutsPerBlock(nHeight);
                // Just to avoid potential zero divisions
                nPayoutsPerBlock = std::max(1,nPayoutsPerBlock);

                CAmount nNodeReward = SmartNodePayments::Payment(nHeight) / nPayoutsPerBlock;

                // If we have an interval check if this is a node payout block
                if( nInterval && !(nHeight % nInterval) ){

                    // If the amount matches and the entry is not yet marked as node do it
                    if( abs(out.nValue - nNodeReward ) < 2 ){

                        if( rEntry->smartnodePaymentTx.IsNull() ){

                            // If it is currently eligible adjust the round's results
                            if( rEntry->IsEligible() ){
                                ++result.disqualifiedEntries;
                                result.disqualifiedSmart += rEntry->balanceEligible;
                            }

                            rEntry->smartnodePaymentTx = tx.GetHash();
                        }
                    }
                }
            }
        }
    }

    int nTime3 = GetTimeMicros();
    int nTimeTx = nTime3 - nTime1;

    if( LogAcceptCategory("smartrewards-tx") ){
        LogPrint("smartrewards-tx", "CSmartRewards::ProcessTransaction - TX %s - %.2fms\n",HexStr(tx.GetHash()), nTimeTx * 0.001);
        LogPrint("smartrewards-tx", " inputs - %.2fms\n", (nTime2 - nTime1) * 0.001);
        LogPrint("smartrewards-tx", " outputs - %.2fms\n", (nTime3 - nTime2) * 0.001);
    }
}

void CSmartRewards::UndoTransaction(CBlockIndex* pIndex, const CTransaction& tx, CCoinsViewCache& coins, const CChainParams& chainparams, CSmartRewardsUpdateResult &result)
{
    LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - %s", tx.GetHash().ToString());

    CSmartRewardEntry *rEntry = nullptr;
    int nFirst_1_3_Round = chainparams.GetConsensus().nRewardsFirst_1_3_Round;

    int nCurrentRound;

    {
        LOCK(cs_rewardrounds);
        nCurrentRound = currentRound.number;
    }

    int nHeight = pIndex->nHeight;

    if(nHeight > sporkManager.GetSporkValue(SPORK_15_SMARTREWARDS_BLOCKS_ENABLED)){
        return;
    }

    int nTime1 = GetTimeMicros();

    CSmartRewardTransaction testTx;

    // First check if the transaction hash did already come up in the past.
    if( GetTransaction(tx.GetHash(), testTx) && testTx.blockHeight == pIndex->nHeight ){
        AddTransaction(testTx);
    }else{
        return;
    }

    CSmartAddress *voteProofCheck = nullptr;
    unsigned char cProofOption = 0;
    CAmount nVoteProofIn = 0, nVoteProofOut = 0;

    if( nCurrentRound >= nFirst_1_3_Round && tx.IsVoteProof() ){

        const Coin &coin = coins.AccessCoin(tx.vin[0].prevout);
        const CTxOut &rOut = coin.out;

        std::vector<CSmartAddress> ids;

        int required = ParseScript(rOut.scriptPubKey ,ids);

        if( !required || required > 1 || ids.size() > 1 ){
            LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - Process VoteProof: Could't parse CSmartAddress: %s\n",rOut.ToString());
            return;
        }

        nVoteProofIn = rOut.nValue;
        voteProofCheck = new CSmartAddress(ids[0]);
    }

    BOOST_REVERSE_FOREACH(const CTxOut &out, tx.vout) {

        if(out.scriptPubKey.IsZerocoinMint() ) continue;

        std::vector<CSmartAddress> ids;
        int required = ParseScript(out.scriptPubKey ,ids);

        if( !required || required > 1 || ids.size() > 1 ){
            LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - Process Outputs: Could't parse CSmartAddress: %s\n",out.ToString());
            continue;
        }else{

            if(!GetCachedRewardEntry(ids.at(0),rEntry)){
                rEntry = new CSmartRewardEntry(ids.at(0));
                ReadRewardEntry(rEntry->id, *rEntry);
                rewardEntries.insert(make_pair(ids.at(0), rEntry));
            }

            if( voteProofCheck ){

                nVoteProofOut += out.nValue;

                if( !out.IsVoteProofData() &&
                    !(*voteProofCheck == rEntry->id) ){

                    CSmartRewardEntry *vkEntry = nullptr;

                    if(!GetCachedRewardEntry(*voteProofCheck,vkEntry)){
                        vkEntry = new CSmartRewardEntry(*voteProofCheck);
                        ReadRewardEntry(vkEntry->id, *vkEntry);
                        rewardEntries.insert(make_pair(vkEntry->id, vkEntry));
                    }

                    if( vkEntry->disqualifyingTx == tx.GetHash() ){

                        vkEntry->disqualifyingTx.SetNull();

                        if( vkEntry->IsEligible() ){
                            --result.disqualifiedEntries;
                            result.disqualifiedSmart -= vkEntry->balanceEligible;
                        }
                    }

                }else if( !out.IsVoteProofData() &&
                         (*voteProofCheck == rEntry->id) ){

                    CSmartRewardEntry *proofEntry = nullptr;
                    unsigned char cAddressType = 0;
                    uint32_t nProofRound;
                    uint160 addressHash;
                    uint256 nProposalHash; // Placeholder only for now
                    CSmartAddress proofAddress;

                    BOOST_FOREACH(const CTxOut &outData, tx.vout) {

                        if( outData.IsVoteProofData() ){

                            std::vector<unsigned char> scriptData;
                            scriptData.insert(scriptData.end(), outData.scriptPubKey.begin() + 3, outData.scriptPubKey.end());
                            CDataStream ss(scriptData, SER_NETWORK, 0);

                            ss >> cProofOption;
                            ss >> nProofRound;
                            ss >> nProposalHash;

                            if( cProofOption == 0x01 &&
                                voteProofCheck->ToString(false) != Params().GetConsensus().strRewardsGlobalVoteProofAddress){
                                proofAddress = *voteProofCheck;
                                proofEntry = rEntry;
                            }else if( cProofOption == 0x02 &&
                                      voteProofCheck->ToString(false) == Params().GetConsensus().strRewardsGlobalVoteProofAddress ){

                                ss >> cAddressType;
                                ss >> addressHash;

                                if( cAddressType == 0x01 ){
                                    proofAddress = CSmartAddress(CKeyID(addressHash));
                                }else if( cAddressType == 0x02){
                                    proofAddress = CSmartAddress(CScriptID(addressHash));
                                }else{
                                    proofAddress = CSmartAddress(); // Invalid address type
                                }

                                if(!GetCachedRewardEntry(proofAddress, proofEntry)){
                                    proofEntry = new CSmartRewardEntry(proofAddress);
                                    ReadRewardEntry(proofEntry->id, *proofEntry);
                                    rewardEntries.insert(make_pair(proofEntry->id, proofEntry));
                                }

                            }else{
                                proofAddress = CSmartAddress(); // Invalid option
                            }
                        }
                    }

                    if( proofAddress.IsValid() && proofEntry != nullptr && !SmartHive::IsHive(*voteProofCheck) ){

                        if( proofEntry->voteProof == tx.GetHash() ){

                            proofEntry->voteProof.SetNull();

                            --result.qualifiedEntries;
                            result.qualifiedSmart -= proofEntry->balanceEligible;

                            if( cProofOption == 0x01 ){
                                proofEntry->balanceEligible += nVoteProofIn - nVoteProofOut;
                            }

                        }
                    }
                }

                delete voteProofCheck;
            }

            rEntry->balance -= out.nValue;

            // If we are in the 1.3 cycles check for node rewards to remove node addresses from lists
            if( nCurrentRound >= nFirst_1_3_Round && tx.IsCoinBase() ){

                if( rEntry->smartnodePaymentTx == tx.GetHash() ){

                    rEntry->smartnodePaymentTx.SetNull();

                    // If it is eligible now adjust the round's results
                    if( rEntry->IsEligible() ){
                        --result.disqualifiedEntries;
                        result.disqualifiedSmart -= rEntry->balanceEligible;
                    }
                }
            }
        }
    }

    int nTime2 = GetTimeMicros();

    // No reason to check the input here for new coins.
    if( !tx.IsCoinBase() ){

        BOOST_REVERSE_FOREACH(const CTxIn &in, tx.vin) {

            if( in.scriptSig.IsZerocoinSpend() ) continue;

            const Coin &coin = coins.AccessCoin(in.prevout);
            const CTxOut &rOut = coin.out;

            std::vector<CSmartAddress> ids;

            int required = ParseScript(rOut.scriptPubKey ,ids);

            if( !required || required > 1 || ids.size() > 1 ){
                LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - Process Inputs: Could't parse CSmartAddress: %s\n",rOut.ToString());
                continue;
            }

            if(!GetCachedRewardEntry(ids.at(0),rEntry)){

                rEntry = new CSmartRewardEntry(ids.at(0));

                if(!ReadRewardEntry(rEntry->id, *rEntry)){
                    delete rEntry;
                    LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - Spend without previous receive - %s", tx.ToString());
                    continue;
                }

                rewardEntries.insert(make_pair(ids.at(0), rEntry));
            }

            rEntry->balance += rOut.nValue;

            // If its a voteproof transaction not instantly make the
            // balance ineligible. First check if the change is sent back
            // to the address or not to avoid exploiting fund sending
            // with voteproof transactions
            if( nCurrentRound >= nFirst_1_3_Round && voteProofCheck == nullptr && rEntry->disqualifyingTx == tx.GetHash() ){

                rEntry->disqualifyingTx.SetNull();

                if( rEntry->IsEligible() ){
                    --result.disqualifiedEntries;
                    result.disqualifiedSmart -= rEntry->balanceEligible;
                }

            }else if( nCurrentRound < nFirst_1_3_Round && rEntry->disqualifyingTx == tx.GetHash() ){

                rEntry->disqualifyingTx.SetNull();

                if( rEntry->balanceEligible ){
                    --result.disqualifiedEntries;
                    result.disqualifiedSmart -= rEntry->balanceEligible;
                }

            }

            if(rEntry->balance < 0 ){
                LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - Negative amount?! - %s", rEntry->ToString());
                rEntry->balance = 0;
            }

        }
    }

    int nTime3 = GetTimeMicros();
    int nTimeTx = nTime3 - nTime1;

    if( LogAcceptCategory("smartrewards-tx") ){
        LogPrint("smartrewards-tx", "CSmartRewards::UndoTransaction - TX %s - %.2fms\n",HexStr(tx.GetHash()), nTimeTx * 0.001);
        LogPrint("smartrewards-tx", " outputs - %.2fms\n", (nTime2 - nTime1) * 0.001);
        LogPrint("smartrewards-tx", " inputs - %.2fms\n", (nTime3 - nTime2) * 0.001);
    }
}

bool CSmartRewards::CommitBlock(CBlockIndex* pIndex, const CSmartRewardsUpdateResult& result)
{
    int nTime1 = GetTimeMicros();

    if(pIndex && pIndex->nHeight > sporkManager.GetSporkValue(SPORK_15_SMARTREWARDS_BLOCKS_ENABLED)){
        return true;
    }

    if(!pIndex || pIndex->nHeight != currentBlock.nHeight + 1 ){
        LogPrintf("CSmartRewards::CommitBlock - Invalid next block!");
        return false;
    }

    if(!SyncCached(result.block, false)){
        LogPrintf("CSmartRewards::CommitBlock - Failed to sync cache - Initial!");
        return false;
    }

    // Update the current block to the processed one
    currentBlock = result.block;

    // For the first round we have special parameter..
    if( !currentRound.number ){

        if( (MainNet() && pIndex->GetBlockTime() > nFirstRoundStartTime) ||
            (TestNet() && pIndex->nHeight >= nFirstRoundStartBlock_Testnet) ){

            // Create the very first smartrewards round.
            CSmartRewardRound first;
            first.number = 1;
            first.startBlockTime = pIndex->GetBlockTime();
            first.startBlockHeight = MainNet() ? nFirstRoundStartBlock : nFirstRoundStartBlock_Testnet;
            first.endBlockTime = MainNet() ? nFirstRoundEndTime : nFirstRoundEndTime_Testnet;
            // Estimate the block, gets updated on the end of the round to the real one.
            first.endBlockHeight = MainNet() ? nFirstRoundEndBlock : nFirstRoundEndBlock_Testnet;

            CSmartRewardEntryList entries;
            CSmartRewardRoundResultList results;

            // Get the current entries
            if( !GetRewardEntries(entries) ){
                LogPrintf("CSmartRewards::CommitBlock - Failed to read all reward entries!");
                return false;
            }

            // Evaluate the round and update the next rounds parameter.
            EvaluateRound(currentRound, first, entries, results );

            CalculateRewardRatio(first);

            if( !StartFirstRound(first, entries) ){
                LogPrintf("CSmartRewards::CommitBlock - Failed to finalize round!");
                return false;
            }

            currentRound = first;
        }

    }else if( result.disqualifiedEntries || result.disqualifiedSmart ||
              result.qualifiedEntries || result.qualifiedSmart ){

        // If there were disqualification during the last block processing
        // update the current round stats.

        currentRound.disqualifiedEntries += result.disqualifiedEntries;
        currentRound.disqualifiedSmart += result.disqualifiedSmart;

        currentRound.eligibleEntries += result.qualifiedEntries;
        currentRound.eligibleSmart += result.qualifiedSmart;

        CalculateRewardRatio(currentRound);
    }

    // If just hit the next round threshold
    if( ( MainNet() && currentRound.number < nRewardsFirstAutomatedRound - 1 && pIndex->GetBlockTime() > currentRound.endBlockTime ) ||
        ( ( TestNet() || currentRound.number >= nRewardsFirstAutomatedRound - 1 ) && pIndex->nHeight == currentRound.endBlockHeight ) ){

        // Write the round to the history
        currentRound.endBlockHeight = pIndex->nHeight;
        currentRound.endBlockTime = pIndex->GetBlockTime();

        CSmartRewardEntryList entries;
        CSmartRewardRoundResultList results;

        // Create the next round.
        CSmartRewardRound next;
        next.number = currentRound.number + 1;
        next.startBlockTime = currentRound.endBlockTime;
        next.startBlockHeight = currentRound.endBlockHeight + 1;

        int nBlocksPerRound = GetBlocksPerRound(next.number);
        time_t startTime = (time_t)next.startBlockTime;

        if( MainNet() ){

            if( next.number == nRewardsFirstAutomatedRound - 1 ){
                // Let the round 12 end at height 574099 so that round 13 starts at 574100
                next.endBlockHeight = HF_V1_2_SMARTREWARD_HEIGHT - 1;
                next.endBlockTime = startTime + ( (next.endBlockHeight - next.startBlockHeight) * 55 );
            }else if(next.number < nRewardsFirstAutomatedRound){

                boost::gregorian::date endDate = boost::posix_time::from_time_t(startTime).date();

                endDate += boost::gregorian::months(1);
                // End date at 00:00:00 + 25200 seconds (7 hours) to match the date at 07:00 UTC
                next.endBlockTime = time_t((boost::posix_time::ptime(endDate, boost::posix_time::seconds(25200)) - epoch).total_seconds());
                next.endBlockHeight = next.startBlockHeight + ( (next.endBlockTime - next.startBlockTime) / 55 );
            }else{
                next.endBlockHeight = next.startBlockHeight + nBlocksPerRound - 1;
                next.endBlockTime = startTime + nBlocksPerRound * 55;
            }

        }else{
            next.endBlockHeight = next.startBlockHeight + nBlocksPerRound - 1;
            next.endBlockTime = startTime + nBlocksPerRound * 55;
        }

        // Get the current entries
        if( !GetRewardEntries(entries) ){
            LogPrintf("CSmartRewards::CommitBlock - Failed to read all reward entries!");
            return false;
        }

        CalculateRewardRatio(currentRound);

        // Evaluate the round and update the next rounds parameter.
        EvaluateRound(currentRound, next, entries, results);

        CalculateRewardRatio(next);

        if( !FinalizeRound(currentRound, next, entries, results) ){
            LogPrintf("CSmartRewards::CommitBlock - Failed to finalize round!");
            return false;
        }

        LOCK(cs_rewardrounds);

        finishedRounds.push_back(currentRound);
        lastRound = currentRound;
        currentRound = next;
    }

    if(!SyncCached()){
        LogPrintf("CSmartRewards::CommitBlock - Failed to sync cached - Processed!");
        return false;
    }

    prewards->UpdateHeights(GetBlockHeight(pIndex), currentBlock.nHeight);

    int nTime2 = GetTimeMicros();

    if( LogAcceptCategory("smartrewards-block") ){
            LogPrint("smartrewards-block", "Round %d - Block: %d - Progress %d%%\n",currentRound.number, currentBlock.nHeight, int(prewards->GetProgress() * 100));
            LogPrint("smartrewards-block", "  Commit block: %.2fms\n", (nTime2 - nTime1) * 0.001);
    }

    // If we are synced notify the UI on each new block.
    // If not notify the UI every nRewardsUISyncUpdateRate blocks to let it update the
    // loading screen.
    if( IsSynced() || !(currentBlock.nHeight % nRewardsUISyncUpdateRate) )
        uiInterface.NotifySmartRewardUpdate();

    return true;
}

bool CSmartRewards::CommitUndoBlock(CBlockIndex *pIndex, const CSmartRewardsUpdateResult &result)
{
    int nTime1 = GetTimeMicros();

    if(pIndex && pIndex->nHeight > sporkManager.GetSporkValue(SPORK_15_SMARTREWARDS_BLOCKS_ENABLED)){
        return true;
    }

    if(!pIndex || pIndex->nHeight != currentBlock.nHeight ){
        LogPrintf("CSmartRewards::CommitUndoBlock - Invalid next block!");
        return false;
    }

    if(!SyncCached(result.block, true)){
        LogPrintf("CSmartRewards::CommitUndoBlock - Failed to sync cache - Initial!");
        return false;
    }

    currentBlock = CSmartRewardBlock(pIndex->pprev->nHeight, pIndex->pprev->GetBlockHash(), pIndex->pprev->GetBlockTime() );

    if( result.disqualifiedEntries || result.disqualifiedSmart ||
        result.qualifiedEntries || result.qualifiedSmart ){

        // If there were disqualification during the last block processing
        // update the current round stats.

        currentRound.disqualifiedEntries += result.disqualifiedEntries;
        currentRound.disqualifiedSmart += result.disqualifiedSmart;

        currentRound.eligibleEntries += result.qualifiedEntries;
        currentRound.eligibleSmart += result.qualifiedSmart;

        CalculateRewardRatio(currentRound);
    }

    // If just hit the last round's threshold
    if( ( MainNet() && currentRound.number < nRewardsFirstAutomatedRound - 1 && pIndex->GetBlockTime() < currentRound.endBlockTime ) ||
            ( ( TestNet() || currentRound.number >= nRewardsFirstAutomatedRound - 1 ) && pIndex->nHeight == currentRound.startBlockHeight ) ){

        LOCK2(cs_rewardsdb, cs_rewardrounds);

        // Recover the last round from the history as current round
        currentRound = finishedRounds.back();
        finishedRounds.pop_back();

        lastRound = finishedRounds.back();

        CSmartRewardEntryList entries;
        CSmartRewardRoundResultList results;

        // Get the current entries
        if( !GetRewardRoundResults(currentRound.number, results) ){
            LogPrintf("CSmartRewards::CommitUndoBlock - Failed to read last round's results!");
            return false;
        }

        CalculateRewardRatio(currentRound);

        if( !UndoFinalizeRound(currentRound, results) ){
            LogPrintf("CSmartRewards::CommitUndoBlock - Failed to finalize round!");
            return false;
        }

    }

    if(!SyncCached(true)){
        LogPrintf("CSmartRewards::CommitUndoBlock - Failed to sync cached - Processed!");
        return false;
    }

    prewards->UpdateHeights(GetBlockHeight(pIndex), currentBlock.nHeight);

    int nTime2 = GetTimeMicros();

    if( LogAcceptCategory("smartrewards-block") ){
        LogPrint("smartrewards-block", "Round %d - Block: %d - Progress %d%%\n",currentRound.number, currentBlock.nHeight, int(prewards->GetProgress() * 100));
        LogPrint("smartrewards-block", "  Commit undo block: %.2fms\n", (nTime2 - nTime1) * 0.001);
    }

    // If we are synced notify the UI on each new block.
    // If not notify the UI every nRewardsUISyncUpdateRate blocks to let it update the
    // loading screen.
    if( IsSynced() || !(currentBlock.nHeight % nRewardsUISyncUpdateRate) )
        uiInterface.NotifySmartRewardUpdate();

    return true;
}


//Copyright(c) 2016 Shuda Li[lishuda1980@gmail.com]
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files(the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions :
//
//The above copyright notice and this permission notice shall be included in
//all copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
//FOR A PARTICULAR PURPOSE AND NON - INFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR
//COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.
//

#include <thrust/device_ptr.h>
#include <thrust/sort.h>

#define EXPORT
#define INFO
#define _USE_MATH_DEFINES
#define  NOMINMAX
#include <GL/glew.h>
#include <cuda.h>
#include <cuda_gl_interop.h>
#include <cuda_runtime_api.h>
//stl
#include <iostream>
#include <string>
#include <vector>
#include <limits>

#ifdef __gnu_linux__
#include <sys/types.h>
#include <sys/stat.h>
#elif _WIN32
#include <direct.h>
#else
#error "OS not supported!"
#endif

#include <math.h>
#include <limits>
//boost
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //isnan
#include <boost/lexical_cast.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/filesystem.hpp>

//openncv
#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudafilters.hpp>
#include <opencv2/cudafeatures2d.hpp>
#include <opencv2/cudawarping.hpp>

#include <se3.hpp>

#include <utility>
#include <OpenNI.h>
#include "Converters.hpp"
#include "GLUtil.hpp"
#include "CVUtil.hpp"
#include "EigenUtil.hpp"
#include "pcl/internal.h"
#include "Camera.h"
#include "IntrinsicAnalysis.cuh"
#include "KeyFrame.h"
#include "VideoSourceKinect.hpp"
#include "TSDF.h"

#include "KinfuTracker.h"
#include "CudaLib.cuh"

#include "DVOICP.cuh"
#include "DirectOrientation.cuh"
#include "ExposureEstimation.cuh"
#include "Utility.hpp"
#include "Kinect.h"
#include "pcl/vector_math.hpp"
#include "Converters.hpp"

using namespace cv;
using namespace cv::cuda;
using namespace std;
using namespace btl::kinect;
using namespace btl::utility;
using namespace btl::image;
using namespace btl::device;
//using namespace pcl::device;
using namespace Eigen;
using namespace Sophus;

namespace btl{ namespace geometry
{

CKinFuTracker::CKinFuTracker(CKeyFrame::tp_ptr pKeyFrame_, CTsdfBlock::tp_shared_ptr pGlobalMap_, int nResolution_/*=0*/, int nPyrHeight_/*=3*/)
	:_pGlobalMap(pGlobalMap_),_nResolution(nResolution_),_nPyrHeight(nPyrHeight_)
{
	_v_T_cw_tracking.clear();

	_aMinEnergy[0] = .5f;//6.f;
	_aMinEnergy[1] = .5f;//100.f;
	_aMinEnergy[2] = .1f;//2000.f;
	_aMinEnergy[3] = 1.f;// 2000.f;
	_aMinEnergy[4] = 2000.f;//1000.f;

}

bool CKinFuTracker::init(const CKeyFrame::tp_ptr pCurFrame_){
	_nCols = pCurFrame_->_agPyrPts[0]->cols;
	_nRows = pCurFrame_->_agPyrPts[0]->rows;

	ostringstream directory;
	directory << "..//data//" << _serial_number << "//crf.yml";
	if (!_pIAPrev)
		_pIAPrev.reset(new btl::device::CIntrinsicsAnalysisMult(directory.str(), _nRows, _nCols, .25, 40));
	if (!_pIACurr)
		_pIACurr.reset(new btl::device::CIntrinsicsAnalysisMult(directory.str(), _nRows, _nCols, .25, 40));
	GpuMat bgr;
	cuda::cvtColor(pCurFrame_->_gRGB, bgr, CV_RGB2BGR);
	_pIACurr->analysis(bgr);
	constructLivePyr();

	_pRefFrame.reset();
	_pRefFrame.reset(new CKeyFrame(pCurFrame_));	//copy pKeyFrame_ to _pPrevFrameWorld

	//integrate the frame into the world
	GpuMat radiance;
	cuda::merge(_pIACurr->_vRadianceBGR, radiance);
	GpuMat er;
	cuda::merge(_pIACurr->_error_bgr, 3, er);
	GpuMat normal_radiance;
	cuda::merge(_pIACurr->_normalized_bgr, 3, normal_radiance);
	_pGlobalMap->gpuIntegrateDepth(*pCurFrame_->_agPyrDepths[0], *pCurFrame_->_agPyrNls[0], normal_radiance, radiance, er,
									SE3Group<double>(pCurFrame_->_R_cw, pCurFrame_->_Tw));

	//initialize pose
	pCurFrame_->getPrjCfW(&_pose_refined_c_f_w);
	_v_T_cw_tracking.push_back(_pose_refined_c_f_w);

	//store current key point
	storeCurrFrameSynthetic(pCurFrame_);
	//storeCurrFrameReal(pCurFrame_);
	PRINTSTR("End of init");

	return true;
}

void CKinFuTracker::storeCurrFrameSynthetic(CKeyFrame::tp_ptr pCurFrame_){
	pCurFrame_->copyTo(&*_pRefFrame);
	_n_rad_origin_2_ref = _n_rad_live[2].clone();
	_pIACurr->copyTo(*_pIAPrev);

	if (_pRefFrame->_gRadiance.empty()){
		_pRefFrame->_gRadiance.create(_pRefFrame->_agPyrPts[0]->size(), CV_32FC3);
	}
	if (_pRefFrame->_gNR.empty()){
		_pRefFrame->_gNR.create(_pRefFrame->_agPyrPts[0]->size(), CV_32FC3);
	}

	_pGlobalMap->gpuRayCastingAll( _pRefFrame->_pRGBCamera->getIntrinsics(0), _pRefFrame->_R_cw, _pRefFrame->_Tw,
										&*_pRefFrame->_agPyrPts[0], &*_pRefFrame->_agPyrNls[0], &_pRefFrame->_gRadiance, &_pRefFrame->_gNR);
	cuda::split(_pRefFrame->_gNR, _pIAPrev->_normalized_bgr);
	cuda::split(_pRefFrame->_gRadiance, _pIAPrev->_vRadianceBGR);

	constructRefePyr();

	pCurFrame_->getPrjCfW(&_pose_refined_c_f_w);
	_v_T_cw_tracking.push_back(_pose_refined_c_f_w);

	return;
}

void CKinFuTracker::displayCameraPath() const{
	vector<Eigen::Affine3d>::const_iterator cit = _v_T_cw_tracking.begin(); //from world to camera

	glDisable(GL_LIGHTING);
	glColor3f ( 0.f,0.f,1.f);
	glLineWidth(2.f);
	glBegin(GL_LINE_STRIP);
	for (; cit != _v_T_cw_tracking.end(); cit++ )
	{
		SO3Group<double> mR = cit->linear();
		Vector3d vT = cit->translation();
		Vector3d vC = mR.inverse() *(-vT);
		vC = mR.inverse()*(-vT);
		glVertex3dv( vC.data() );
	}
	glEnd();
	return;
}

void CKinFuTracker::getPrevView( Eigen::Affine3d* pSystemPose_ ){
	int s = _v_T_cw_tracking.size();
	*pSystemPose_ = _v_T_cw_tracking[s-2];
	return;
}
void CKinFuTracker::getNextView( Eigen::Affine3d* pSystemPose_ ){
	*pSystemPose_ = _v_T_cw_tracking.front();
	PRINT(_v_T_cw_tracking.front().matrix());
	return;
}

double CKinFuTracker::dvoICPIC(const CKeyFrame::tp_ptr pRefeFrame_, CKeyFrame::tp_ptr pLiveFrame_, const short asICPIterations_[], SE3Group<double>* pT_rl_, Eigen::Vector4i* pActualIter_) const
{
	SE3Group<double> PrevT_rl = *pT_rl_;
	SE3Group<double> NewT_rl = *pT_rl_;
	//get R,T of previous
	Matrix3d R_rl_t_tmp = PrevT_rl.so3().inverse().matrix();
	const Matd33&  devR_rl = pcl::device::device_cast<pcl::device::Matd33> (R_rl_t_tmp); //implicit inverse

	Vector3d t_rl = PrevT_rl.translation();
	const double3& devT_rl = pcl::device::device_cast<double3> (t_rl);

	//from low resolution to high
	double dCurEnergy = numeric_limits<double>::max();
	for (short sPyrLevel = pLiveFrame_->pyrHeight() - 1; sPyrLevel >= 0; sPyrLevel--){
		// for each pyramid level we have a min energy and corresponding best R t
		if (asICPIterations_[sPyrLevel] > 0){
			dCurEnergy = btl::device::dvo_icp_energy(pLiveFrame_->_pRGBCamera->getIntrinsics(sPyrLevel),
				devR_rl, devT_rl,
				*pRefeFrame_->_agPyrPts[sPyrLevel], *pRefeFrame_->_agPyrNls[sPyrLevel], _n_rad_ref[sPyrLevel],
				*pLiveFrame_->_agPyrPts[sPyrLevel], *pLiveFrame_->_agPyrNls[sPyrLevel], _n_rad_live[sPyrLevel],
				*pLiveFrame_->_agPyrDepths[sPyrLevel], _err_live[sPyrLevel], *pLiveFrame_->_pry_mask[sPyrLevel]);
			//PRINT(dMinEnergy);
		}

		SE3Group<double> MinT_rl = NewT_rl;
		double dMin = dCurEnergy;
		double dPrevEnergy = dCurEnergy;
		for (short sIter = 0; sIter < asICPIterations_[sPyrLevel]; ++sIter) {
			//get R and T
			GpuMat cvgmSumBuf = btl::device::dvo_icp(pLiveFrame_->_pRGBCamera->getIntrinsics(sPyrLevel),
				devR_rl, devT_rl,
				*pRefeFrame_->_agPyrPts[sPyrLevel], *pRefeFrame_->_agPyrNls[sPyrLevel], _n_rad_ref[sPyrLevel],
				*pLiveFrame_->_agPyrPts[sPyrLevel], *pLiveFrame_->_agPyrNls[sPyrLevel], _n_rad_live[sPyrLevel],
				*pLiveFrame_->_agPyrDepths[sPyrLevel], _err_live[sPyrLevel], *pLiveFrame_->_pry_mask[sPyrLevel]);
			Mat Buf; cvgmSumBuf.download(Buf);
			SE3Group<double> Tran_nc = btl::utility::extractRTFromBuffer<double>((double*)Buf.data);
			NewT_rl = Tran_nc * PrevT_rl;
			R_rl_t_tmp = NewT_rl.so3().inverse().matrix();
			t_rl = NewT_rl.translation();
			dCurEnergy = btl::device::dvo_icp_energy(pLiveFrame_->_pRGBCamera->getIntrinsics(sPyrLevel),
				devR_rl, devT_rl,
				*pRefeFrame_->_agPyrPts[sPyrLevel], *pRefeFrame_->_agPyrNls[sPyrLevel], _n_rad_ref[sPyrLevel],
				*pLiveFrame_->_agPyrPts[sPyrLevel], *pLiveFrame_->_agPyrNls[sPyrLevel], _n_rad_live[sPyrLevel],
				*pLiveFrame_->_agPyrDepths[sPyrLevel], _err_live[sPyrLevel], *pLiveFrame_->_pry_mask[sPyrLevel]);
			//cout << sIter << ": " << dPrevEnergy << " " << dCurEnergy << endl;
			if (dCurEnergy < dMin){
				dMin = dCurEnergy;
				MinT_rl = NewT_rl;
			}
			if (dMin / dCurEnergy > 1.125){ //diverges
				//cout << "Diverge Warning:" << endl;
				//cout <<"New "<< NewT_rl.matrix() << endl;
				//cout <<"Prev" <<PrevT_rl.matrix() << endl;
				NewT_rl = MinT_rl;
				dCurEnergy = dMin;
				break;
			}
			PrevT_rl = NewT_rl;
			if (fabs(dPrevEnergy / dCurEnergy - 1) < 1e-6f){ //converges
				//cout << "Converges" << endl;
				dCurEnergy = dMin;
				NewT_rl = MinT_rl;
				break;
			}
			dPrevEnergy = dCurEnergy;
		}//for each iteration
	}//for pyrlevel
	*pT_rl_ = NewT_rl;
	SE3Group<double> T_rw(pRefeFrame_->_R_cw, pRefeFrame_->_Tw);
	T_rw = NewT_rl.inverse()*T_rw;
	pLiveFrame_->_R_cw = T_rw.so3();
	pLiveFrame_->_Tw = T_rw.translation();

	return dCurEnergy;
}

double CKinFuTracker::directRotation(const CKeyFrame::tp_ptr pRefeFrame_, const CKeyFrame::tp_ptr pLiveFrame_, SO3Group<double>* pR_rl_)
{
	Intr sCamIntr_ = pRefeFrame_->_pRGBCamera->getIntrinsics(2);
	Matrix3d K = Matrix3d::Identity();
	//note that camera parameters are
	K(0, 0) = sCamIntr_.fx;
	K(1, 1) = sCamIntr_.fy;
	K(0, 2) = sCamIntr_.cx;
	K(1, 2) = sCamIntr_.cy;
	SO3Group<double> CurR_rl_ = *pR_rl_;
	SO3Group<double> PrevR_rl_ = *pR_rl_;
	SO3Group<double> MinR_rl_ = *pR_rl_;
	Matrix3d R_rl_Kinv = PrevR_rl_.matrix() *K.inverse();
	Matrix3d H_rl = K * R_rl_Kinv;

	//get R,T of previous
	Matrix3d H_rl_t = H_rl.transpose();
	Matrix3d R_rl_Kinv_t = R_rl_Kinv.transpose();
	const Matd33&  devH_rl = pcl::device::device_cast<pcl::device::Matd33> (H_rl_t);
	const Matd33&  devR_rl_Kinv = pcl::device::device_cast<pcl::device::Matd33> (R_rl_Kinv_t);
	double dMinEnergy = numeric_limits<double>::max();
	double dPrevEnergy = numeric_limits<double>::max();
	dPrevEnergy = energy_direct_radiance_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
	dMinEnergy = dPrevEnergy;
	//cout << setprecision(15) << dMinEnergy << endl;
	for (short sIter = 0; sIter < 5; ++sIter) {
		//get R and T
		GpuMat gSumBuf = btl::device::direct_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
		Mat Buf; gSumBuf.download(Buf);
		SO3Group<double> R_rl = btl::utility::extractRFromBuffer<double>((double*)Buf.data);
		//cout << Tran_nc.matrix() << endl;
		CurR_rl_ = R_rl *PrevR_rl_;
		R_rl_Kinv = CurR_rl_.matrix()*K.inverse();
		H_rl = K * R_rl_Kinv;

		H_rl_t = H_rl.transpose();
		R_rl_Kinv_t = R_rl_Kinv.transpose();
		double dCurEnergy = energy_direct_radiance_rotation(sCamIntr_, devR_rl_Kinv, devH_rl, _n_rad_origin_2_ref, _n_rad_live[2], _err_live[2]);
		//cout << sIter << ": " << dPrevEnergy << " " << dCurEnergy << endl;
		if (dCurEnergy < dMinEnergy){
			dMinEnergy = dCurEnergy;
			MinR_rl_ = CurR_rl_;
		}
		if (dMinEnergy / dCurEnergy < 0.25){ //divereges
			//cout << "Diverge Warning:" << endl;
			dCurEnergy = dMinEnergy;
			CurR_rl_ = MinR_rl_;
			break;
		}
		PrevR_rl_ = CurR_rl_;
		if (fabs(dPrevEnergy / dCurEnergy - 1) < 0.01f){ //converges
			//cout << "Converges" << endl;
			dCurEnergy = dMinEnergy;
			CurR_rl_ = MinR_rl_;
			break;
		}
		dPrevEnergy = dCurEnergy;
	}
	*pR_rl_ = CurR_rl_;
	return dMinEnergy;
}

double CKinFuTracker::icp(CKeyFrame::tp_ptr pRefeFrame_, CKeyFrame::tp_ptr pLiveFrame_){
	using namespace btl::device;
	//using the ICP to refine each hypotheses and store their alignment score
	double dICPEnergy = numeric_limits<double>::max();
	//do ICP as one of pose hypotheses
	SO3Group<double> R_rl; Vector3d T(0, 0, 0);
	//calc low for rotation estimation
	//Newcombe, R. A., Lovegrove, S. J., & Davison, A. J. (2011). DTAM : Dense Tracking and Mapping in Real-Time. In ICCV. Retrieved from http://www.youtube.com/watch?v=Df9WhgibCQA
	double E = directRotation(pRefeFrame_, pLiveFrame_, &R_rl);
	//cout << R_rl.matrix() << endl;
	Eigen::Vector4i eivIter;
	SE3Group<double> T_rl(R_rl, T);
	short asICPIterations[4] = { 4, 3, 3, 1 };
	dICPEnergy = dvoICPIC(pRefeFrame_, pLiveFrame_, asICPIterations, &T_rl, &eivIter);
	//estimate dt
	estimateExposure(pLiveFrame_, T_rl);

	return dICPEnergy;
}

void CKinFuTracker::estimateExposure(CKeyFrame::tp_ptr pCurFrame_, const SE3Group<double>& T_rl){
	SE3Group<double> T_lr = T_rl.inverse();
	//get R,T of previous
	Matrix3d R_lr_t_tmp = T_lr.so3().inverse().matrix();
	const Matd33&  devR_lr = pcl::device::device_cast<pcl::device::Matd33> (R_lr_t_tmp); //implicit inverse

	Vector3d tt = T_lr.translation();
	const double3& devT_lr = pcl::device::device_cast<double3> (tt);

	GpuMat mask;
	if(true){
		_avg_exposure = btl::device::exposure_est2(pCurFrame_->_pRGBCamera->getIntrinsics(0), devR_lr, devT_lr,
			_pIACurr->_vRadianceBGR[0], _pIACurr->_error_bgr[0],
			_pIACurr->_vRadianceBGR[1], _pIACurr->_error_bgr[1],
			_pIACurr->_vRadianceBGR[2], _pIACurr->_error_bgr[2],
			*_pRefFrame->_agPyrPts[0], _pIAPrev->_vRadianceBGR[0], _pIAPrev->_vRadianceBGR[1], _pIAPrev->_vRadianceBGR[2], mask);
		//cout << "avg exposure: " << _avg_exposure << endl;
	}

	for (int ch = 0; ch < 3; ch++) {
		_pIACurr->_vRadianceBGR[ch].convertTo(_pIACurr->_vRadianceBGR[ch], CV_32FC1, _avg_exposure);
	}

	if(false){
		btl::device::remove_outlier(pCurFrame_->_pRGBCamera->getIntrinsics(0), devR_lr, devT_lr,
			_pIACurr->_vRadianceBGR[0], _pIACurr->_error_bgr[0],
			_pIACurr->_vRadianceBGR[1], _pIACurr->_error_bgr[1],
			_pIACurr->_vRadianceBGR[2], _pIACurr->_error_bgr[2],
			*_pRefFrame->_agPyrPts[0], _pIAPrev->_vRadianceBGR[0], _pIAPrev->_vRadianceBGR[1], _pIAPrev->_vRadianceBGR[2], mask);
	}

	return;
}

void CKinFuTracker::constructLivePyr(){
	//construct pyramid of normalized_radiance;
	calc_avg_min_frame( _pIACurr->_normalized_bgr[0], _pIACurr->_normalized_bgr[1], _pIACurr->_normalized_bgr[2], &(_n_rad_live[0]),
						_pIACurr->_error_bgr[0], _pIACurr->_error_bgr[1], _pIACurr->_error_bgr[2], &(_err_live[0]));

	cuda::pyrDown(_n_rad_live[0], _n_rad_live[1]);
	cuda::pyrDown(_n_rad_live[1], _n_rad_live[2]);
	cuda::pyrDown(_err_live[0], _err_live[1]);
	cuda::pyrDown(_err_live[1], _err_live[2]);

}

void CKinFuTracker::constructRefePyr(){
	//construct pyramid of normalized_radiance;
	calc_avg_frame(_pIAPrev->_normalized_bgr[0], _pIAPrev->_normalized_bgr[1], _pIAPrev->_normalized_bgr[2], &(_n_rad_ref[0]));

	cuda::pyrDown(_n_rad_ref[0], _n_rad_ref[1]);
	cuda::pyrDown(_n_rad_ref[1], _n_rad_ref[2]);
}

void CKinFuTracker::tracking(CKeyFrame::tp_ptr pCurFrame_)
{
	GpuMat bgr;
	cuda::cvtColor(pCurFrame_->_gRGB, bgr, CV_RGB2BGR);
	_pIACurr->analysis(bgr);

	constructLivePyr();

	if (icp(_pRefFrame.get(), pCurFrame_) < _aMinEnergy[_nResolution])
	{
		SE3Group<double> T_cw(pCurFrame_->_R_cw, pCurFrame_->_Tw);
		GpuMat radiance;
		cuda::merge(_pIACurr->_vRadianceBGR, radiance);
		GpuMat er;
		cuda::merge(_pIACurr->_error_bgr, 3, er);
		GpuMat normal_radiance;
		cuda::merge(_pIACurr->_normalized_bgr, 3, normal_radiance);
		_pGlobalMap->gpuIntegrateDepth(*pCurFrame_->_agPyrDepths[0], *pCurFrame_->_agPyrNls[0], normal_radiance, radiance, er,
										SE3Group<double>(pCurFrame_->_R_cw, pCurFrame_->_Tw));
		//insert features into feature-base
		pCurFrame_->getPrjCfW(&_pose_refined_c_f_w);
		_v_T_cw_tracking.push_back(_pose_refined_c_f_w);

		storeCurrFrameSynthetic(pCurFrame_);
	}//if current frame is lost aligned

	return;
}//track

}//geometry
}//btl