/**
 * @file ompl_default_plan_profile.cpp
 * @brief
 *
 * @author Levi Armstrong
 * @date June 18, 2020
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2020, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/tools/multiplan/ParallelPlan.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/goals/GoalStates.h>
#include <boost/algorithm/string.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_command_language/instruction_type.h>
#include <tesseract_command_language/move_instruction.h>
#include <tesseract_command_language/plan_instruction.h>

#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
#include <tesseract_motion_planners/ompl/utils.h>

#include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
#include <tesseract_motion_planners/ompl/discrete_motion_validator.h>
#include <tesseract_motion_planners/ompl/state_collision_validator.h>
#include <tesseract_motion_planners/ompl/compound_state_validator.h>

#include <tesseract_kinematics/core/utils.h>

namespace tesseract_planning
{
OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element)
{
  const tinyxml2::XMLElement* state_space_element = xml_element.FirstChildElement("StateSpace");
  const tinyxml2::XMLElement* planning_time_element = xml_element.FirstChildElement("PlanningTime");
  const tinyxml2::XMLElement* max_solutions_element = xml_element.FirstChildElement("MaxSolutions");
  const tinyxml2::XMLElement* simplify_element = xml_element.FirstChildElement("Simplify");
  const tinyxml2::XMLElement* optimize_element = xml_element.FirstChildElement("Optimize");
  const tinyxml2::XMLElement* planners_element = xml_element.FirstChildElement("Planners");
  //  const tinyxml2::XMLElement* collision_check_element = xml_element.FirstChildElement("CollisionCheck");
  //  const tinyxml2::XMLElement* collision_continuous_element = xml_element.FirstChildElement("CollisionContinuous");
  //  const tinyxml2::XMLElement* collision_safety_margin_element =
  //  xml_element.FirstChildElement("CollisionSafetyMargin");
  //  const tinyxml2::XMLElement* longest_valid_segment_fraction_element =
  //  xml_element.FirstChildElement("LongestValidSegme"
  //                                                                                "ntFraction");
  //  const tinyxml2::XMLElement* longest_valid_segment_length_element =
  //  xml_element.FirstChildElement("LongestValidSegment"
  //                                                                                "Length");

  tinyxml2::XMLError status{ tinyxml2::XMLError::XML_SUCCESS };

  if (state_space_element != nullptr)
  {
    auto type = static_cast<int>(OMPLProblemStateSpace::REAL_STATE_SPACE);
    status = state_space_element->QueryIntAttribute("type", &type);
    if (status != tinyxml2::XML_SUCCESS)
      throw std::runtime_error("OMPLPlanProfile: Error parsing StateSpace type attribute.");

    state_space = static_cast<OMPLProblemStateSpace>(type);
  }

  if (planning_time_element != nullptr)
  {
    std::string planning_time_string;
    status = tesseract_common::QueryStringText(planning_time_element, planning_time_string);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      throw std::runtime_error("OMPLPlanProfile: Error parsing PlanningTime string");

    if (!tesseract_common::isNumeric(planning_time_string))
      throw std::runtime_error("OMPLPlanProfile: PlanningTime is not a numeric values.");

    tesseract_common::toNumeric<double>(planning_time_string, planning_time);
  }

  if (max_solutions_element != nullptr)
  {
    std::string max_solutions_string;
    status = tesseract_common::QueryStringText(max_solutions_element, max_solutions_string);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      throw std::runtime_error("OMPLPlanProfile: Error parsing MaxSolutions string");

    if (!tesseract_common::isNumeric(max_solutions_string))
      throw std::runtime_error("OMPLPlanProfile: MaxSolutions is not a numeric values.");

    tesseract_common::toNumeric<int>(max_solutions_string, max_solutions);
  }

  if (simplify_element != nullptr)
  {
    status = simplify_element->QueryBoolText(&simplify);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      throw std::runtime_error("OMPLPlanProfile: Error parsing Simplify string");
  }

  if (optimize_element != nullptr)
  {
    status = optimize_element->QueryBoolText(&optimize);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      throw std::runtime_error("OMPLPlanProfile: Error parsing Optimize string");
  }

  if (planners_element != nullptr)
  {
    planners.clear();
    for (const tinyxml2::XMLElement* e = planners_element->FirstChildElement("Planner"); e != nullptr;
         e = e->NextSiblingElement("Planner"))
    {
      int type{ 0 };
      status = e->QueryIntAttribute("type", &type);
      if (status != tinyxml2::XML_SUCCESS)
        throw std::runtime_error("OMPLPlanProfile: Error parsing Planner type attribute.");

      switch (type)
      {
        case static_cast<int>(OMPLPlannerType::SBL):
        {
          SBLConfigurator::ConstPtr ompl_planner = std::make_shared<const SBLConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::EST):
        {
          ESTConfigurator::ConstPtr ompl_planner = std::make_shared<const ESTConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::LBKPIECE1):
        {
          LBKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared<const LBKPIECE1Configurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::BKPIECE1):
        {
          BKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared<const BKPIECE1Configurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::KPIECE1):
        {
          KPIECE1Configurator::ConstPtr ompl_planner = std::make_shared<const KPIECE1Configurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::BiTRRT):
        {
          BiTRRTConfigurator::ConstPtr ompl_planner = std::make_shared<const BiTRRTConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::RRT):
        {
          RRTConfigurator::ConstPtr ompl_planner = std::make_shared<const RRTConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::RRTConnect):
        {
          RRTConnectConfigurator::ConstPtr ompl_planner = std::make_shared<const RRTConnectConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::RRTstar):
        {
          RRTstarConfigurator::ConstPtr ompl_planner = std::make_shared<const RRTstarConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::TRRT):
        {
          TRRTConfigurator::ConstPtr ompl_planner = std::make_shared<const TRRTConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::PRM):
        {
          PRMConfigurator::ConstPtr ompl_planner = std::make_shared<const PRMConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::PRMstar):
        {
          PRMstarConfigurator::ConstPtr ompl_planner = std::make_shared<const PRMstarConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::LazyPRMstar):
        {
          LazyPRMstarConfigurator::ConstPtr ompl_planner = std::make_shared<const LazyPRMstarConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        case static_cast<int>(OMPLPlannerType::SPARS):
        {
          SPARSConfigurator::ConstPtr ompl_planner = std::make_shared<const SPARSConfigurator>(*e);
          planners.push_back(ompl_planner);
          break;
        }
        default:
        {
          throw std::runtime_error("Unsupported OMPL Planner type");
        }
      }
    }
  }

  /// @todo Update XML
  //  if (collision_check_element)
  //  {
  //    status = collision_check_element->QueryBoolText(&collision_check);
  //    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
  //      throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionCheck string");
  //  }

  //  if (collision_continuous_element)
  //  {
  //    status = collision_continuous_element->QueryBoolText(&collision_continuous);
  //    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
  //      throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionContinuous string");
  //  }

  /// @todo Update XML
  //  if (collision_safety_margin_element)
  //  {
  //    std::string collision_safety_margin_string;
  //    status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string);
  //    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
  //      throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string");

  //    if (!tesseract_common::isNumeric(collision_safety_margin_string))
  //      throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values.");

  //    tesseract_common::toNumeric<double>(collision_safety_margin_string, collision_safety_margin);
  //  }

  //  if (longest_valid_segment_fraction_element)
  //  {
  //    std::string longest_valid_segment_fraction_string;
  //    status = tesseract_common::QueryStringText(longest_valid_segment_fraction_element,
  //                                               longest_valid_segment_fraction_string);
  //    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
  //      throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentFraction string");

  //    if (!tesseract_common::isNumeric(longest_valid_segment_fraction_string))
  //      throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentFraction is not a numeric values.");

  //    tesseract_common::toNumeric<double>(longest_valid_segment_fraction_string, longest_valid_segment_fraction);
  //  }

  //  if (longest_valid_segment_length_element)
  //  {
  //    std::string longest_valid_segment_length_string;
  //    status =
  //        tesseract_common::QueryStringText(longest_valid_segment_length_element,
  //        longest_valid_segment_length_string);
  //    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
  //      throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentLength string");

  //    if (!tesseract_common::isNumeric(longest_valid_segment_length_string))
  //      throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentLength is not a numeric values.");

  //    tesseract_common::toNumeric<double>(longest_valid_segment_length_string, longest_valid_segment_length);
  //  }
}

void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const
{
  prob.planners = planners;
  prob.planning_time = planning_time;
  prob.max_solutions = max_solutions;
  prob.simplify = simplify;
  prob.optimize = optimize;

  prob.contact_checker->applyContactManagerConfig(collision_check_config.contact_manager_config);

  std::vector<std::string> joint_names = prob.manip->getJointNames();
  auto dof = static_cast<unsigned>(prob.manip->numJoints());
  auto limits = prob.manip->getLimits().joint_limits;

  if (state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
    prob.extractor = [dof](const ompl::base::State* state) -> Eigen::Map<Eigen::VectorXd> {
      return tesseract_planning::RealVectorStateSpaceExtractor(state, dof);
    };

#ifndef OMPL_LESS_1_4_0
  else if (state_space == OMPLProblemStateSpace::REAL_CONSTRAINTED_STATE_SPACE)
    prob.extractor = tesseract_planning::ConstrainedStateSpaceExtractor;
#endif
  else
    throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Unsupported configuration!");

  if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
  {
    // Construct the OMPL state space for this manipulator
    ompl::base::StateSpacePtr state_space_ptr;

    auto rss = std::make_shared<ompl::base::RealVectorStateSpace>();
    for (unsigned i = 0; i < dof; ++i)
      rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1));

    if (state_sampler_allocator)
    {
      rss->setStateSamplerAllocator(
          [=](const ompl::base::StateSpace* space) { return state_sampler_allocator(space, prob); });
    }
    else
    {
      Eigen::VectorXd weights = Eigen::VectorXd::Ones(dof);
      rss->setStateSamplerAllocator(
          [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr {
            return allocWeightedRealVectorStateSampler(state_space, weights, limits);
          });
    }

    state_space_ptr = rss;

    // Setup Longest Valid Segment
    processLongestValidSegment(state_space_ptr, collision_check_config);

    // Create Simple Setup from state space
    prob.simple_setup = std::make_shared<ompl::geometric::SimpleSetup>(state_space_ptr);

    // Setup state checking functionality
    ompl::base::StateValidityCheckerPtr svc_without_collision = processStateValidator(prob);

    // Setup motion validation (i.e. collision checking)
    processMotionValidator(prob, svc_without_collision);

    // make sure the planners run until the time limit, and get the best possible solution
    processOptimizationObjective(prob);
  }
}

void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob,
                                             const Eigen::Isometry3d& cartesian_waypoint,
                                             const Instruction& parent_instruction,
                                             const ManipulatorInfo& manip_info,
                                             const std::vector<std::string>& /*active_links*/,
                                             int /*index*/) const
{
  const auto dof = prob.manip->numJoints();
  tesseract_common::KinematicLimits limits = prob.manip->getLimits();

  assert(isPlanInstruction(parent_instruction));
  const auto& base_instruction = parent_instruction.as<PlanInstruction>();
  assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
  ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());

  if (mi.manipulator.empty())
    throw std::runtime_error("OMPL, manipulator is empty!");

  if (mi.tcp_frame.empty())
    throw std::runtime_error("OMPL, tcp_frame is empty!");

  if (mi.working_frame.empty())
    throw std::runtime_error("OMPL, working_frame is empty!");

  Eigen::Isometry3d tcp_offset = prob.env->findTCPOffset(mi);

  Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse();

  if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
  {
    /** @todo Need to add Descartes pose sample to ompl profile */
    tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame);
    tesseract_kinematics::IKSolutions joint_solutions =
        std::dynamic_pointer_cast<const tesseract_kinematics::KinematicGroup>(prob.manip)
            ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof));
    auto goal_states = std::make_shared<ompl::base::GoalStates>(prob.simple_setup->getSpaceInformation());
    std::vector<tesseract_collision::ContactResultMap> contact_map_vec(
        static_cast<std::size_t>(joint_solutions.size()));

    for (std::size_t i = 0; i < joint_solutions.size(); ++i)
    {
      Eigen::VectorXd& solution = joint_solutions[i];

      // Check limits
      if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits))
      {
        tesseract_common::enforcePositionLimits(solution, limits.joint_limits);
      }
      else
      {
        CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds");
      }

      // Get discrete contact manager for testing provided start and end position
      // This is required because collision checking happens in motion validators now
      // instead of the isValid function to avoid unnecessary collision checks.
      if (!checkStateInCollision(prob, solution, contact_map_vec[i]))
      {
        {
          ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace());
          for (unsigned j = 0; j < dof; ++j)
            goal_state[j] = solution[static_cast<Eigen::Index>(j)];

          goal_states->addState(goal_state);
        }

        auto redundant_solutions = tesseract_kinematics::getRedundantSolutions<double>(
            solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices());
        for (const auto& rs : redundant_solutions)
        {
          ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace());
          for (unsigned j = 0; j < dof; ++j)
            goal_state[j] = rs[static_cast<Eigen::Index>(j)];

          goal_states->addState(goal_state);
        }
      }
    }

    if (!goal_states->hasStates())
    {
      for (std::size_t i = 0; i < contact_map_vec.size(); i++)
        for (const auto& contact_vec : contact_map_vec[i])
          for (const auto& contact : contact_vec.second)
            CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + "  Links: " + contact.link_names[0] + ", " +
                                     contact.link_names[1] + "  Distance: " + std::to_string(contact.distance))
                                        .c_str());
      throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits");
    }
    prob.simple_setup->setGoal(goal_states);
  }
}

void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob,
                                             const Eigen::VectorXd& joint_waypoint,
                                             const Instruction& /*parent_instruction*/,
                                             const ManipulatorInfo& /*manip_info*/,
                                             const std::vector<std::string>& /*active_links*/,
                                             int /*index*/) const
{
  const auto dof = prob.manip->numJoints();
  tesseract_common::KinematicLimits limits = prob.manip->getLimits();
  if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
  {
    // Check limits
    Eigen::VectorXd solution = joint_waypoint;
    if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits))
    {
      tesseract_common::enforcePositionLimits(solution, limits.joint_limits);
    }
    else
    {
      CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds");
    }

    // Get discrete contact manager for testing provided start and end position
    // This is required because collision checking happens in motion validators now
    // instead of the isValid function to avoid unnecessary collision checks.
    tesseract_collision::ContactResultMap contact_map;
    if (checkStateInCollision(prob, solution, contact_map))
    {
      CONSOLE_BRIDGE_logError("In OMPLDefaultPlanProfile: Goal state is in collision");
      for (const auto& contact_vec : contact_map)
        for (const auto& contact : contact_vec.second)
          CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] +
                                   "  Distance: " + std::to_string(contact.distance))
                                      .c_str());
    }

    ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace());
    for (unsigned i = 0; i < dof; ++i)
      goal_state[i] = joint_waypoint[i];

    prob.simple_setup->setGoalState(goal_state);
  }
}

void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob,
                                              const Eigen::Isometry3d& cartesian_waypoint,
                                              const Instruction& parent_instruction,
                                              const ManipulatorInfo& manip_info,
                                              const std::vector<std::string>& /*active_links*/,
                                              int /*index*/) const
{
  const auto dof = prob.manip->numJoints();
  tesseract_common::KinematicLimits limits = prob.manip->getLimits();

  assert(isPlanInstruction(parent_instruction));
  const auto& base_instruction = parent_instruction.as<PlanInstruction>();
  assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
  ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());

  if (mi.manipulator.empty())
    throw std::runtime_error("OMPL, manipulator is empty!");

  if (mi.tcp_frame.empty())
    throw std::runtime_error("OMPL, tcp_frame is empty!");

  if (mi.working_frame.empty())
    throw std::runtime_error("OMPL, working_frame is empty!");

  Eigen::Isometry3d tcp = prob.env->findTCPOffset(mi);

  Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp.inverse();

  if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
  {
    /** @todo Need to add Descartes pose sampler to ompl profile */
    /** @todo Need to also provide the seed instruction to use here */
    tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame);
    tesseract_kinematics::IKSolutions joint_solutions =
        std::dynamic_pointer_cast<const tesseract_kinematics::KinematicGroup>(prob.manip)
            ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof));
    bool found_start_state = false;
    std::vector<tesseract_collision::ContactResultMap> contact_map_vec(joint_solutions.size());

    for (std::size_t i = 0; i < joint_solutions.size(); ++i)
    {
      Eigen::VectorXd& solution = joint_solutions[i];

      // Check limits
      if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits))
      {
        tesseract_common::enforcePositionLimits(solution, limits.joint_limits);
      }
      else
      {
        CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state has invalid bounds");
      }

      // Get discrete contact manager for testing provided start and end position
      // This is required because collision checking happens in motion validators now
      // instead of the isValid function to avoid unnecessary collision checks.
      if (!checkStateInCollision(prob, solution, contact_map_vec[i]))
      {
        found_start_state = true;
        {
          ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace());
          for (unsigned j = 0; j < dof; ++j)
            start_state[j] = solution[static_cast<Eigen::Index>(j)];

          prob.simple_setup->addStartState(start_state);
        }

        auto redundant_solutions = tesseract_kinematics::getRedundantSolutions<double>(
            solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices());
        for (const auto& rs : redundant_solutions)
        {
          ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace());
          for (unsigned j = 0; j < dof; ++j)
            start_state[j] = rs[static_cast<Eigen::Index>(j)];

          prob.simple_setup->addStartState(start_state);
        }
      }
    }

    if (!found_start_state)
    {
      for (std::size_t i = 0; i < contact_map_vec.size(); i++)
        for (const auto& contact_vec : contact_map_vec[i])
          for (const auto& contact : contact_vec.second)
            CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + "  Links: " + contact.link_names[0] + ", " +
                                     contact.link_names[1] + "  Distance: " + std::to_string(contact.distance))
                                        .c_str());
      throw std::runtime_error("In OMPLPlannerFreespaceConfig: All start states are either in collision or outside "
                               "limits");
    }
  }
}

void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob,
                                              const Eigen::VectorXd& joint_waypoint,
                                              const Instruction& /*parent_instruction*/,
                                              const ManipulatorInfo& /*manip_info*/,
                                              const std::vector<std::string>& /*active_links*/,
                                              int /*index*/) const
{
  const auto dof = prob.manip->numJoints();
  tesseract_common::KinematicLimits limits = prob.manip->getLimits();

  if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE)
  {
    Eigen::VectorXd solution = joint_waypoint;

    if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits))
    {
      tesseract_common::enforcePositionLimits(solution, limits.joint_limits);
    }
    else
    {
      CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state is outside limits");
    }

    // Get discrete contact manager for testing provided start and end position
    // This is required because collision checking happens in motion validators now
    // instead of the isValid function to avoid unnecessary collision checks.
    tesseract_collision::ContactResultMap contact_map;
    if (checkStateInCollision(prob, joint_waypoint, contact_map))
    {
      CONSOLE_BRIDGE_logError("In OMPLPlannerFreespaceConfig: Start state is in collision");
      for (const auto& contact_vec : contact_map)
        for (const auto& contact : contact_vec.second)
          CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] +
                                   "  Distance: " + std::to_string(contact.distance))
                                      .c_str());
    }

    ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace());
    for (unsigned i = 0; i < dof; ++i)
      start_state[i] = joint_waypoint[i];

    prob.simple_setup->addStartState(start_state);
  }
}

tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const
{
  Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");

  tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner");
  xml_planner->SetAttribute("type", std::to_string(2).c_str());

  tinyxml2::XMLElement* xml_ompl = doc.NewElement("OMPLPlanProfile");

  tinyxml2::XMLElement* xml_ompl_planners = doc.NewElement("Planners");

  for (const auto& planner : planners)
  {
    tinyxml2::XMLElement* xml_ompl_planner = doc.NewElement("Planner");
    xml_ompl_planner->SetAttribute("type", std::to_string(static_cast<int>(planner->getType())).c_str());
    tinyxml2::XMLElement* xml_planner = planner->toXML(doc);
    xml_ompl_planner->InsertEndChild(xml_planner);
    xml_ompl_planners->InsertEndChild(xml_ompl_planner);
  }

  xml_ompl->InsertEndChild(xml_ompl_planners);

  tinyxml2::XMLElement* xml_state_space = doc.NewElement("StateSpace");
  xml_state_space->SetAttribute("type", std::to_string(static_cast<int>(state_space)).c_str());
  xml_ompl->InsertEndChild(xml_state_space);

  tinyxml2::XMLElement* xml_planning_time = doc.NewElement("PlanningTime");
  xml_planning_time->SetText(planning_time);
  xml_ompl->InsertEndChild(xml_planning_time);

  tinyxml2::XMLElement* xml_max_solutions = doc.NewElement("MaxSolutions");
  xml_max_solutions->SetText(max_solutions);
  xml_ompl->InsertEndChild(xml_max_solutions);

  tinyxml2::XMLElement* xml_simplify = doc.NewElement("Simplify");
  xml_simplify->SetText(simplify);
  xml_ompl->InsertEndChild(xml_simplify);

  tinyxml2::XMLElement* xml_optimize = doc.NewElement("Optimize");
  xml_optimize->SetText(optimize);
  xml_ompl->InsertEndChild(xml_optimize);

  /// @todo Update XML
  //  tinyxml2::XMLElement* xml_collision_check = doc.NewElement("CollisionCheck");
  //  xml_collision_check->SetText(collision_check);
  //  xml_ompl->InsertEndChild(xml_collision_check);

  //  tinyxml2::XMLElement* xml_collision_continuous = doc.NewElement("CollisionContinuous");
  //  xml_collision_continuous->SetText(collision_continuous);
  //  xml_ompl->InsertEndChild(xml_collision_continuous);

  //  tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin");
  //  xml_collision_safety_margin->SetText(collision_safety_margin);
  //  xml_ompl->InsertEndChild(xml_collision_safety_margin);

  //  tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction");
  //  xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction);
  //  xml_ompl->InsertEndChild(xml_long_valid_seg_frac);

  //  tinyxml2::XMLElement* xml_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength");
  //  xml_long_valid_seg_len->SetText(longest_valid_segment_length);
  //  xml_ompl->InsertEndChild(xml_long_valid_seg_len);

  // TODO: Add plugins for state_sampler_allocator, optimization_objective_allocator, svc_allocator,
  // mv_allocator

  xml_planner->InsertEndChild(xml_ompl);

  return xml_planner;
}

ompl::base::StateValidityCheckerPtr OMPLDefaultPlanProfile::processStateValidator(OMPLProblem& prob) const
{
  ompl::base::StateValidityCheckerPtr svc_without_collision;
  auto csvc = std::make_shared<CompoundStateValidator>();
  if (svc_allocator != nullptr)
  {
    svc_without_collision = svc_allocator(prob.simple_setup->getSpaceInformation(), prob);
    csvc->addStateValidator(svc_without_collision);
  }

  if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE ||
      collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)
  {
    auto svc = std::make_shared<StateCollisionValidator>(
        prob.simple_setup->getSpaceInformation(), *prob.env, prob.manip, collision_check_config, prob.extractor);
    csvc->addStateValidator(svc);
  }
  prob.simple_setup->setStateValidityChecker(csvc);

  return svc_without_collision;
}

void OMPLDefaultPlanProfile::processMotionValidator(
    OMPLProblem& prob,
    const ompl::base::StateValidityCheckerPtr& svc_without_collision) const
{
  if (mv_allocator != nullptr)
  {
    auto mv = mv_allocator(prob.simple_setup->getSpaceInformation(), prob);
    prob.simple_setup->getSpaceInformation()->setMotionValidator(mv);
  }
  else
  {
    if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE)
    {
      ompl::base::MotionValidatorPtr mv;
      if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS ||
          collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
      {
        mv = std::make_shared<ContinuousMotionValidator>(prob.simple_setup->getSpaceInformation(),
                                                         svc_without_collision,
                                                         *prob.env,
                                                         prob.manip,
                                                         collision_check_config,
                                                         prob.extractor);
      }
      else
      {
        // Collision checking is preformed using the state validator which this calls.
        mv = std::make_shared<DiscreteMotionValidator>(prob.simple_setup->getSpaceInformation());
      }
      prob.simple_setup->getSpaceInformation()->setMotionValidator(mv);
    }
  }
}

void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) const
{
  if (optimization_objective_allocator)
  {
    prob.simple_setup->getProblemDefinition()->setOptimizationObjective(
        optimization_objective_allocator(prob.simple_setup->getSpaceInformation(), prob));
  }
  else if (prob.optimize)
  {
    // Add default optimization function to minimize path length
    prob.simple_setup->getProblemDefinition()->setOptimizationObjective(
        std::make_shared<ompl::base::PathLengthOptimizationObjective>(prob.simple_setup->getSpaceInformation()));
  }
}

}  // namespace tesseract_planning


/*
 * Author: Johannes M Dieterich
 */

#ifndef FOURIERKEDF_HPP
#define	FOURIERKEDF_HPP

#include <armadillo>
#include <memory>
#include <string>
#include "FourierGrid.hpp"
#include "Grid.hpp"
#include "StressTensor.hpp"
using namespace std;

class FourierKEDF {
public:
    virtual ~FourierKEDF(){};
    virtual string getMethodDescription() const = 0;
    virtual vector<string> getCitations() const = 0;
    virtual vector<string> getWorkingEquations() const = 0;
    virtual double calcEnergy(const FourierGrid& grid) const = 0;
    virtual double calcPotential(const FourierGrid& grid, FourierGrid& potential) const = 0;
    virtual unique_ptr<StressTensor> calcStress(const FourierGrid& grid) const = 0;
private:
};

#endif	/* FOURIERKEDF_HPP */


#include "eigen-delegate.hpp"
#include <memory>

#include <Eigen/Core>
#include <tensorflow/lite/c/builtin_op_data.h>

#define DEBUG

using namespace tflite::tools;

namespace tflite
{

EigenDelegate::EigenDelegate(
    const SimpleDelegateInterface::Options &options
) : options(options)
{}

bool EigenDelegate::IsNodeSupportedByDelegate(
        const TfLiteRegistration *registration,
        const TfLiteNode *node,
        TfLiteContext *context) const
{
    switch (registration->builtin_code)
    {
        // TODO uncomment
        // case kTfLiteBuiltinAdd:
        case kTfLiteBuiltinFullyConnected:
            break;
        default:
            printf("Skipped builtin code %d\n", registration->builtin_code);
            return false;
    }
    // Only support float32
    for (int i = 0; i < node->inputs->size; i++)
    {
        auto &tensor = context->tensors[node->inputs->data[i]];
        if (tensor.type != kTfLiteFloat32)
        {
            printf("Skipped tensor type %d for %d (%s,%d)\n",
                tensor.type,
                i,
                registration->custom_name == NULL ? "" : registration->custom_name,
                registration->builtin_code
            );
            return false;
        }
    }
    return true;
}

TfLiteStatus EigenDelegate::Initialize(TfLiteContext *context)
{
    return kTfLiteOk;
}

const char *EigenDelegate::Name() const
{
    static constexpr char kName[] = "EigenDelegate";
    return kName;
}

std::unique_ptr<SimpleDelegateKernelInterface> EigenDelegate::CreateDelegateKernelInterface()
{
    return std::make_unique<EigenDelegateKernel>();
}

TfLiteStatus EigenDelegateKernel::Init(TfLiteContext* context, const TfLiteDelegateParams* params)
{
    // Save index to all nodes which are part of this delegate.
    inputs_.resize(params->nodes_to_replace->size);
    outputs_.resize(params->nodes_to_replace->size);
    builtin_code_.resize(params->nodes_to_replace->size);
    nodes_.resize(params->nodes_to_replace->size);
    for (int i = 0; i < params->nodes_to_replace->size; i++)
    {
        const int node_index = params->nodes_to_replace->data[i];
        // Get this node information.
        TfLiteNode* delegated_node = nullptr;
        TfLiteRegistration* delegated_node_registration = nullptr;
        TF_LITE_ENSURE_EQ(
            context,
            context->GetNodeAndRegistration(
                context,
                node_index,
                &delegated_node,
                &delegated_node_registration
            ),
            kTfLiteOk
        );
        for (int j = 0; j < delegated_node->inputs->size; j++)
        {
            inputs_[i].push_back(delegated_node->inputs->data[j]);
        }
        for (int j = 0; j < delegated_node->outputs->size; j++)
        {
            outputs_[i].push_back(delegated_node->outputs->data[j]);
        }
        builtin_code_[i] = delegated_node_registration->builtin_code;
        nodes_[i] = delegated_node;
    }
    return kTfLiteOk;
}

TfLiteStatus EigenDelegateKernel::Prepare(TfLiteContext* context, TfLiteNode* node)
{
    intermediatedata.clear();
#ifdef DEBUG
    printf("BEFORE ALLOCATION (tensor id, data address, dims address)\n");
    for (int i = 0; i < context->tensors_size; i++)
    {
        printf("tensor %d:  %x %x\n",
            i,
            context->tensors[i].data,
            context->tensors[i].dims
        );
    }
#endif // DEBUG
    for (auto &out : outputs_)
    {
        for (auto &outid : out)
        {
            if (GetTensorData<float *>(&context->tensors[outid]) == NULL)
            {
                intermediatedata.push_back(
                    std::make_unique<std::vector<float>>(
                        NumElements(&context->tensors[outid])
                    )
                );
                context->tensors[outid].data.f = intermediatedata.back()->data();
            }
        }
    }
#ifdef DEBUG
    printf("AFTER ALLOCATION (tensor id, data address, dims address)\n");
    for (int i = 0; i < context->tensors_size; i++)
    {
        printf("tensor %d:  %x %x\n",
            i,
            context->tensors[i].data,
            context->tensors[i].dims
        );
    }
#endif // DEBUG
    return kTfLiteOk;
}

TfLiteStatus EigenDelegateKernel::Eval(TfLiteContext* context, TfLiteNode* node)
{
    TfLiteStatus ret = kTfLiteOk;
    for (int i = 0; i < inputs_.size(); i++)
    {
        switch (builtin_code_[i])
        {
            // TODO uncomment
            // case kTfLiteBuiltinAdd:
            //     ret = add(context, i);
            //     break;
            case kTfLiteBuiltinFullyConnected:
                ret = fullyConnected(context, i);
                break;
            default:
                ret = kTfLiteDelegateError;
        }
        if (ret != kTfLiteOk)
        {
            break;
        }
    }
    return ret;
}

TfLiteStatus EigenDelegateKernel::add(TfLiteContext *context, int nodeid)
{
    // TODO implement using Eigen routines
    return kTfLiteOk;
}

TfLiteStatus EigenDelegateKernel::fullyConnected(TfLiteContext *context, int nodeid)
{
    auto &tfinput = context->tensors[inputs_[nodeid][0]];
    auto &tfweights = context->tensors[inputs_[nodeid][1]];
    auto &tfbias = context->tensors[inputs_[nodeid][2]];
    auto &tfoutput = context->tensors[outputs_[nodeid][0]];

    auto ws = GetTensorShape(&tfweights);

    Eigen::Map<Eigen::VectorXf> input(
        GetTensorData<float>(&tfinput), NumElements(&tfinput)
    );

    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> weights(
        GetTensorData<float>(&tfweights),
        ws.Dims(0),
        ws.Dims(1)
    );

    Eigen::Map<Eigen::VectorXf> bias(
        GetTensorData<float>(&tfbias),
        NumElements(&tfbias)
    );

    Eigen::Map<Eigen::VectorXf> output(
        GetTensorData<float>(&tfoutput),
        NumElements(&tfoutput)
    );

    output = weights * input + bias;

    TfLiteFullyConnectedParams *params = static_cast<TfLiteFullyConnectedParams *>(
        nodes_[nodeid]->builtin_data
    );

    switch (params->activation)
    {
        case TfLiteFusedActivation::kTfLiteActNone:
            break;
        case TfLiteFusedActivation::kTfLiteActRelu:
            output = output.cwiseMax(0);
            break;
        default:
            return kTfLiteDelegateError;
    }
    return kTfLiteOk;
}

} // namespace tflite

tflite::SimpleDelegateInterface::Options TfLiteEigenDelegateOptionsDefault() {
    tflite::SimpleDelegateInterface::Options options = {0};
    return options;
}

TfLiteDelegate *TfLiteEigenDelegateCreate(const tflite::SimpleDelegateInterface::Options* options) {
    std::unique_ptr<tflite::EigenDelegate> custom(
        new tflite::EigenDelegate(
            options ? *options : TfLiteEigenDelegateOptionsDefault()
        )
    );
    return tflite::TfLiteDelegateFactory::CreateSimpleDelegate(std::move(custom));
}

void TfLiteEigenDelegateDelete(TfLiteDelegate* delegate) {
    tflite::TfLiteDelegateFactory::DeleteSimpleDelegate(delegate);
}

TfLiteDelegate *CreateEigenDelegateFromOptions(char **options_keys, char **options_values, size_t num_options)
{
    return TfLiteEigenDelegateCreate(NULL);
}

TfLiteDelegate *tflite_plugin_create_delegate(char** options_keys, char** options_values, size_t num_options, void (*report_error)(const char*))
{
	return CreateEigenDelegateFromOptions(options_keys, options_values, num_options);
}

void tflite_plugin_destroy_delegate(TfLiteDelegate *delegate)
{
	TfLiteEigenDelegateDelete(delegate);
}


// Copyright (c) 2016 The UUV Simulator Authors.
// All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <uuv_sensor_plugins/UnderwaterCameraPlugin.hh>

#include <chrono>
#include <cmath>
#include <iostream>
#include <stdio.h>
#include <string>

#include <boost/bind.hpp>

#include "Common.hh"

namespace gazebo {

UnderwaterCameraPlugin::UnderwaterCameraPlugin()
    : DepthCameraPlugin(),
      lastImage(nullptr)
{
}

UnderwaterCameraPlugin::~UnderwaterCameraPlugin()
{
    if (lastImage)
        delete [] lastImage;

    if (depth2rangeLUT)
        delete [] depth2rangeLUT;
}

void UnderwaterCameraPlugin::Load(sensors::SensorPtr _sensor,
                                  sdf::ElementPtr _sdf)
{
    DepthCameraPlugin::Load(_sensor, _sdf);

    lastImage = new unsigned char[this->width*this->height*this->depth];

    // Only need to load settings specific to this sensor.
    getSdfParam<float>(_sdf, "attenuationR", attenuation[0], 1.f/30.f);
    getSdfParam<float>(_sdf, "attenuationG", attenuation[1], 1.f/30.f);
    getSdfParam<float>(_sdf, "attenuationB", attenuation[2], 1.f/30.f);

    getSdfParam<unsigned char>(_sdf, "backgroundR", background[0], 0);
    getSdfParam<unsigned char>(_sdf, "backgroundG", background[1], 0);
    getSdfParam<unsigned char>(_sdf, "backgroundB", background[2], 0);

    // Compute camera intrinsics fx, fy from FOVs:
#if GAZEBO_MAJOR_VERSION >= 7
    math::Angle hfov = math::Angle(this->depthCamera->HFOV().Radian());
    math::Angle vfov = math::Angle(this->depthCamera->VFOV().Radian());
#else
    math::Angle hfov = this->depthCamera->GetHFOV();
    math::Angle vfov = this->depthCamera->GetVFOV();
#endif

    double fx = (0.5*this->width)/tan(0.5*hfov.Radian());
    double fy = (0.5*this->height)/tan(0.5*vfov.Radian());

    // Assume the camera's principal point to be at the sensor's center:
    double cx = 0.5*this->width;
    double cy = 0.5*this->height;

    // Create and fill depth2range LUT
    this->depth2rangeLUT = new float[this->width*this->height];
    float* lutPtr = this->depth2rangeLUT;
    for (int v = 0; v < this->height; v++)
    {
        double y_z = (v - cy)/fy;
        for (int u = 0; u < this->width; u++)
        {
            double x_z = (u - cx)/fx;
            // Precompute the per-pixel factor in the following formula:
            // range = || (x, y, z) ||_2
            // range = || z * (x/z, y/z, 1.0) ||_2
            // range = z * || (x/z, y/z, 1.0) ||_2
            *(lutPtr++) = sqrt(1.0 + x_z*x_z + y_z*y_z);
        }
    }

    // TODO: Advertise a gazebo topic?
    //    publisher_ = node_handle_->Advertise<sensor_msgs::msgs::Camera>(
    //                sensor_topic_, 10);
}

void UnderwaterCameraPlugin::OnNewDepthFrame(const float *_image,
                                             unsigned int _width,
                                             unsigned int _height,
                                             unsigned int _depth,
                                             const std::string &_format)
{
    // TODO: Can we assume this pointer to always remain valid?
    lastDepth = _image;
}

void UnderwaterCameraPlugin::OnNewRGBPointCloud(const float *_pcd,
                                                unsigned int _width,
                                                unsigned int _height,
                                                unsigned int _depth,
                                                const std::string &_format)
{
}

void UnderwaterCameraPlugin::OnNewImageFrame(const unsigned char *_image,
                                             unsigned int _width,
                                             unsigned int _height,
                                             unsigned int _depth,
                                             const std::string &_format)
{
    // Only create cv::Mat wrappers around existing memory
    // (neither allocates nor copies any images).
    const cv::Mat input(_height, _width, CV_8UC3,
                        const_cast<unsigned char*>(_image));
    const cv::Mat depth(_height, _width, CV_32FC1,
                        const_cast<float*>(lastDepth));

    cv::Mat output(_height, _width, CV_8UC3, lastImage);

    SimulateUnderwater(input, depth, output);

    // TODO: Publish Gazebo topic
}

void UnderwaterCameraPlugin::SimulateUnderwater(const cv::Mat& _inputImage,
                                                const cv::Mat& _inputDepth,
                                                cv::Mat& _outputImage)
{
    const float* lutPtr = this->depth2rangeLUT;
    for (unsigned int row = 0; row < this->height; row++)
    {
        const cv::Vec3b* inrow = _inputImage.ptr<cv::Vec3b>(row);
        const float* depthrow = _inputDepth.ptr<float>(row);
        cv::Vec3b* outrow = _outputImage.ptr<cv::Vec3b>(row);

        for (int col = 0; col < this->width; col++)
        {
            // Convert depth to range using the depth2range LUT
            float r = *(lutPtr++)*depthrow[col];
            const cv::Vec3b& in = inrow[col];
            cv::Vec3b& out = outrow[col];

            if (r < 1e-3)
            {
              r = 1e10;
            }

            for (int c = 0; c < 3; c++)
            {
                // Simplifying assumption: intensity ~ irradiance.
                // This is not really the case but a good enough approximation
                // for now (it would be better to use a proper Radiometric
                // Response Function).
                float e = std::exp(-r*attenuation[c]);
                out[c] = e*in[c] + (1.0f-e)*background[c];
            }
        }
    }
}
GZ_REGISTER_SENSOR_PLUGIN(UnderwaterCameraPlugin);
}


#include "../base/oz.h"
#include "ozShapeDetector.h"

#include "../base/ozAlarmFrame.h"
#include <dlib/svm_threaded.h>
#include <dlib/image_processing.h>
#include <dlib/data_io.h>
#include <iostream>
#include <vector>

/**
* @brief
*
* @param name
*/
ShapeDetector::ShapeDetector( const std::string &name, const std::string &objectData, ShapeMarkup shapeMarkup ) :
    VideoProvider( cClass(), name ),
    Thread( identity() ),
    mObjectData( objectData ),
    mShapeMarkup( shapeMarkup )
{
}

/**
* @brief
*
* @param markup
* @param provider
* @param link
*/
ShapeDetector::ShapeDetector( const std::string &objectData, ShapeMarkup shapeMarkup, VideoProvider &provider, const FeedLink &link ) :
    VideoConsumer( cClass(), provider, link ),
    VideoProvider( cClass(), provider.name() ),
    Thread( identity() ),
    mObjectData( objectData ),
    mShapeMarkup( shapeMarkup )
{
}

/**
* @brief
*/
ShapeDetector::~ShapeDetector()
{
}

/**
* @brief
*
* @return
*/
int ShapeDetector::run()
{
    config.dir_events = "/transfer";
    config.record_diag_images = true;

    // Wait for encoder to be ready
    if ( waitForProviders() )
    {
        AVPixelFormat pixelFormat = videoProvider()->pixelFormat();
        int16_t width = videoProvider()->width();
        int16_t height = videoProvider()->height();
        Debug( 1,"pf:%d, %dx%d", pixelFormat, width, height );

        typedef dlib::scan_fhog_pyramid<dlib::pyramid_down<6> > image_scanner_type;

        dlib::object_detector<image_scanner_type> detector;
        dlib::deserialize( mObjectData.c_str() ) >> detector;

        setReady();
        while ( !mStop )
        {
            if ( mStop )
               break;
            mQueueMutex.lock();
            if ( !mFrameQueue.empty() )
            {
                Debug( 3, "Got %zd frames on queue", mFrameQueue.size() );
                for ( FrameQueue::iterator iter = mFrameQueue.begin(); iter != mFrameQueue.end(); iter++ )
                {
                    // Only operate on video frames
                    const VideoFrame *frame = dynamic_cast<const VideoFrame *>(iter->get());
                    if ( frame )
                    {
                        Image image( pixelFormat, width, height, frame->buffer().data() );
                        Image convImage( Image::FMT_RGB, image );
                        dlib::array2d<dlib::rgb_pixel> img( height, width );
                        memcpy( image_data( img ), convImage.buffer().data(), convImage.buffer().size() );
                        //assign_image( img, frame->buffer().data() );

                        //Info( "B4: %d x %d", num_rows(img), num_columns(img) );
                        //dlib::pyramid_up(img);
                        //Info( "AFT: %d x %d", num_rows(img), num_columns(img) );
                        std::vector<dlib::rectangle> dets = detector(img);

                       	Debug( (dets.size() > 0) ? 1:2,"%jd @ %ju: Got %jd shapes", frame->id(), frame->timestamp(), dets.size() );
                        if ( dets.size() > 0 && mShapeMarkup != OZ_SHAPE_MARKUP_NONE )
                        {
                            // Now we will go ask the shape_predictor to tell us the pose of
                            // each shape we detected.
                            std::vector<dlib::full_object_detection> shapes;
                            typedef std::pair<dlib::point,dlib::point> line_t;
                            std::vector<line_t> lines;
                            for ( unsigned int i = 0; i < dets.size(); i++ )
                            {
                                if ( mShapeMarkup & OZ_SHAPE_MARKUP_OUTLINE )
                                    draw_rectangle( img, dets[i], dlib::rgb_pixel( 255, 0, 0 ), 1 );
                            }

                            //dlib::save_png( img, "/transfer/image.png" );
                            //Info( "%d x %d = %d", num_rows(img), num_columns(img), 3*num_rows(img)*num_columns(img) );
                        }
						else
						{
                        	Debug( 2,"%jd @ %ju: Got %jd shapes", frame->id(), frame->timestamp(), dets.size() );
						}
                        // Move inside preceding 'if' to only output 'shape' frames
                        AlarmFrame *alarmFrame = new AlarmFrame( this, *iter, frame->id(), frame->timestamp(), (uint8_t *)image_data(img), 3*num_rows(img)*num_columns(img), dets.size()>0 );

                        distributeFrame( FramePtr( alarmFrame ) );
                        mFrameCount++;
                    }
                }
                mFrameQueue.clear();
            }
            mQueueMutex.unlock();
            checkProviders();
            // Quite short so we can always keep up with the required packet rate for 25/30 fps
            usleep( INTERFRAME_TIMEOUT );
        }
    }
    FeedProvider::cleanup();
    FeedConsumer::cleanup();
    return( !ended() );
}


// Copyright 2019 Apex.AI, Inc.
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <lidar_utils/point_cloud_utils.hpp>
#include <ndt/visibility_control.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <vector>
#include <Eigen/src/Core/Matrix.h>

namespace autoware{
namespace localization{
namespace ndt{

class NdtNormal
{

  using Point = Eigen::Vector3d;
  using Covariance = Eigen::Matrix3d;

  // getters, setters
  Point centroid;
  Covariance covariance;
};

//CRTP base class for NDT maps
template<typename Derived, typename NdtUnit = NdtNormal>
class NdtMapBase
{
    // neighbourhood search
  const std::vector<NdtUnit> & cells(double x, double y, double z);
};

//CRTP base class for NDT scans.
template<typename Derived, typename NdtUnit = common::lidar_utils::PointXYZIF>
class NdtScanBase{
public:
  using Point = NdtUnit;
  using IterT = typename std::vector<NdtUnit>::iterator;
  //get the point for a given index.
  NdtUnit & get(size_t index);

  IterT begin();

  IterT end();

private:
  const Derived & impl() const
  {
    return *static_cast<const Derived *>(this);
  }
};

class NDTMap : public NdtMapBase<NDTMap, NdtNormal>{
    // Use voxelgrid or spatial hash directly etc. instead
    using MapContainer = std::vector<NdtNormal>;
private:
    MapContainer m_cells;
};

class P2DNDTScan : public NdtScanBase<P2DNDTScan, common::lidar_utils::PointXYZIF>{
  using PointT = common::lidar_utils::PointXYZIF;
  const std::vector<PointT> & cells(double x, double y, double z);

  void advance(size_t advancement);
  PointT next();
  PointT prev();
  PointT front();
  PointT back();
  void transform(const geometry_msgs::msg::Transform &);
private:
 sensor_msgs::msg::PointCloud2 m_cloud;
};

}
}
}


#include <math.h>
#include <stdio.h>
#include <iostream>

#include <boost/math/quaternion.hpp>

extern "C" void externalForceG(double t, double* f, unsigned int size_z, double *z){
  // std::cout << "externalForceG at time "<< t << std::endl;
  f[0]=0;
  f[1]=0;
  f[2]=0;

  if (t > 0.04){
    f[2] = +1000.0;
  }
}
extern "C" void externalMomentumY(double t,double *m, unsigned int size_z, double *z){
  // std::cout << "externalMomentumY"<<std::endl;
  m[0]=5000;
  m[1]=0;
  m[2]=0;
}


#ifndef GIFS_SH_BOMD_CORE_H
#define GIFS_SH_BOMD_CORE_H

#include <armadillo>
#include "qm_interface.hpp"
#include "configreader.hpp"

class BOMD
{
public:
  explicit BOMD(arma::mat& qm_grd, arma::mat& mm_grd);
  virtual ~BOMD() {delete qm;}

  virtual double update_gradient(void);
  virtual bool rescale_velocities(arma::mat &velocities, arma::vec &masses, arma::mat &total_gradient, double total_energy);
  // setup from user input
  void setup(FileHandle& fh,
             const arma::uvec& atomicnumbers,
	         arma::mat& qm_crd,
	         arma::mat& mm_crd,
	         arma::vec& mm_chg
          );
  //
protected:
  inline arma::uword NQM(void) const { return qm_grd.n_cols; }
  inline arma::uword NMM(void) const { return mm_grd.n_cols; }
  inline int call_idx() const noexcept { return md_call_idx; };

  /* Config for keys common to all dynamics classes */
  void add_common_keys(ConfigBlockReader& reader);

  /* Child classes will override these methods for own setup */
  virtual ConfigBlockReader setup_reader(void);
  virtual void get_reader_data(ConfigBlockReader& reader);

  QMInterface* qm{nullptr};
  // fixed size
  arma::mat& qm_grd;
  // flexible size
  arma::mat& mm_grd;
  arma::vec energy{};

  // On which surface are we running?
  size_t active_state;

  // To track energy drift from GMX
  double elast = 0, edrift = 0;

private:
  int md_call_idx = 0; // tracks each call to rescale_velocities();
};

class PrintBomd:
    public BOMD
{
public:
  virtual ~PrintBomd() {};
  explicit PrintBomd(arma::mat& qm_grd,
                     arma::mat& mm_grd) : BOMD(qm_grd, mm_grd) {}

  double update_gradient(void) {
    double e = BOMD::update_gradient();
    qm->crd_qm.t().print("qm_crd: ");
    qm->crd_mm.t().print("mm_crd: ");
    qm_grd.t().print("qm_grd");
    mm_grd.t().print("mm_grd");
    return e;
  }

  bool rescale_velocities(arma::mat &velocities, arma::vec &masses, arma::mat &total_gradient, double total_energy) {
    BOMD::rescale_velocities(velocities, masses, total_gradient, total_energy);
    velocities.t().print("Velocities:");
    masses.t().print("Masses:");
    total_gradient.t().print("Total Gradient");
    return true;
  }
};

#endif