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

#include <gtest/gtest.h>

#include <map>
#include <math.h>
#include <string>
#include <vector>
#include <boost/thread.hpp>

#include <Eigen/Geometry>
#include <kdl/frames.hpp>
#include <kdl/path_roundedcomposite.hpp>
#include <kdl/rotational_interpolation_sa.hpp>
#include <kdl/trajectory.hpp>
#include <kdl/trajectory_segment.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif

#include "pilz_industrial_motion_planner/cartesian_trajectory.h"
#include "pilz_industrial_motion_planner/cartesian_trajectory_point.h"
#include "pilz_industrial_motion_planner/limits_container.h"
#include "pilz_industrial_motion_planner/trajectory_functions.h"
#include "test_utils.h"

#define _USE_MATH_DEFINES

static constexpr double EPSILON{ 1.0e-6 };
static constexpr double IK_SEED_OFFSET{ 0.1 };
static constexpr double L0{ 0.2604 };  // Height of foot
static constexpr double L1{ 0.3500 };  // Height of first connector
static constexpr double L2{ 0.3070 };  // Height of second connector
static constexpr double L3{ 0.0840 };  // Distance last joint to flange

// parameters from parameter server
const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
const std::string GROUP_TIP_LINK_NAME("group_tip_link");
const std::string ROBOT_TCP_LINK_NAME("tcp_link");
const std::string IK_FAST_LINK_NAME("ik_fast_link");
const std::string RANDOM_TEST_NUMBER("random_test_number");

/**
 * @brief test fixtures base class
 */
class TrajectoryFunctionsTestBase : public testing::Test
{
protected:
  /**
   * @brief Create test scenario for trajectory functions
   *
   */
  void SetUp() override
  {
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    node_ = rclcpp::Node::make_shared("unittest_trajectory_functions", node_options);

    // load robot model
    robot_model_loader::RobotModelLoader rm_loader(node_);
    robot_model_ = rm_loader.getModel();
    ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
    planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);

    // get parameters
    ASSERT_TRUE(node_->has_parameter("planning_group"));
    node_->get_parameter<std::string>("planning_group", planning_group_);
    ASSERT_TRUE(node_->has_parameter("group_tip_link"));
    node_->get_parameter<std::string>("group_tip_link", group_tip_link_);
    ASSERT_TRUE(node_->has_parameter("tcp_link"));
    node_->get_parameter<std::string>("tcp_link", tcp_link_);
    ASSERT_TRUE(node_->has_parameter("ik_fast_link"));
    node_->get_parameter<std::string>("ik_fast_link", ik_fast_link_);
    ASSERT_TRUE(node_->has_parameter("random_test_number"));
    node_->get_parameter<int>("random_test_number", random_test_number_);

    // check robot model
    testutils::checkRobotModel(robot_model_, planning_group_, tcp_link_);

    // initialize the zero state configurationg and test joint state
    joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames();
    for (const auto& joint_name : joint_names_)
    {
      zero_state_[joint_name] = 0.0;
    }
  }

  /**
   * @brief check if two transformations are close
   * @param pose1
   * @param pose2
   * @param epsilon
   * @return
   */
  bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon);

protected:
  // ros stuff
  rclcpp::Node::SharedPtr node_;
  moveit::core::RobotModelConstPtr robot_model_;
  planning_scene::PlanningSceneConstPtr planning_scene_;

  // test parameters from parameter server
  std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_;
  int random_test_number_;
  std::vector<std::string> joint_names_;
  std::map<std::string, double> zero_state_;

  // random seed
  boost::uint32_t random_seed_{ 100 };
  random_numbers::RandomNumberGenerator rng_{ random_seed_ };
};

bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2,
                                         const double& epsilon)
{
  for (std::size_t i = 0; i < 3; ++i)
    for (std::size_t j = 0; j < 4; ++j)
    {
      if (fabs(pose1(i, j) - pose2(i, j)) > fabs(epsilon))
        return false;
    }
  return true;
}

/**
 * @brief Parametrized class for tests with and without gripper.
 */
class TrajectoryFunctionsTestFlangeAndGripper : public TrajectoryFunctionsTestBase
{
};

// TODO(henningkayser): re-enable gripper tests
// /**
//  * @brief Parametrized class for tests, that only run with a gripper
//  */
// class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase
// {
// };

/**
 * @brief Test the forward kinematics function with simple robot poses for robot
 * tip link
 * using robot model without gripper.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
{
  Eigen::Isometry3d tip_pose;
  std::map<std::string, double> test_state = zero_state_;
  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose));
  EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON);
  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
  EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON);

  test_state[joint_names_.at(1)] = M_PI_2;
  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose));
  EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON);
  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
  EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON);

  test_state[joint_names_.at(1)] = -M_PI_2;
  test_state[joint_names_.at(2)] = M_PI_2;
  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, group_tip_link_, test_state, tip_pose));
  EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON);
  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
  EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON);

  // wrong link name
  std::string link_name = "wrong_link_name";
  EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(robot_model_, link_name, test_state, tip_pose));
}

/**
 * @brief Test the inverse kinematics directly through ikfast solver
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
{
  // Load solver
  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();

  // robot state
  moveit::core::RobotState rstate(robot_model_);

  while (random_test_number_ > 0)
  {
    // sample random robot state
    rstate.setToRandomPositions(jmg, rng_);
    rstate.update();
    geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_));

    // prepare inverse kinematics
    std::vector<geometry_msgs::msg::Pose> ik_poses;
    ik_poses.push_back(pose_expect);
    std::vector<double> ik_seed, ik_expect, ik_actual;
    for (const auto& joint_name : jmg->getActiveJointModelNames())
    {
      ik_expect.push_back(rstate.getVariablePosition(joint_name));
      if (rstate.getVariablePosition(joint_name) > 0)
        ik_seed.push_back(rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET);
      else
        ik_seed.push_back(rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET);
    }

    std::vector<std::vector<double>> ik_solutions;
    kinematics::KinematicsResult ik_result;
    moveit_msgs::msg::MoveItErrorCodes err_code;
    kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions();

    // compute all ik solutions
    EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result, options));

    // compute one ik solution
    EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code));

    ASSERT_EQ(ik_expect.size(), ik_actual.size());

    for (std::size_t i = 0; i < ik_expect.size(); ++i)
    {
      EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET);
    }

    --random_test_number_;
  }
}

/**
 * @brief Test the inverse kinematics using RobotState class (setFromIK) using
 * robot model
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKRobotState)
{
  // robot state
  moveit::core::RobotState rstate(robot_model_);
  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);

  while (random_test_number_ > 0)
  {
    // sample random robot state
    rstate.setToRandomPositions(jmg, rng_);

    Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);

    // copy the random state and set ik seed
    std::map<std::string, double> ik_seed, ik_expect;
    for (const auto& joint_name : joint_names_)
    {
      ik_expect[joint_name] = rstate.getVariablePosition(joint_name);
      if (rstate.getVariablePosition(joint_name) > 0)
        ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET;
      else
        ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET;
    }

    rstate.setVariablePositions(ik_seed);
    rstate.update();

    // compute the ik
    std::map<std::string, double> ik_actual;

    EXPECT_TRUE(rstate.setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_));

    for (const auto& joint_name : joint_names_)
    {
      ik_actual[joint_name] = rstate.getVariablePosition(joint_name);
    }

    // compare ik solution and expected value
    for (const auto& joint_pair : ik_actual)
    {
      EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
    }

    // compute the pose from ik_solution
    rstate.setVariablePositions(ik_actual);
    rstate.update();
    Eigen::Isometry3d pose_actual = rstate.getFrameTransform(tcp_link_);

    EXPECT_TRUE(tfNear(pose_expect, pose_actual, EPSILON));

    --random_test_number_;
  }
}

/**
 * @brief Test the wrapper function to compute inverse kinematics using robot
 * model
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
{
  // robot state
  moveit::core::RobotState rstate(robot_model_);

  const std::string frame_id = robot_model_->getModelFrame();
  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);

  while (random_test_number_ > 0)
  {
    // sample random robot state
    rstate.setToRandomPositions(jmg, rng_);

    Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);

    // copy the random state and set ik seed
    std::map<std::string, double> ik_seed, ik_expect;
    for (const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames())
    {
      ik_expect[joint_name] = rstate.getVariablePosition(joint_name);
      if (rstate.getVariablePosition(joint_name) > 0)
      {
        ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET;
      }
      else
      {
        ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET;
      }
    }

    // compute the ik
    std::map<std::string, double> ik_actual;
    EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                              frame_id, ik_seed, ik_actual, false));

    // compare ik solution and expected value
    for (const auto& joint_pair : ik_actual)
    {
      EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
    }

    --random_test_number_;
  }
}

/**
 * @brief Test computePoseIK for invalid group_name
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
{
  const std::string frame_id = robot_model_->getModelFrame();
  Eigen::Isometry3d pose_expect;

  std::map<std::string, double> ik_seed;

  // compute the ik
  std::map<std::string, double> ik_actual;
  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, "InvalidGroupName", tcp_link_,
                                                             pose_expect, frame_id, ik_seed, ik_actual, false));
}

/**
 * @brief Test computePoseIK for invalid link_name
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
{
  const std::string frame_id = robot_model_->getModelFrame();
  Eigen::Isometry3d pose_expect;

  std::map<std::string, double> ik_seed;

  // compute the ik
  std::map<std::string, double> ik_actual;
  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, "WrongLink", pose_expect,
                                                             frame_id, ik_seed, ik_actual, false));
}

/**
 * @brief Test computePoseIK for invalid frame_id
 *
 * Currently only robot_model_->getModelFrame() == frame_id
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
{
  Eigen::Isometry3d pose_expect;

  std::map<std::string, double> ik_seed;

  // compute the ik
  std::map<std::string, double> ik_actual;
  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                             "InvalidFrameId", ik_seed, ik_actual, false));
}

// /**
//  * @brief Test if activated self collision for a pose that would be in self
//  * collision without the check results in a
//  * valid ik solution.
//  */
// TEST_F(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition)
// {
//   const std::string frame_id = robot_model_->getModelFrame();
//   const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
//
//   // create seed
//   std::vector<double> ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 };
//   auto joint_names = jmg->getActiveJointModelNames();
//
//   std::map<std::string, double> ik_seed;
//   for (unsigned int i = 0; i < ik_seed_states.size(); ++i)
//   {
//     ik_seed[joint_names[i]] = ik_seed_states[i];
//   }
//
//   // create expected pose
//   geometry_msgs::msg::Pose pose;
//   pose.position.x = -0.454;
//   pose.position.y = -0.15;
//   pose.position.z = 0.431;
//   pose.orientation.y = 0.991562;
//   pose.orientation.w = -0.1296328;
//   Eigen::Isometry3d pose_expect;
//   normalizeQuaternion(pose.orientation);
//   tf2::fromMsg(pose, pose_expect);
//
//   // compute the ik without self collision check and expect the resulting pose
//   // to be in self collision.
//   std::map<std::string, double> ik_actual1;
//   EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
//                                                             frame_id, ik_seed, ik_actual1, false));
//
//   moveit::core::RobotState rstate(robot_model_);
//   planning_scene::PlanningScene rscene(robot_model_);
//
//   std::vector<double> ik_state;
//   std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state),
//                  boost::bind(&std::map<std::string, double>::value_type::second, _1));
//
//   rstate.setJointGroupPositions(jmg, ik_state);
//   rstate.update();
//
//   collision_detection::CollisionRequest collision_req;
//   collision_req.group_name = jmg->getName();
//   collision_detection::CollisionResult collision_res;
//
//   rscene.checkSelfCollision(collision_req, collision_res, rstate);
//
//   EXPECT_TRUE(collision_res.collision);
//
//   // compute the ik with collision detection activated and expect the resulting
//   // pose to be without self collision.
//   std::map<std::string, double> ik_actual2;
//   EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
//                                                             frame_id, ik_seed, ik_actual2, true));
//
//   std::vector<double> ik_state2;
//   std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2),
//                  boost::bind(&std::map<std::string, double>::value_type::second, _1));
//   rstate.setJointGroupPositions(jmg, ik_state2);
//   rstate.update();
//
//   collision_detection::CollisionResult collision_res2;
//   rscene.checkSelfCollision(collision_req, collision_res2, rstate);
//
//   EXPECT_FALSE(collision_res2.collision);
// }

/**
 * @brief Test if self collision is considered by using a pose that always has
 * self collision.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
{
  // robot state
  moveit::core::RobotState rstate(robot_model_);

  const std::string frame_id = robot_model_->getModelFrame();
  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);

  // create seed
  std::map<std::string, double> ik_seed;
  for (const auto& joint_name : jmg->getActiveJointModelNames())
  {
    ik_seed[joint_name] = 0;
  }

  // create goal
  std::vector<double> ik_goal = { 0, 2.3, -2.3, 0, 0, 0 };

  rstate.setJointGroupPositions(jmg, ik_goal);

  Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);

  // compute the ik with disabled collision check
  std::map<std::string, double> ik_actual;
  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                            frame_id, ik_seed, ik_actual, false));

  // compute the ik with enabled collision check
  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                             frame_id, ik_seed, ik_actual, true));
}

/**
 * @brief Check that function VerifySampleJointLimits() returns 'false' in case
 * of very small sample duration.
 *
 * Test Sequence:
 *    1. Call function with very small sample duration.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
{
  const std::map<std::string, double> position_last, velocity_last, position_current;
  double duration_last{ 0.0 };
  const pilz_industrial_motion_planner::JointLimitsContainer joint_limits;

  double duration_current = 10e-7;

  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
                                                                       duration_last, duration_current, joint_limits));
}

/**
 * @brief Check that function VerifySampleJointLimits() returns 'false' in case
 * of a velocity violation.
 *
 * Test Sequence:
 *    1. Call function with a velocity violation.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
{
  const std::string test_joint_name{ "joint" };

  std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
  std::map<std::string, double> position_current{ { test_joint_name, 10.0 } };
  std::map<std::string, double> velocity_last;
  double duration_current{ 1.0 };
  double duration_last{ 0.0 };
  pilz_industrial_motion_planner::JointLimitsContainer joint_limits;

  pilz_industrial_motion_planner::JointLimit test_joint_limits;
  // Calculate the max allowed velocity in such a way that it is always smaller
  // than the current velocity.
  test_joint_limits.max_velocity =
      ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0;
  test_joint_limits.has_velocity_limits = true;
  joint_limits.addLimit(test_joint_name, test_joint_limits);

  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
                                                                       duration_last, duration_current, joint_limits));
}

/**
 * @brief Check that function VerifySampleJointLimits() returns 'false' in case
 * of a acceleration violation.
 *
 * Test Sequence:
 *    1. Call function with a acceleration violation.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
{
  const std::string test_joint_name{ "joint" };

  double duration_current = 1.0;
  double duration_last = 1.0;

  std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
  std::map<std::string, double> position_current{ { test_joint_name, 20.0 } };
  double velocity_current =
      ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
  std::map<std::string, double> velocity_last{ { test_joint_name, 9.0 } };
  pilz_industrial_motion_planner::JointLimitsContainer joint_limits;

  pilz_industrial_motion_planner::JointLimit test_joint_limits;
  // Calculate the max allowed velocity in such a way that it is always bigger
  // than the current velocity.
  test_joint_limits.max_velocity = velocity_current + 1.0;
  test_joint_limits.has_velocity_limits = true;

  double acceleration_current =
      (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
  // Calculate the max allowed acceleration in such a way that it is always
  // smaller than the current acceleration.
  test_joint_limits.max_acceleration = acceleration_current - 1.0;
  test_joint_limits.has_acceleration_limits = true;

  joint_limits.addLimit(test_joint_name, test_joint_limits);

  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
                                                                       duration_last, duration_current, joint_limits));
}

/**
 * @brief Check that function VerifySampleJointLimits() returns 'false' in case
 * of a deceleration violation.
 *
 * Test Sequence:
 *    1. Call function with a deceleration violation.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
{
  const std::string test_joint_name{ "joint" };

  double duration_current = 1.0;
  double duration_last = 1.0;

  std::map<std::string, double> position_last{ { test_joint_name, 20.0 } };
  std::map<std::string, double> position_current{ { test_joint_name, 2.0 } };
  double velocity_current =
      ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
  std::map<std::string, double> velocity_last{ { test_joint_name, 19.0 } };
  pilz_industrial_motion_planner::JointLimitsContainer joint_limits;

  pilz_industrial_motion_planner::JointLimit test_joint_limits;
  // Calculate the max allowed velocity in such a way that it is always bigger
  // than the current velocity.
  test_joint_limits.max_velocity = fabs(velocity_current) + 1.0;
  test_joint_limits.has_velocity_limits = true;

  double acceleration_current =
      (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
  // Calculate the max allowed deceleration in such a way that it is always
  // bigger than the current acceleration.
  test_joint_limits.max_deceleration = acceleration_current + 1.0;
  test_joint_limits.has_deceleration_limits = true;

  joint_limits.addLimit(test_joint_name, test_joint_limits);

  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
                                                                       duration_last, duration_current, joint_limits));
}

/**
 * @brief Check that function generateJointTrajectory() returns 'false' if
 * a joint trajectory cannot be computed from a cartesian trajectory.
 *
 * Please note: Both function variants are tested in this test.
 *
 * Test Sequence:
 *    1. Call function with a cartesian trajectory which cannot be transformed
 * into a joint trajectory by using
 *        an invalid group_name.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
{
  // Create random test trajectory
  // Note: 'path' is deleted by KDL::Trajectory_Segment
  KDL::Path_RoundedComposite* path =
      new KDL::Path_RoundedComposite(0.2, 0.01, new KDL::RotationalInterpolation_SingleAxis());
  path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0)));
  path->Finish();
  // Note: 'velprof' is deleted by KDL::Trajectory_Segment
  KDL::VelocityProfile* vel_prof = new KDL::VelocityProfile_Trap(0.5, 0.1);
  vel_prof->SetProfile(0, path->PathLength());
  KDL::Trajectory_Segment kdl_trajectory(path, vel_prof);

  pilz_industrial_motion_planner::JointLimitsContainer joint_limits;
  std::string group_name{ "invalid_group_name" };
  std::map<std::string, double> initial_joint_position;
  double sampling_time{ 0.1 };
  trajectory_msgs::msg::JointTrajectory joint_trajectory;
  moveit_msgs::msg::MoveItErrorCodes error_code;
  bool check_self_collision{ false };

  EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory(
      planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
      joint_trajectory, error_code, check_self_collision));

  std::map<std::string, double> initial_joint_velocity;

  pilz_industrial_motion_planner::CartesianTrajectory cart_traj;
  cart_traj.group_name = group_name;
  cart_traj.link_name = tcp_link_;
  pilz_industrial_motion_planner::CartesianTrajectoryPoint cart_traj_point;
  cart_traj.points.push_back(cart_traj_point);

  EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory(
      planning_scene_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
      joint_trajectory, error_code, check_self_collision));
}

/**
 * @brief Check that function determineAndCheckSamplingTime() returns 'false' if
 * both of the needed vectors have an incorrect vector size.
 *
 *
 * Test Sequence:
 *    1. Call function with vectors of incorrect size.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
{
  robot_trajectory::RobotTrajectoryPtr first_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  robot_trajectory::RobotTrajectoryPtr second_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  double epsilon{ 0.0 };
  double sampling_time{ 0.0 };

  moveit::core::RobotState rstate(robot_model_);
  first_trajectory->insertWayPoint(0, rstate, 0.1);
  second_trajectory->insertWayPoint(0, rstate, 0.1);

  EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
                                                                             epsilon, sampling_time));
}

/**
 * @brief Check that function determineAndCheckSamplingTime() returns 'true' if
 * sampling time is correct.
 *
 *
 * Test Sequence:
 *    1. Call function with trajectories which do NOT violate sampling time.
 *
 * Expected Results:
 *    1. Function returns 'true'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
{
  robot_trajectory::RobotTrajectoryPtr first_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  robot_trajectory::RobotTrajectoryPtr second_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  double epsilon{ 0.0001 };
  double sampling_time{ 0.0 };
  double expected_sampling_time{ 0.1 };

  moveit::core::RobotState rstate(robot_model_);
  first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
  first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);

  second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
  second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
  second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);

  EXPECT_TRUE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
                                                                            epsilon, sampling_time));
  EXPECT_EQ(expected_sampling_time, sampling_time);
}

/**
 * @brief Check that function determineAndCheckSamplingTime() returns 'false' if
 * sampling time is violated.
 *
 *
 * Test Sequence:
 *    1. Call function with trajectories which violate sampling time.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
{
  robot_trajectory::RobotTrajectoryPtr first_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  robot_trajectory::RobotTrajectoryPtr second_trajectory =
      std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
  double epsilon{ 0.0001 };
  double sampling_time{ 0.0 };
  double expected_sampling_time{ 0.1 };

  moveit::core::RobotState rstate(robot_model_);
  first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
  first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
  first_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
  // Violate sampling time
  first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0);
  first_trajectory->insertWayPoint(3, rstate, expected_sampling_time);

  second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
  second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
  second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
  second_trajectory->insertWayPoint(3, rstate, expected_sampling_time);

  EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
                                                                             epsilon, sampling_time));
  EXPECT_EQ(expected_sampling_time, sampling_time);
}

/**
 * @brief Check that function isRobotStateEqual() returns 'false' if
 * the positions of the robot states are not equal.
 *
 *
 * Test Sequence:
 *    1. Call function with robot states with different positions.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
{
  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);

  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
  // Ensure that the joint positions of both robot states are different
  default_joint_position[0] = default_joint_position[0] + 70.0;
  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);

  double epsilon{ 0.0001 };
  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
}

/**
 * @brief Check that function isRobotStateEqual() returns 'false' if
 * the velocity of the robot states are not equal.
 *
 *
 * Test Sequence:
 *    1. Call function with robot states with different velocities.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
{
  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);

  // Ensure that the joint positions of both robot state are equal
  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);

  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
  // Ensure that the joint velocites of both robot states are different
  default_joint_velocity[1] = default_joint_velocity[1] + 10.0;
  rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity);

  double epsilon{ 0.0001 };
  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
}

/**
 * @brief Check that function isRobotStateEqual() returns 'false' if
 * the acceleration of the robot states are not equal.
 *
 *
 * Test Sequence:
 *    1. Call function with robot states with different acceleration.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
{
  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);

  // Ensure that the joint positions of both robot state are equal
  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);

  // Ensure that the joint velocities of both robot state are equal
  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
  rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity);

  double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration);
  // Ensure that the joint accelerations of both robot states are different
  default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0;
  rstate_2.setJointGroupAccelerations(planning_group_, default_joint_acceleration);

  double epsilon{ 0.0001 };
  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
}

/**
 * @brief Check that function isRobotStateStationary() returns 'false' if
 * the joint velocities are not equal to zero.
 *
 *
 * Test Sequence:
 *    1. Call function with robot state with joint velocities != 0.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
{
  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);

  // Ensure that the joint velocities are NOT zero
  double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);

  double epsilon{ 0.0001 };
  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon));
}

/**
 * @brief Check that function isRobotStateStationary() returns 'false' if
 * the joint acceleration are not equal to zero.
 *
 *
 * Test Sequence:
 *    1. Call function with robot state with joint acceleration != 0.
 *
 * Expected Results:
 *    1. Function returns 'false'.
 */
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
{
  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);

  // Ensure that the joint velocities are zero
  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);

  // Ensure that the joint acceleration are NOT zero
  double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration);

  double epsilon{ 0.0001 };
  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon));
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

ext: cpp


text:
// Copyright @2012 JDSU Xtreme

#include <assert.h>

#include "xtreme/mmus/simple_content_handler.h"
#include "xtreme/mmus/mmus_web_processor.h"
#include "xtreme/mmus/types.h"
#include "xtreme/mmus/xml_parser.h"
#include "xtreme/mmus/parameter_mgr.h"
#include "xtreme/common/external_req.h"
#include "xtreme/common/external_resp.h"
#include "xtreme/common/webserver/types.h"
#include <boost/lexical_cast.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <deque>

namespace xtreme {
namespace mmus {

int CmMUSWebProcessor::Process(const CMapHttpArgPair &args_pair, std::string &out) {
  using namespace xtreme::webserver;
  CMapHttpArgPair::const_iterator itor = args_pair.find(CUtil::REQ_DATA_KEY);
  if (itor == args_pair.end()) {
    MMUS_ERROR("ERROR: Can't find req data with key " << CUtil::REQ_DATA_KEY);
    out = "<resp result_code=\"1\"> <info>No data with key req</info></resp>";
    return MHD_HTTP_OK;
  }

  const std::string & data_str = itor->second;
  MMUS_DEBUG("key = " << CUtil::REQ_DATA_KEY);
  MMUS_DEBUG("data = " << data_str);

  std::map<std::string, std::string> value_map;
  bool parse_result = CXMLParser::Parse(data_str, value_map);
  MMUS_INFO("xml parser result = " << parse_result);
  if (!parse_result) {
    MMUS_ERROR("Wrong xml format");
    std::ostringstream stream;
    stream << "<resp result_code=\"" << xtreme::kWrongXmlFormat << "\">\n</resp>";
    return MHD_HTTP_OK;
  }

  std::string http_ip;
  CMapHttpArgPair::const_iterator itor_http_args = args_pair.find(CWebServerUtil::HTTP_IP_NAME);
  if (itor_http_args == args_pair.end()) {
    MMUS_ERROR("No client IP founded in the map");
  } else {
    http_ip = itor_http_args->second;
  }
  MMUS_DEBUG("http_ip = " << http_ip);

  std::string token = value_map[CUtil::REQ_ATTR_SESSION_TOKEN_NAME];
  MMUS_DEBUG("token = " << token);
  std::string category = value_map[CUtil::REQ_ATTR_CAT_NAME];
  MMUS_DEBUG("category = " << category);

  // process the
  if (category == CParameterMgr::GetInstance()->hello_msg_category()) {
    std::string ipaddr;
    unsigned int port;
    if (GetIpAndPort(data_str, ipaddr, port) == false) {
      std::ostringstream stream;
      stream << "<resp result_code=\"" << xtreme::kWrongIpOrPort << "\">\n</resp>";
      return MHD_HTTP_OK;
    }
    MMUS_DEBUG("ipaddr = " << ipaddr);
    MMUS_DEBUG("port = " << port);
    CParameterMgr::GetInstance()->SetCmdIpPort(ipaddr, port);
    std::ostringstream stream;
    stream << "<resp result_code=\"" << xtreme::kOK << "\">\n</resp>";
    return MHD_HTTP_OK;
  }

  xtreme::EnExternalReqType en_cat = GetReqtypeByName(category);
  xtreme::CExternalReq req(http_ip, token, en_cat, data_str);

  static xtreme::cs::CS * cs = CParameterMgr::GetInstance()->get_cs();
  xtreme::ExternalRespPtr resp = cs->ProcessReq(req);
  MMUS_DEBUG("resp->result_code_ = " << resp->result_code_);
  MMUS_DEBUG("resp->info_ = " << resp->info_);

  out = resp->info_;

  return MHD_HTTP_OK;
}

xtreme::EnExternalReqType CmMUSWebProcessor::GetReqtypeByName(std::string name) {
  MMUS_DEBUG("name = " << name);
  if (CUtil::CaseInsensitiveStringCompare(name, "job")) {
    // return xtreme::KCMDMusJob;
	  return xtreme::kMMusJob;
  }
  else if (CUtil::CaseInsensitiveStringCompare(name, "config")) {
	  return xtreme::kMMusConfig;
  }
  else if (CUtil::CaseInsensitiveStringCompare(name, "refdata")) {
    return xtreme::kMMusRefdata;
  }

  MMUS_DEBUG("unsupported external req name " << name);
  return xtreme::kUnknowReqType;
}

bool CmMUSWebProcessor::GetIpAndPort(const std::string& req, std::string& ip, unsigned int& port) {
  boost::property_tree::ptree pt;
  std::stringstream ss(req); // put string into stream
  try {
    boost::property_tree::xml_parser::read_xml(ss, pt);
    ip = pt.get<std::string>("req.ipaddr");
    std::string str_port = pt.get<std::string>("req.port");
    port = boost::lexical_cast<unsigned int>(str_port);
  } catch (boost::property_tree::xml_parser::xml_parser_error &ex) {
    MMUS_ERROR(ex.message());
    return false;
  }
  return true;
}

}}

ext: cc


text:
/*
  Copyright (c) 2012 by Procera Networks, Inc. ("PROCERA")

  Permission to use, copy, modify, and/or distribute this software for
  any purpose with or without fee is hereby granted, provided that the
  above copyright notice and this permission notice appear in all
  copies.

  THE SOFTWARE IS PROVIDED "AS IS" AND PROCERA DISCLAIMS ALL WARRANTIES
  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL PROCERA BE LIABLE FOR
  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT
  OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#include "koi.hpp"
#include "msg.hpp"
#include "archive.hpp"
#include "crypt.hpp"
#include "sha1.hpp"

#include "hex.hpp"
#include "strfmt.hpp"
#include "static_vector.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/variant.hpp>

#define MINIZ_HEADER_FILE_ONLY
#include "miniz.c"

using namespace std;
using namespace boost;
using namespace koi;
using namespace boost::posix_time;
using namespace boost::uuids;

namespace {
	static const int COMPRESSION_THRESHOLD = 500; // messages larger than this are compressed

	struct msg_error : public std::exception {
		static const int SIZE = 1024;
		char _msg[SIZE];
		explicit msg_error(const char* fmt, ...) {
			va_list va_args;
			va_start(va_args, fmt);
			vsnprintf(_msg, SIZE, fmt, va_args);
			va_end(va_args);
		}
		char const* what() const throw() {
			return _msg;
		}
	};

	SHA1::digest hashpass(const char* pass) {
		SHA1 sha1((const uint8_t*)pass, strlen(pass));
		return sha1.end();
	}

	void append_nonce(vector<uint8_t>& to, int nonce) {
		const size_t tosize = to.size();
		to.resize(tosize + 4);
		to[tosize] = nonce&0xff;
		to[tosize+1] = (nonce>>8)&0xff;
		to[tosize+2] = (nonce>>16)&0xff;
		to[tosize+3] = (nonce>>24)&0xff;
	}

	int extract_nonce(vector<uint8_t>& from) {
		if (from.size() < 4)
			return 0;
		const size_t fromsize = from.size() - 4;
		const int nonce = (from[fromsize]&0xff) | ((from[fromsize+1]&0xff)<<8) | ((from[fromsize+2]&0xff)<<16) | ((from[fromsize+3]&0xff)<<24);
		from.resize(fromsize);
		return nonce;
	}

	struct response_appender : public static_visitor<> {
		response_appender() : static_visitor<>(), _builder(0) {}
		response_appender(const response_appender& a) : static_visitor<>(), _builder(a._builder) {}
		response_appender(archive& a) : _builder(&a) {}
		void operator()(bool value) const { _builder->append(value); }
		void operator()(int value) const { _builder->append(value); }
		void operator()(const string& value) const { _builder->append(value); }
		void operator()(const uuids::uuid& value) const { _builder->append(value); }
		void operator()(const vector<string>& value) const { _builder->append(value); }
		void operator()(const ptime& value) const { _builder->append(value); }
		void operator()(const vector<uint8_t>& value) const { _builder->append(value.data(), value.size()); }

		archive* _builder;
	};

	using namespace koi;
	using namespace koi::msg;

	void write_archive(archive& a, const healthreport* hr) {
		a << hr->_name
		  << hr->_uptime
		  << (int)hr->_state
		  << (int)hr->_mode
		  << hr->_maintenance
		  << (int)hr->_service_action
		  << (int)hr->_services.size();
		FOREACH(const service_info& si, hr->_services)
			a << si._name
			  << si._event
			  << (int)si._state
			  << si._failed;
	}

	void read_archive(reader& r, healthreport* hr) {
		int state, mode;
		r >> hr->_name
		  >> hr->_uptime
		  >> state
		  >> mode;
		if (state < 0 || state > S_NumStates)
			throw msg_error("Invalid state: %d", state);

		if (mode < 0 || mode > R_NumModes)
			throw msg_error("Invalid mode: %d", mode);

		hr->_state = (koi::State)state;
		hr->_mode = (koi::RunnerMode)mode;

		r >> hr->_maintenance;

		int saction;
		r >> saction;
		if (validate_service_action(saction) < 0)
			throw msg_error("Invalid action: %d", saction);

		hr->_service_action = (koi::ServiceAction)saction;
		hr->_services.clear();
		uint32_t nservices;
		r >> nservices;
		for (size_t i = 0; i < (size_t)nservices; ++i) {
			int sstate;
			service_info inf;
			r >> inf._name
			  >> inf._event
			  >> sstate
			  >> inf._failed;
			if (validate_service_state(sstate) < 0)
				throw msg_error("Invalid service state: %d", sstate);

			inf._state = (koi::ServiceState)sstate;
			hr->_services.push_back(inf);
		}
	}

	void write_archive(archive& a, const stateupdate* su) {
		a << su->_uptime
		  << su->_master_uuid;
		if (!su->_master_uuid.is_nil()) {
			a << su->_master_last_seen
			  << su->_master_name
			  << su->_master_addr;
		}
	}

	void read_archive(reader& r, stateupdate* su) {
		r >> su->_uptime
		  >> su->_master_uuid;
		if (!su->_master_uuid.is_nil()) {
			r >> su->_master_last_seen
			  >> su->_master_name
			  >> su->_master_addr;
		}
		else {
			su->_master_last_seen = ptime(min_date_time);
			su->_master_name = "";
			su->_master_addr = net::endpoint();
		}
	}

	void write_archive(archive& a, const heartbeat* hb) {
		a << hb->_name
		  << hb->_flags;
		if (hb->_elector.is_nil()) {
			a << false;
		}
		else {
			a << true
			  << hb->_elector
			  << hb->_master
			  << hb->_cluster_maintenance
			  << (int)hb->_nodes.size();
			FOREACH(heartbeat::node const& n, hb->_nodes) {
				a << n._id << n._name << n._last_seen << n._flags << n._addrs;
			}
		}
	}

	void read_archive(reader& r, heartbeat* hb) {
		bool state;
		hb->_nodes.clear();
		r >> hb->_name
		  >> hb->_flags
		  >> state;
		if (state) {
			int nnodes;
			r >> hb->_elector
			  >> hb->_master
			  >> hb->_cluster_maintenance
			  >> nnodes;
			// TODO: range-check nnodes
			for (int i = 0; i < nnodes; ++i) {
				heartbeat::node n;
				r >> n._id >> n._name >> n._last_seen >> n._flags >> n._addrs;
				hb->_nodes.push_back(n);
			}
		}
		else {
			hb->_elector = nil_uuid();
			hb->_master = nil_uuid();
		}
	}

	void write_archive(archive& a, const request* rq) {
		a << rq->_cmd
		  << rq->_args;
	}

	void read_archive(reader& r, request* rq) {
		r >> rq->_cmd
		  >> rq->_args;
	}

	void write_archive(archive& a, const response* rs) {
		FOREACH(const response::values::value_type& e, rs->_response) {
			a << e.first;
			apply_visitor(response_appender(a), e.second);
		}
	}

	void read_archive(reader& r, response* rs) {
		// todo: clean up
		archive::iterator i = r.get();
		string fieldname;
		for (; i != r.end(); ++i) {
			fieldname = i.get<string>();
			++i;
			switch (i.type()) {
			case SmallString:
			case String:
				rs->_response[fieldname] = i.get<string>();
				break;
			case Bool:
				rs->_response[fieldname] = i.get<bool>();
				break;
			case SmallInt:
			case Int:
			case Uint8:
			case Uint16:
				rs->_response[fieldname] = i.get<int>();
				break;
			case List: {
				vector<string> vals;
				i.contents(vals);
				rs->_response[fieldname] = vals;
			}break;
			case UUID: {
				rs->_response[fieldname] = i.get<boost::uuids::uuid>();
			} break;
			case NilUUID: {
				rs->_response[fieldname] = nil_uuid();
			} break;
			case PosixTime: {
				rs->_response[fieldname] = i.get<boost::posix_time::ptime>();
			} break;
			case RawData: {
				typedef std::vector<uint8_t> bytes;
				size_t len = i.size();
				if (len > MAX_MSG_LEN) {
					LOG_ERROR("Size error in raw data: %lu bytes for: %s",
					          (unsigned long)len,
					          fieldname.c_str());
					break;
				}
				else if (len == 0) {
					rs->_response[fieldname] = bytes();
				}
				else {
					rs->_response[fieldname] = bytes(i.body(), i.body() + len);
				}
			} break;
			default:
				LOG_ERROR("Can't handle data type: %s for field: %s",
				          i.type_name(),
				          fieldname.c_str());
				break;
			}
		}
	}

	template <typename Body>
	void read_message_body(message* msg, reader& r) {
		Body* b = new Body;
		msg->_body.reset(b);
		read_archive(r, b);
	}

	// this only works because koi is single-threaded..
	static static_vector<uint8_t, MAX_MSG_LEN*10> compression_buffer;
	void perhaps_compress(vector<uint8_t>& to, const archive& a);
	void perhaps_decompress(vector<uint8_t>& from);

	void perhaps_compress(vector<uint8_t>& to, const archive& a) {
		if (a.size() > COMPRESSION_THRESHOLD) {
			compression_buffer.resize_max();
			unsigned long outsize = compression_buffer.size();
			int zip_ret = mz_compress(&compression_buffer.front(), &outsize, a.data(), a.size());
			if ((zip_ret == MZ_OK) && (outsize < MAX_MSG_LEN - 4)) {
				if (debug_mode) {
					LOG_TRACE("compressed from %d to %d bytes", (int)a.size(), (int)outsize);
				}

				compression_buffer.resize(outsize);
				to.clear();
				const uint8_t h[] = {0x80, 0, 0, 0};
				to.insert(to.end(), h, h+4);
				to.insert(to.end(), compression_buffer.begin(), compression_buffer.end());
			}
			else {
				LOG_ERROR("Failed to compress message: %s (%lu > %d bytes).", (zip_ret==MZ_OK)?"":mz_error(zip_ret), (unsigned long)outsize, MAX_MSG_LEN);
				to.clear();
				to.insert(to.end(), a.data(), a.data() + a.size());
			}
		}
		else {
			to.clear();
			to.insert(to.end(), a.data(), a.data() + a.size());
		}
	}

	void perhaps_decompress(vector<uint8_t>& from) {
		if ((from[0] == 0x80) &&
		    (from[1] == 0) &&
		    (from[2] == 0) &&
		    (from[3] == 0)) {
			compression_buffer.resize_max();
			unsigned long decomp_size = compression_buffer.size();
			int zip_ret = mz_uncompress(&compression_buffer.front(), &decomp_size, &(from[4]), from.size()-4);
			if (zip_ret != MZ_OK) {
				throw msg_error("Failed to decompress message: %s", mz_error(zip_ret));
			}
			if (debug_mode) {
				LOG_TRACE("decompressed from %d to %d bytes", (int)from.size()-4, (int)decomp_size);
			}
			compression_buffer.resize(decomp_size);
			from.clear();
			from.insert(from.end(), compression_buffer.begin(), compression_buffer.end());
		}
	}
}

namespace koi {
	inline
	net::endpoint read_endpoint(const archive::iterator& i) {
		using namespace boost::asio::ip;
		size_t nbytes = i.size();
		const uint8_t* data = i.body();
		net::endpoint ep;
		if (nbytes == 18) {
			address_v6::bytes_type b;
			memcpy(b.begin(), data, 16);
			ep.address(address(address_v6(b)));
			ep.port((data[16]<<8)|data[17]);
		}
		else if (nbytes == 6) {
			address_v4::bytes_type b;
			memcpy(b.begin(), data, 4);
			ep.address(address(address_v4(b)));
			ep.port((data[4]<<8)|data[5]);
		}
		return ep;
	}

	archive& operator<<(archive& a, const net::endpoint& ep) {
		if (ep.address().is_v6()) {
			boost::asio::ip::address_v6::bytes_type b;
			b = ep.address().to_v6().to_bytes();
			uint8_t buf[18];
			memcpy(buf, b.begin(), 16);
			buf[16] = (ep.port()>>8)&0xff;
			buf[17] = ep.port()&0xff;
			a.append(buf, 18);
		}
		else {
			boost::asio::ip::address_v4::bytes_type b;
			b = ep.address().to_v4().to_bytes();
			uint8_t buf[6];
			memcpy(buf, b.begin(), 4);
			buf[4] = (ep.port()>>8)&0xff;
			buf[5] = ep.port()&0xff;
			a.append(buf, 6);
		}
		return a;
	}

	reader& operator>>(reader& r, net::endpoint& ep) {
		ep = read_endpoint(r.get());
		++r;
		return r;
	}

	archive& operator<<(archive& a, const mru<net::endpoint>& m) {
		std::vector<net::endpoint> addrs;
		for (auto i = m.begin(); i != m.end(); ++i) {
			addrs.push_back(*i);
		}
		a << addrs;
		return a;
	}

	reader& operator>>(reader& r, mru<net::endpoint>& m) {
		auto addrs = r.get();
		if (!addrs.is_list()) {
			throw archive_error("list expected", addrs.type_name(), addrs.size());
		}
		else {
			for (auto i = addrs.begin(); i != addrs.end(); ++i) {
				m.insert(read_endpoint(i));
			}
		}
		r.skip();
		return r;
	}

	namespace msg {
		bool encode(vector<uint8_t>& to, const message* msg, const string& pass) {
			try {
				to.resize(0);

				archive a;
				a << (int)koi::version
				  << msg->_seqnr
				  << (int)msg->_op
				  << msg->_cluster_id
				  << msg->_sender_uuid;

				switch(msg->_op) {
				case base::HealthReport:
					write_archive(a, msg->body<healthreport>());
					break;
				case base::StateUpdate:
					write_archive(a, msg->body<stateupdate>());
					break;
				case base::Request:
					write_archive(a, msg->body<request>());
					break;
				case base::Response:
					write_archive(a, msg->body<response>());
					break;
				case base::HeartBeat:
					write_archive(a, msg->body<heartbeat>());
					break;
				default:
					throw msg_error("Invalid message type: %d", msg->_op);
				};

				a.done();
				if (!a.valid())
					throw msg_error("%s", "Invalid archive");

				if (debug_mode) {
					string repr = a.to_string();
					LOG_TRACE("encode: %s [%d bytes]", repr.c_str(), a.size());
				}

				perhaps_compress(to, a);

				// pad to 4 byte interval
				bytevector_pad4(to);

				// generate nonce
				static boost::mt19937 random_engine;
				static boost::variate_generator<boost::mt19937&, boost::uniform_int<>> mkrandom(random_engine, boost::uniform_int<>(0, INT_MAX));
				int nonce = mkrandom();
				string nonces = pass + lexical_cast<string>(nonce);

				// encrypt w pass + nonce
				SHA1::digest pwd = hashpass(nonces.c_str());
				if (!crypto::encrypt(&to.front(), to.size(), pwd._data32, 5))
					throw msg_error("%s", "Failed to encrypt");

				append_nonce(to, nonce);

				if (to.size() > MAX_MSG_LEN)
					throw msg_error("length > max: %d > %d", (int)to.size(), MAX_MSG_LEN);

				return true;
			}
			catch (archive_error const& e) {
				LOG_ERROR("Failed to encode message into archive: %s (%s : %zu)", e.what(), e.type, e.size);
				return false;
			}
			catch (std::exception const& e) {
				LOG_ERROR("Encode error: %s", e.what());
				return false;
			}
		}

		bool decode(message* msg, vector<uint8_t>& from, const string& pass) {
			try {
				if (from.size() > MAX_MSG_LEN)
					throw msg_error("length > max: %d > %d", (int)from.size(), MAX_MSG_LEN);

				// drop nonce
				int nonce = extract_nonce(from);
				strfmt<64> nonces("%s%d", pass.c_str(), nonce);

				// decrypt
				SHA1::digest pwd = hashpass(nonces.c_str());
				if (!crypto::decrypt(&from.front(), from.size(), pwd._data32, 5))
					throw msg_error("Decrypt failed for size %d", (int)from.size());

				perhaps_decompress(from);

				archive a(from);
				reader r(a);

				if (!a.valid()) {
					string hd = a.hexdump();
					LOG_ERROR("Invalid archive: %s", hd.c_str());
					throw msg_error("%s", "Invalid archive");
				}

				if (debug_mode) {
					string repr = a.to_string();
					LOG_TRACE("decode: %s", repr.c_str());
				}

				r >> msg->_version;
				if (msg->_version != koi::version)
					throw msg_error("Version mismatch: our (%d) != msg (%d)", koi::version, msg->_version);

				r >> msg->_seqnr;

				int op;
				r >> op;
				if (!(op >= 0 && op < base::NumOps))
					throw msg_error("Unknown operation: %d", op);

				msg->_op = (base::Type)op;

				r >> msg->_cluster_id
				  >> msg->_sender_uuid;

				switch (msg->_op) {
				case base::HealthReport:
					read_message_body<healthreport>(msg, r);
					break;
				case base::StateUpdate:
					read_message_body<stateupdate>(msg, r);
					break;
				case base::Request:
					read_message_body<request>(msg, r);
					break;
				case base::Response:
					read_message_body<response>(msg, r);
					break;
				case base::HeartBeat:
					read_message_body<heartbeat>(msg, r);
					break;
				default:
					throw msg_error("Invalid message type: %d", msg->_op);
				}

				return true;
			}
			catch (archive_error const& e) {
				LOG_ERROR("Assert when unspooling archive: %s (%s : %zu)", e.what(), e.type, e.size);
				return false;
			}
			catch (std::exception const& e) {
				LOG_ERROR("Decode error: %s", e.what());
				return false;
			}
		}
	}
}

max_stars_repo_name: yogarajbaskaravel/koi

max_issues_repo_name: yogarajbaskaravel/koi

max_forks_repo_name: yogarajbaskaravel/koi


text:
/*
  KeePass Password Safe - The Open-Source Password Manager
  Copyright (C) 2003-2021 Dominik Reichl <dominik.reichl@t-online.de>

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "StdAfx.h"
#include "StrUtil.h"

#include "AppUtil.h"
#include "MemUtil.h"
#include "../Crypto/MemoryProtectionEx.h"

#ifndef _WIN32_WCE
#pragma warning(push)
#pragma warning(disable: 4996) // SCL warning
#include <boost/algorithm/string.hpp>
#pragma warning(pop)
#endif

static std::vector<WCHAR> g_vNormHyphen;

// Securely erase a CString object
void EraseCString(CString* pString)
{
	if(pString == NULL) { ASSERT(FALSE); return; }

	const int n = pString->GetLength();
	if(n <= 0) return;

	LPTSTR lp = pString->GetBuffer(0);
	SecureZeroMemory(lp, n * sizeof(TCHAR));
	pString->ReleaseBuffer();
}

void EraseWCharVector(std::vector<WCHAR>& vBuffer, bool bClear)
{
	const size_t n = vBuffer.size();
	if(n == 0) return;

	SecureZeroMemory(&vBuffer[0], n * sizeof(WCHAR));
	if(bClear) vBuffer.clear();
}

void EraseTCharVector(std::vector<TCHAR>& vBuffer, bool bClear)
{
	const size_t n = vBuffer.size();
	if(n == 0) return;

	SecureZeroMemory(&vBuffer[0], n * sizeof(TCHAR));
	if(bClear) vBuffer.clear();
}

void FixURL(CString *pstrURL)
{
	CString strTemp;
	BOOL bPre = FALSE;

	ASSERT(pstrURL != NULL);

	// Load string and make lower
	strTemp = *pstrURL;
	strTemp = strTemp.MakeLower();

	// If the string begins with one of the following prefixes, it is a URL
	if(strTemp.Left(5) == _T("http:")) bPre = TRUE;
	else if(strTemp.Left(6) == _T("https:")) bPre = TRUE;
	else if(strTemp.Left(4) == _T("cmd:")) bPre = TRUE;
	else if(strTemp.Left(4) == _T("ftp:")) bPre = TRUE;
	else if(strTemp.Left(5) == _T("file:")) bPre = TRUE;
	else if(strTemp.Left(7) == _T("gopher:")) bPre = TRUE;
	else if(strTemp.Left(7) == _T("mailto:")) bPre = TRUE;
	else if(strTemp.Left(5) == _T("news:")) bPre = TRUE;
	else if(strTemp.Left(5) == _T("nntp:")) bPre = TRUE;
	else if(strTemp.Left(9) == _T("prospero:")) bPre = TRUE;
	else if(strTemp.Left(7) == _T("telnet:")) bPre = TRUE;
	else if(strTemp.Left(5) == _T("wais:")) bPre = TRUE;
	else if(strTemp.Left(4) == _T("irc:")) bPre = TRUE;
	else
	{
		int nIndex = strTemp.Find(_T("://"));
		int nNoIndex = strTemp.FindOneOf(_T("/@;: \t\\"));

		if((nIndex != -1) && ((nNoIndex == -1) || (nNoIndex >= nIndex))) bPre = TRUE;
	}

	if(bPre == FALSE) // The string isn't a valid URL, so assume it's HTTP
	{
		strTemp = _T("http:");
		if(pstrURL->Left(1) != _T("/")) strTemp += _T("//");
		strTemp += *pstrURL;

		*pstrURL = strTemp;
	}
}

void _PwTimeToString(PW_TIME t, CString *pstrDest)
{
	CString strFormatted;
	_PwTimeToStringEx(t, strFormatted, FALSE);
	*pstrDest = strFormatted;
}

void _PwTimeToStringEx(const PW_TIME& t, CString& strDest, BOOL bUseLocalFormat)
{
	strDest.Empty();

	BOOL bUseIso = FALSE;

	if(bUseLocalFormat == TRUE)
	{
		SYSTEMTIME st;
		ZeroMemory(&st, sizeof(SYSTEMTIME));
		st.wYear = t.shYear;
		st.wMonth = t.btMonth;
		st.wDay = t.btDay;
		st.wHour = t.btHour;
		st.wMinute = t.btMinute;
		st.wSecond = t.btSecond;

		TCHAR tszBuf[32];

		tszBuf[0] = 0; tszBuf[1] = 0;
		if(GetDateFormat(LOCALE_USER_DEFAULT, DATE_SHORTDATE, &st, NULL,
			tszBuf, 31) == 0)
		{
			bUseIso = TRUE; ASSERT(FALSE);
		}
		else strDest += tszBuf;

		strDest += _T(" ");

		tszBuf[0] = 0; tszBuf[1] = 0;
		if(GetTimeFormat(LOCALE_USER_DEFAULT, 0, &st, NULL, tszBuf, 31) == 0)
		{
			bUseIso = TRUE; ASSERT(FALSE);
		}
		else strDest += tszBuf;
	}
	else bUseIso = TRUE;

	if(bUseIso == TRUE)
	{
		strDest.Format(_T("%04u-%02u-%02u %02u:%02u:%02u"), t.shYear,
			t.btMonth, t.btDay, t.btHour, t.btMinute, t.btSecond);
	}
}

void _PwTimeToXmlTime(PW_TIME t, CString *pstrDest)
{
	ASSERT(pstrDest != NULL);
	pstrDest->Empty();
	pstrDest->Format(_T("%04u-%02u-%02uT%02u:%02u:%02u"), t.shYear, t.btMonth,
		t.btDay, t.btHour, t.btMinute, t.btSecond);
}

void _UuidToString(const BYTE *pUuid, CString *pstrDest)
{
	CString strTemp;
	ASSERT(pstrDest != NULL);

	pstrDest->Empty();
	for(int i = 0; i < 16; ++i)
	{
		strTemp.Format(_T("%02x"), pUuid[i]);
		*pstrDest += strTemp;
	}
}

void _StringToUuid(const TCHAR *ptszSource, BYTE *pUuid)
{
	ASSERT((ptszSource != NULL) && (pUuid != NULL));
	if((ptszSource == NULL) || (pUuid == NULL)) return;

	// Check against invalid UUID
	ASSERT(_tcslen(ptszSource) == 32);
	if(_tcslen(ptszSource) != 32) return;

	for(DWORD i = 0; i < 16; ++i)
	{
		TCHAR ch = ptszSource[i << 1];

		BYTE bt;
		if((ch >= _T('0')) && (ch <= _T('9')))
			bt = static_cast<BYTE>(ch - _T('0'));
		else if((ch >= _T('a')) && (ch <= _T('f')))
			bt = static_cast<BYTE>(ch - _T('a') + 0x0A);
		else
		{
			ASSERT((ch >= _T('A')) && (ch <= _T('F')));
			bt = static_cast<BYTE>(ch - _T('A') + 0x0A);
		}

		bt <<= 4;

		ch = ptszSource[(i << 1) + 1];

		if((ch >= _T('0')) && (ch <= _T('9')))
			bt |= static_cast<BYTE>(ch - _T('0'));
		else if((ch >= _T('a')) && (ch <= _T('f')))
			bt |= static_cast<BYTE>(ch - _T('a') + 0x0A);
		else
		{
			ASSERT((ch >= _T('A')) && (ch <= _T('F')));
			bt |= static_cast<BYTE>(ch - _T('A') + 0x0A);
		}

		pUuid[i] = bt;
	}
}

// If pReferenceSource is not NULL, it'll be used to dereference
// lpReplaceWith before replacing lpFind
/* BOOL SeqReplace(CString& str, LPCTSTR lpFind, LPCTSTR lpReplaceWith,
	BOOL bMakeSimString, BOOL bCmdQuotes, BOOL bRemoveMeta, PW_ENTRY* peEntryInfo,
	CPwManager* pReferenceSource, DWORD dwRecursionLevel)
{
	ASSERT(lpFind != NULL); if(lpFind == NULL) return FALSE;
	ASSERT(lpReplaceWith != NULL); if(lpReplaceWith == NULL) return FALSE;

	const int nFindLen = static_cast<int>(_tcslen(lpFind));
	CString strFindLower = lpFind;
	strFindLower = strFindLower.MakeLower();

	BOOL bReplaced = FALSE;
	for(int iLoop = 0; iLoop < 20; ++iLoop)
	{
		const int nPos = str.Find(lpFind);
		if(nPos < 0) break;

		const int nStrLen = str.GetLength();

		CString strTemp = lpReplaceWith;
		bool bDoFilter = true;

		if((pReferenceSource != NULL) && (strTemp.GetLength() > 0))
		{
			CString strInner = strTemp;
			ParseURL(&strInner, peEntryInfo, bMakeSimString, bCmdQuotes, pReferenceSource,
				dwRecursionLevel + 1);

			if(strInner != strTemp)
			{
				strTemp = strInner;
				bDoFilter = false;
			}
		}

		if(bDoFilter)
		{
			if(bRemoveMeta == TRUE) strTemp = CsRemoveMeta(&strTemp);

			if(bCmdQuotes == TRUE) strTemp.Replace(_T("\""), _T("\"\"\""));
		}

		if((bMakeSimString == FALSE) || (bDoFilter == false))
			str = str.Left(nPos) + strTemp + str.Right(nStrLen - nPos - nFindLen);
		else
			str = str.Left(nPos) + TagSimString(strTemp) + str.Right(nStrLen -
				nPos - nFindLen);

		bReplaced = TRUE;
	}

	return bReplaced;
} */

/* void ParseURL(CString *pString, PW_ENTRY *pEntry, BOOL bMakeSimString, BOOL bCmdQuotes,
	CPwManager* pDataSource, DWORD dwRecursionLevel)
{
	ASSERT(pString != NULL); if(pString == NULL) return;
	ASSERT_ENTRY(pEntry); if(pEntry == NULL) return;
	ASSERT(pDataSource != NULL); if(pDataSource == NULL) return;
	if(dwRecursionLevel >= PRL_MAX_DEPTH) return;

	CString str = *pString;

	TCHAR tszBufP[512];
	VERIFY(GetApplicationDirectory(tszBufP, 512 - 1, TRUE, FALSE));

	for(int iLoop = 0; iLoop < 20; ++iLoop)
	{
		BOOL b = FALSE;

		b |= SeqReplace(str, _T("{TITLE}"), pEntry->pszTitle, bMakeSimString,
			bCmdQuotes, FALSE, pEntry, pDataSource, dwRecursionLevel);
		b |= SeqReplace(str, _T("{USERNAME}"), pEntry->pszUserName, bMakeSimString,
			bCmdQuotes, FALSE, pEntry, pDataSource, dwRecursionLevel);
		b |= SeqReplace(str, _T("{URL}"), pEntry->pszURL, bMakeSimString,
			bCmdQuotes, FALSE, pEntry, pDataSource, dwRecursionLevel);

		pDataSource->UnlockEntryPassword(pEntry);
		CString strPwCopy = pEntry->pszPassword;
		pDataSource->LockEntryPassword(pEntry);
		b |= SeqReplace(str, _T("{PASSWORD}"), strPwCopy, bMakeSimString,
			bCmdQuotes, FALSE, pEntry, pDataSource, dwRecursionLevel);
		EraseCString(&strPwCopy);

		CString strNotes = ((pEntry->pszAdditional != NULL) ? pEntry->pszAdditional : _T(""));
		strNotes = CsRemoveMeta(&strNotes);
		b |= SeqReplace(str, _T("{NOTES}"), strNotes, bMakeSimString,
			bCmdQuotes, FALSE, pEntry, pDataSource, dwRecursionLevel);

		b |= SeqReplace(str, _T("{APPDIR}"), &tszBufP[0], bMakeSimString,
			bCmdQuotes, FALSE, NULL, NULL, dwRecursionLevel); // {APPDIR} can't contain reference

		str.Replace(_T("{CLEARFIELD}"), _T("{DELAY 150}{HOME}(+{END}){DEL}{DELAY 150}"));

		b |= FillRefPlaceholders(str, bMakeSimString, bCmdQuotes, pDataSource, dwRecursionLevel);

		if(b == FALSE) break;
	}

	if(bMakeSimString == TRUE)
	{
		CString strSourceCopy = str;
		EraseCString(&str);

		for(int nPos = 0; nPos < strSourceCopy.GetLength(); ++nPos)
		{
			const unsigned char uch = static_cast<unsigned char>(strSourceCopy.GetAt(nPos));

			if(uch > 0x7E)
			{
				str += _T("%({NUMPAD0}");

				CString strTemp;
				strTemp.Format(_T("%u"), uch);
				ASSERT(strTemp.GetLength() == 3);

				for(int i = 0; i < strTemp.GetLength(); i++)
				{
					str += _T("{NUMPAD");
					str += strTemp.GetAt(i);
					str += _T("}");
				}

				str += _T(")");
			}
			else str += static_cast<TCHAR>(uch);
		}
	}

	*pString = str;
} */

/* BOOL FillRefPlaceholders(CString& str, BOOL bMakeSimString, BOOL bCmdQuotes,
	CPwManager* pDataSource, DWORD dwRecursionLevel)
{
	ASSERT(pDataSource != NULL); if(pDataSource == NULL) return FALSE;

	UNREFERENCED_PARAMETER(bMakeSimString);

	LPCTSTR lpStart = _T("{REF:");
	const int nStartLen = static_cast<int>(_tcslen(lpStart));
	LPCTSTR lpEnd = _T("}");
	// const int nEndLen = static_cast<int>(_tcslen(lpEnd));

	BOOL bReplaced = FALSE;
	for(int iLoop = 0; iLoop < 20; ++iLoop)
	{
		const int nStart = str.Find(lpStart);
		if(nStart < 0) break;
		const int nEnd = str.Find(lpEnd, nStart);
		if(nEnd < 0) break;

		CString strRef = str.Mid(nStart + nStartLen, nEnd - nStart - nStartLen);
		if(strRef.GetLength() <= 4) break;
		if(strRef.GetAt(1) != _T('@')) break;
		if(strRef.GetAt(3) != _T(':')) break;

		const TCHAR tchScan = static_cast<TCHAR>(toupper(strRef.GetAt(2)));
		const TCHAR tchWanted = static_cast<TCHAR>(toupper(strRef.GetAt(0)));
		CString strID = strRef.Mid(4);

		DWORD dwFlags = 0;
		if(tchScan == _T('T')) dwFlags |= PWMF_TITLE;
		else if(tchScan == _T('U')) dwFlags |= PWMF_USER;
		else if(tchScan == _T('A')) dwFlags |= PWMF_URL;
		else if(tchScan == _T('P')) dwFlags |= PWMF_PASSWORD;
		else if(tchScan == _T('N')) dwFlags |= PWMF_ADDITIONAL;
		else if(tchScan == _T('I')) dwFlags |= PWMF_UUID;
		else break;

		const DWORD dwIndex = pDataSource->FindEx(strID, FALSE, dwFlags, 0, NULL);
		if(dwIndex != DWORD_MAX)
		{
			PW_ENTRY *pFound = pDataSource->GetEntry(dwIndex);
			ASSERT_ENTRY(pFound);

			CString strInsData;
			if(tchWanted == _T('T')) strInsData = pFound->pszTitle;
			else if(tchWanted == _T('U')) strInsData = pFound->pszUserName;
			else if(tchWanted == _T('A')) strInsData = pFound->pszURL;
			else if(tchWanted == _T('P'))
			{
				pDataSource->UnlockEntryPassword(pFound);
				strInsData = pFound->pszPassword;
				pDataSource->LockEntryPassword(pFound);
			}
			else if(tchWanted == _T('N'))
			{
				CString strNotes = pFound->pszAdditional;
				strInsData = CsRemoveMeta(&strNotes);
			}
			else if(tchWanted == _T('I')) _UuidToString(pFound->uuid, &strInsData);
			else break;

			ParseURL(&strInsData, pFound, FALSE, bCmdQuotes, pDataSource, dwRecursionLevel + 1);

			str = str.Left(nStart) + strInsData + str.Right(str.GetLength() -
				nEnd - 1);
			bReplaced = TRUE;
		}
		else break;
	}

	return bReplaced;
} */

CString CsRemoveMeta(CString *psString)
{
	CString str = _T(""), strLower;
	LPCTSTR lpRemove = NULL;

	ASSERT(psString != NULL); if(psString == NULL) return str;

	str = *psString;
	strLower = str; strLower = strLower.MakeLower();

	for(int i = 0; i < 2; ++i)
	{
		if(i == 0) lpRemove = _T("auto-type:");
		else if(i == 1) lpRemove = _T("auto-type-window:");

		int nPos = strLower.Find(lpRemove, 0);
		if(nPos != -1)
		{
			if(nPos != 0)
				if(strLower.GetAt(nPos - 1) == _T('\n')) nPos -= 1;
			if(nPos != 0)
				if(strLower.GetAt(nPos - 1) == _T('\r')) nPos -= 1;

			int nCount = strLower.Find(_T('\n'), (int)(nPos + _tcslen(lpRemove) - 1));
			if(nCount == -1) nCount = strLower.GetLength() - nPos;
			else nCount -= nPos - 1;

			strLower.Delete(nPos, nCount);
			str.Delete(nPos, nCount);
		}
	}

	return str;
}

CString CsFileOnly(const CString *psFilePath)
{
	ASSERT(psFilePath != NULL); if(psFilePath == NULL) return CString("");

	const int i = psFilePath->ReverseFind(_T('\\'));
	const int j = psFilePath->ReverseFind(_T('/'));
	const int k = ((i > j) ? i : j);

	if(k <= -1) return *psFilePath;

	return psFilePath->Right(psFilePath->GetLength() - k - 1);
}

/* #define LOCAL_NUMXMLCONV 7
TCHAR *MakeSafeXmlString(const TCHAR *ptString)
{
	size_t i, j;
	size_t dwNeededChars = 0, dwOutPos = 0;
	TCHAR tch;
	BOOL bFound;
	TCHAR *pFinal;

	TCHAR aChar[LOCAL_NUMXMLCONV] = {
		_T('<'), _T('>'), _T('&'), _T('\"'), _T('\''),
		_T('\r'), _T('\n')
	};

	TCHAR *pTrans[LOCAL_NUMXMLCONV] = {
		_T("<"), _T(">"), _T("&"), _T("""), _T("'"),
		_T("
"), _T("
")
	};

	ASSERT(ptString != NULL); if(ptString == NULL) return NULL;

	const size_t dwStringLen = _tcslen(ptString);
	for(i = 0; i < dwStringLen; ++i)
	{
		tch = ptString[i];

		bFound = FALSE;
		for(j = 0; j < LOCAL_NUMXMLCONV; ++j)
		{
			if(tch == aChar[j])
			{
				dwNeededChars += _tcslen(pTrans[j]);
				bFound = TRUE;
			}
		}
		if(bFound == FALSE) ++dwNeededChars;
	}

	pFinal = new TCHAR[dwNeededChars + 4];
	ASSERT(pFinal != NULL); if(pFinal == NULL) return NULL;

	for(i = 0; i < dwStringLen; ++i)
	{
		tch = ptString[i];

		bFound = FALSE;
		for(j = 0; j < LOCAL_NUMXMLCONV; ++j)
		{
			if(tch == aChar[j])
			{
				_tcscpy(&pFinal[dwOutPos], pTrans[j]);
				dwOutPos += _tcslen(pTrans[j]);
				bFound = TRUE;
			}
		}

		if(bFound == FALSE)
		{
			pFinal[dwOutPos] = tch;
			dwOutPos++;
		}
	}

	ASSERT(dwOutPos == dwNeededChars);
	pFinal[dwOutPos] = 0; pFinal[dwOutPos + 1] = 0;
	ASSERT(_tcslen(pFinal) == dwNeededChars);

	return pFinal;
} */

std::basic_string<TCHAR> MakeSafeXmlString(LPCTSTR lpString, DWORD dwFlags)
{
	std::basic_string<TCHAR> strRet;
	if(lpString == NULL) { ASSERT(FALSE); return strRet; }
	if(*lpString == _T('\0')) return strRet;

	std::basic_string<WCHAR> strOrgW = _StringToUnicodeStl(lpString);
	std::vector<WCHAR> vRet;

	const bool bNbsp = ((dwFlags & XEF_NBSP) != XEF_NONE);

	// WCHAR vBufW[8];

	for(std::basic_string<WCHAR>::const_iterator it = strOrgW.begin();
		it != strOrgW.end(); ++it)
	{
		WCHAR wch = *it;
		ASSERT(wch != L'\0');

		// Encode XML characters
		if(wch == L'<')
			SU_AppendW(vRet, L"<");
		else if(wch == L'>')
			SU_AppendW(vRet, L">");
		else if(wch == L'&')
			SU_AppendW(vRet, L"&");
		else if(wch == L'\"')
			SU_AppendW(vRet, L""");
		else if(wch == L'\'')
			SU_AppendW(vRet, L"'");
		else if(wch == L'\r')
			SU_AppendW(vRet, L"
");
		else if(wch == L'\n')
			SU_AppendW(vRet, L"
");
		else if((wch == L' ') && bNbsp)
			SU_AppendW(vRet, L" ");
		// https://www.w3.org/TR/xml/#charsets
		else if((wch < L'\x09') || (wch == L'\x0B') || (wch == L'\x0C') ||
			((wch > L'\x0D') && (wch < L'\x20')))
		{
			// _itow_s((int)(DWORD)wch, vBufW, 16);
			// SU_AppendW(vRet, L"&#x");
			// SU_AppendW(vRet, vBufW);
			// vRet.push_back(L';');

			// Skip the character
		}
		else vRet.push_back(wch);
	}
	vRet.push_back(L'\0'); // Terminate string

#ifdef _UNICODE
	strRet = &vRet[0];
#else
	LPSTR lpAnsi = _StringToAnsi(&vRet[0]);
	if(lpAnsi != NULL)
	{
		strRet = lpAnsi;
		SAFE_DELETE_ARRAY(lpAnsi);
	}
	else { ASSERT(FALSE); }
#endif
	return strRet;
}

#pragma warning(push)
#pragma warning(disable: 4996) // strcpy deprecated

char *szcpy(char *szDestination, const char *szSource)
{
	ASSERT(szDestination != NULL); if(szDestination == NULL) return NULL;
	ASSERT(szSource != NULL); if(szSource == NULL) { szDestination[0] = 0; return szDestination; }
	return strcpy(szDestination, szSource);
}

#pragma warning(pop) // strcpy deprecated

DWORD szlen(const char *pszString)
{
	ASSERT(pszString != NULL); if(pszString == NULL) return 0;

	return static_cast<DWORD>(strlen(pszString));
}

// Extracts a substring from the lpstr string
// Example: to extract the auto-type command, pass "auto-type:" in lpStart
CString ExtractParameterFromString(LPCTSTR lpstr, LPCTSTR lpStart,
	DWORD dwInstance)
{
	CString str;

	ASSERT(lpstr != NULL); if(lpstr == NULL) return str; // _T("")

	CString strSource = lpstr;
	strSource = strSource.MakeLower();
	TCHAR *lp = const_cast<TCHAR *>(lpstr);

	int nPos = -1, nSearchFrom = 0;

	// nPos = strSource.Find(lpStart, 0);
	while(dwInstance != DWORD_MAX)
	{
		nPos = strSource.Find(lpStart, nSearchFrom);

		if(nPos != -1) nSearchFrom = nPos + 1;
		else return str; // _T("")

		--dwInstance;
	}

	if(nPos != -1)
	{
		lp += _tcslen(lpStart);
		lp += nPos;

		while(1)
		{
			const TCHAR tch = *lp;

			if(tch == '\0') break;
			else if(tch == '\n') break;
			else if(tch == '\r') { }
			else str += tch;

			++lp;
		}
	}

	str = str.Trim();
	return str;
}

/* #pragma warning(push)
#pragma warning(disable: 4996) // _tcscpy deprecated

void _GetPathFromFile(TCHAR *pszFile, TCHAR *pszPath)
{
	unsigned int i;

	_tcscpy(pszPath, pszFile);

	for(i = _tcslen(pszFile) - 1; i > 1; i--)
	{
		if((pszFile[i] == _T('\\')) || (pszFile[i] == _T('/')))
		{
			pszPath[i] = 0;
			pszPath[i+1] = 0;
			break;
		}
	}

}

#pragma warning(pop) */

TCHAR *_TcsSafeDupAlloc(const TCHAR *tszSource)
{
	TCHAR *ptsz;

	if(tszSource == NULL)
	{
		ptsz = new TCHAR[1];
		if(ptsz != NULL) ptsz[0] = 0; // Terminate string
	}
	else
	{
		const size_t sizeNewBuffer = (_tcslen(tszSource) + 1);
		ptsz = new TCHAR[sizeNewBuffer];
		if(ptsz != NULL) _tcscpy_s(ptsz, sizeNewBuffer, tszSource); // Clone
	}

	return ptsz;
}

LPTSTR _TcsCryptDupAlloc(LPCTSTR lpSource)
{
	ASSERT((CRYPTPROTECTMEMORY_BLOCK_SIZE % sizeof(TCHAR)) == 0);

	TCHAR *ptsz;
	if(lpSource == NULL)
	{
		ptsz = new TCHAR[CRYPTPROTECTMEMORY_BLOCK_SIZE / sizeof(TCHAR)];
		if(ptsz != NULL) ptsz[0] = 0; // Terminate string
	}
	else
	{
		const DWORD bytesNewText = static_cast<DWORD>(((_tcslen(lpSource) + 1) * sizeof(TCHAR)));
		const DWORD allocSize = CMemoryProtectionEx::ToBlockSize(bytesNewText);
		const DWORD allocChars = (allocSize / sizeof(TCHAR)); // See ASSERT above
		ASSERT(CMemoryProtectionEx::ToBlockSize(allocChars * sizeof(TCHAR)) ==
			(allocChars * sizeof(TCHAR)));

		ptsz = new TCHAR[allocChars];

		ASSERT(allocChars >= static_cast<DWORD>(_tcslen(lpSource) + 1));
		if(ptsz != NULL) _tcscpy_s(ptsz, allocChars, lpSource); // Clone
	}

	return ptsz;
}

void RemoveAcceleratorTip(CString *pString)
{
	ASSERT(pString != NULL); if(pString == NULL) return;

	pString->Replace(_T("(&A)"), _T(""));
	pString->Replace(_T("(&D)"), _T(""));
	pString->Replace(_T("(&E)"), _T(""));
	pString->Replace(_T("(&F)"), _T(""));
	pString->Replace(_T("(&H)"), _T(""));
	pString->Replace(_T("(&L)"), _T(""));
	pString->Replace(_T("(&N)"), _T(""));
	pString->Replace(_T("(&O)"), _T(""));
	pString->Replace(_T("(&P)"), _T(""));
	pString->Replace(_T("(&S)"), _T(""));

	pString->Remove(_T('&'));

	pString->Trim();
}

CString RemoveAcceleratorTipEx(LPCTSTR lpString)
{
	if(lpString == NULL) return CString();

	CString str = lpString;
	RemoveAcceleratorTip(&str);
	return str;
}

// Assumes that lpSearch is lower-case when bCaseSensitive == FALSE
// If pUseRegex is not NULL, a regular expression search will be
// performed, otherwise a simple substring matching.
bool StrMatchText(LPCTSTR lpEntryData, LPCTSTR lpSearch,
	BOOL bCaseSensitive, const boost::basic_regex<TCHAR>* pUseRegex)
{
	ASSERT(lpEntryData != NULL);
	if((lpEntryData == NULL) || (lpEntryData[0] == 0)) return false;

	ASSERT((lpSearch != NULL) && (lpSearch[0] != 0));

	// #ifndef _WIN64
	if(pUseRegex != NULL)
	{
		try
		{
			// return boost::regex_match(lpEntryData, *pUseRegex,
			//	boost::regex_constants::match_any);
			return boost::regex_search(lpEntryData, *pUseRegex,
				boost::regex_constants::match_any);
		}
		catch(...) { return false; }
	}
	// #else
	// #pragma message("No regular expression support in x64 library.")
	// UNREFERENCED_PARAMETER(pUseRegex);
	// #endif

	if(bCaseSensitive == FALSE)
	{
		CString strEntryData = lpEntryData;
		strEntryData = strEntryData.MakeLower();
		return (strEntryData.Find(lpSearch) != -1);
	}

	return (_tcsstr(lpEntryData, lpSearch) != NULL);
}

std::vector<std::basic_string<TCHAR> > SU_SplitSearchTerms(LPCTSTR lpSearch)
{
	std::vector<std::basic_string<TCHAR> > v;
	if((lpSearch == NULL) || (lpSearch[0] == 0)) return v;

	CStringBuilderEx sbTerm;
	bool bQuoted = false;

	const size_t uLen = _tcslen(lpSearch);
	for(size_t i = 0; i < uLen; ++i)
	{
		TCHAR ch = lpSearch[i];

		if(((ch == _T(' ')) || (ch == _T('\t')) || (ch == _T('\r')) ||
			(ch == _T('\n'))) && !bQuoted)
		{
			if(sbTerm.GetLength() > 0) v.push_back(sbTerm.ToString());

			sbTerm.Clear();
		}
		else if(ch == _T('\"')) bQuoted = !bQuoted;
		else sbTerm.Append(ch);
	}
	if(sbTerm.GetLength() > 0) v.push_back(sbTerm.ToString());

	return v;
}

#ifndef _WIN32_WCE
#pragma warning(push)
#pragma warning(disable: 4996) // SCL warning
std::basic_string<TCHAR> SU_GetQuotedPath(const std::basic_string<TCHAR>& strPath)
{
	std::basic_string<TCHAR> str = strPath;

	LPCTSTR lpTrim = _T(" \t\r\n");
	boost::algorithm::trim_if(str, boost::algorithm::is_any_of(lpTrim));

	if(str.size() <= 1) return str;
	if(str[0] != _T('\"')) return str;

	const std::basic_string<TCHAR>::size_type iSecond = str.find(_T('\"'), 1);
	if((iSecond == str.npos) || (iSecond == 0)) return str;

	return str.substr(1, iSecond - 1);
}
#pragma warning(pop)
#endif

CString SU_ConvertNewLines(LPCTSTR lpText, LPCTSTR lpNewLineSeq)
{
	if(lpText == NULL) { ASSERT(FALSE); return CString(); }
	if(lpNewLineSeq == NULL) { ASSERT(FALSE); return CString(); }

	CString str = lpText;
	str.Replace(_T("\r"), _T(""));
	str.Replace(_T("\n"), lpNewLineSeq);
	return str;
}

#pragma warning(push)
#pragma warning(disable: 4996) // SCL warning
void SU_Split(std::vector<std::basic_string<TCHAR> >& vOut,
	const std::basic_string<TCHAR>& strData, LPCTSTR lpSplitChars)
{
	if((lpSplitChars == NULL) || (lpSplitChars[0] == 0))
	{
		vOut.push_back(strData);
		return;
	}

	boost::algorithm::split(vOut, strData, boost::algorithm::is_any_of(lpSplitChars));
}
#pragma warning(pop)

std::basic_string<TCHAR> SU_CompactWith3Dots(LPCTSTR lpString, size_t uMaxChars)
{
	std::basic_string<TCHAR> str;
	if(lpString == NULL) { ASSERT(FALSE); return str; }
	if(uMaxChars == 0) return str;

	str = lpString;
	if(str.size() <= uMaxChars) return str;

	if(uMaxChars <= 3) return str.substr(0, uMaxChars);

	str = str.substr(0, uMaxChars - 3);
	str += _T("...");
	return str;
}

int SU_FindUnescapedCharW(LPCWSTR lpw, WCHAR wch)
{
	if(lpw == NULL) { ASSERT(FALSE); return -1; }

	const size_t uLen = wcslen(lpw);
	for(size_t i = 0; i < uLen; ++i)
	{
		const WCHAR wchCur = lpw[i];

		if(wchCur == L'\\') ++i; // Next is escaped, skip it
		else if(wchCur == wch) return static_cast<int>(i);
	}

	return -1;
}

void SU_AppendW(std::vector<WCHAR>& v, LPCWSTR lp)
{
	if(lp == NULL) { ASSERT(FALSE); return; }

	LPCWSTR lpCur = lp;
	while(*lpCur != L'\0') { v.push_back(*lpCur); ++lpCur; }
}

std_string SU_DriveLetterToUpper(const std_string& strPath)
{
	std_string str = strPath;
	if(str.size() < 3) return str;

	if((str[1] == _T(':')) && (str[2] == _T('\\')))
		str[0] = static_cast<TCHAR>(toupper(str[0]));

	return str;
}

TCHAR SU_GetDriveLetter(LPCTSTR lp)
{
	if(lp == NULL) { ASSERT(FALSE); return _T('\0'); }

	const TCHAR ch = lp[0];
	if(ch == _T('\0')) return _T('\0');

	if(lp[1] != _T(':')) return _T('\0');
	if(lp[2] != _T('\\')) return _T('\0');

	if((ch >= _T('A')) && (ch <= _T('Z'))) return ch;
	if((ch >= _T('a')) && (ch <= _T('z'))) return (ch - _T('a') + _T('A'));
	return _T('\0');
}

WCHAR SU_ToAnsiAndBack(WCHAR ch)
{
	if(ch == L'\0') return L'\0';
	if((ch >= L'\xD800') && (ch <= L'\xDFFF')) { ASSERT(FALSE); return L'\0'; } // Surrogate

	WCHAR wsz[2] = { ch, L'\0' };
	WCHAR chRet = L'\0';

	char* lpA = _StringToAnsi(wsz);
	if(lpA != NULL)
	{
		WCHAR* lpW = _StringToUnicode(lpA);
		if(lpW != NULL)
		{
			if((lpW[0] != L'\0') && (lpW[1] == L'\0')) chRet = lpW[0];

			SAFE_DELETE_ARRAY(lpW);
		}
		else { ASSERT(FALSE); }

		SAFE_DELETE_ARRAY(lpA);
	}
	else { ASSERT(FALSE); }

	return chRet;
}

const std::vector<WCHAR>& SU_GetNormDashes()
{
	if(g_vNormHyphen.size() == 0)
	{
		g_vNormHyphen.push_back(L'-'); // Hyphen-minus (U+002D)

		const size_t ccOther = 7;
		WCHAR wszOther[ccOther] = {
			L'\x2010', // Hyphen
			L'\x2011', // Non-breaking hyphen
			L'\x2012', // Figure dash
			L'\x2013', // En dash
			L'\x2014', // Em dash
			L'\x2015', // Horizontal bar
			L'\x2212' // Minus sign
		};

		for(size_t i = 0; i < ccOther; ++i)
		{
			if(SU_ToAnsiAndBack(wszOther[i]) == wszOther[i])
				g_vNormHyphen.push_back(wszOther[i]);
		}
	}

	return g_vNormHyphen;
}

CString SU_NormalizeDashes(LPCTSTR lpText)
{
	if(lpText == NULL) { ASSERT(FALSE); return CString(); }
	if(*lpText == _T('\0')) return CString();

	std::basic_string<WCHAR> strW = _StringToUnicodeStl(lpText);
	const size_t ccW = strW.size();

	const std::vector<WCHAR>& vDashes = SU_GetNormDashes();
	ASSERT((vDashes.size() > 0) && (vDashes[0] == L'-'));
	for(size_t iD = 1; iD < vDashes.size(); ++iD)
	{
		const WCHAR chD = vDashes[iD];

		for(size_t iW = 0; iW < ccW; ++iW)
		{
			if(strW[iW] == chD) strW[iW] = vDashes[0];
		}
	}

#ifdef _UNICODE
	return CString(strW.c_str());
#else
	char* lpA = _StringToAnsi(strW.c_str());
	if(lpA == NULL) { ASSERT(FALSE); return CString(lpText); }
	CString str(lpA);
	SAFE_DELETE_ARRAY(lpA);
	return str;
#endif
}

/////////////////////////////////////////////////////////////////////////////
// WCharStream class

WCharStream::WCharStream(LPCWSTR lpData)
{
	if(lpData != NULL) m_str = lpData;
	else { ASSERT(FALSE); }

	m_i = 0;
}

WCHAR WCharStream::ReadChar()
{
	if(m_i == m_str.size()) return L'\0';

	return m_str[m_i++];
}

WCHAR WCharStream::PeekChar() const
{
	if(m_i == m_str.size()) return L'\0';

	return m_str[m_i];
}

/////////////////////////////////////////////////////////////////////////////
// CStringBuilderEx class

CStringBuilderEx::CStringBuilderEx()
{
}

void CStringBuilderEx::Append(TCHAR tch)
{
	m_vBuf.push_back(tch);
}

void CStringBuilderEx::Append(LPCTSTR lpString)
{
	ASSERT(lpString != NULL); if(lpString == NULL) return;

	DWORD i = 0;
	while(lpString[i] != 0)
	{
		m_vBuf.push_back(lpString[i]);
		++i;
	}
}

std::basic_string<TCHAR> CStringBuilderEx::ToString() const
{
	std::vector<TCHAR> vCopy(m_vBuf);
	vCopy.push_back(0);

	return std::basic_string<TCHAR>(&vCopy[0]);
}

/////////////////////////////////////////////////////////////////////////////
// CStringSetEx class

CStringSetEx::CStringSetEx()
{
}

CStringSetEx::~CStringSetEx()
{
	this->Clear();
}

void CStringSetEx::Clear()
{
	const size_t uStringCount = m_vStrings.size();
	for(size_t i = 0; i < uStringCount; ++i)
		SAFE_DELETE_ARRAY(m_vStrings[i]);

	m_vStrings.clear();
}

LPCTSTR CStringSetEx::Add(LPCTSTR lpString)
{
	if(lpString == NULL) { ASSERT(FALSE); return NULL; }

	const size_t uStringCount = m_vStrings.size();
	for(size_t i = 0; i < uStringCount; ++i)
	{
		if(_tcscmp(m_vStrings[i], lpString) == 0)
			return m_vStrings[i];
	}

	LPTSTR lp = _TcsSafeDupAlloc(lpString);
	m_vStrings.push_back(lp);
	return lp;
}

max_stars_repo_name: rrvt/KeePassLastPass

max_issues_repo_name: rrvt/KeePassLastPass

max_forks_repo_name: rrvt/KeePassLastPass


text:
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <iostream>

int main()
{
	//定义显示窗口
	dlib::image_window win, win_faces;
	//定义检测器
	dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
	//定义数据输入sp
	dlib::shape_predictor sp;
	dlib::deserialize("shape_predictor_68_face_landmarks.dat") >> sp;
	//定义图像格式，与opencv不同
	dlib::array2d<dlib::rgb_pixel> img;
	//载入图像
	dlib::load_image(img, "test.jpg");
	//你可以使用下行pyramid_up来放大图像检测更小的脸
	//dlib::pyramid_up(img);
	std::vector<dlib::rectangle> dets = detector(img);
	std::vector<dlib::full_object_detection> shapes;
	//dets.size表示人脸数
	for (unsigned long j = 0; j < dets.size(); ++j)
	{
		dlib::full_object_detection shape = sp(img, dets[j]);
		std::cout << "number of parts: " << shape.num_parts() << std::endl;
		std::cout << "pixel position of first part:  " << shape.part(0) << std::endl;
		std::cout << "pixel position of second part: " << shape.part(1) << std::endl;
		// You get the idea, you can get all the face part locations if
		// you want them.  Here we just store them in shapes so we can
		// put them on the screen.
		shapes.push_back(shape);
	}
	//清除覆盖
	win.clear_overlay();
	//显示图像
	win.set_image(img);
	win.add_overlay(render_face_detections(shapes));

	dlib::array<dlib::array2d<dlib::rgb_pixel> > face_chips;
	extract_image_chips(img, get_face_chip_details(shapes), face_chips);
	//显示脸部，小图
	win_faces.set_image(tile_images(face_chips));
	win_faces.wait_until_closed();
}


text: #include <boost/functional/factory.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


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

#include <boost/hana/core/make.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
namespace hana = boost::hana;

// A trait representing whether a tag has a single C++ type.
// This is entirely optional, but can enable some optimizations.
template <typename T, typename Enable = void>
struct is_homogeneous {
    static constexpr bool value = false;
};

template <typename T>
struct is_homogeneous<T, std::enable_if_t<std::is_arithmetic<T>::value>> {
    static constexpr bool value = true;
};

// a list of types
template <typename ...xs>
constexpr auto list_t = hana::tuple_t<xs...>;

// a list of homogeneous Constants
template <typename X, X ...xs>
constexpr auto list_c = hana::tuple_c<X, xs...>;

template <typename T, typename Enable = void>
struct list_of {
    template <typename ...X>
    constexpr auto operator()(X ...x) const
    { return hana::make<hana::tuple_tag>(x...); }
};

template <>
struct list_of<hana::type_tag> {
    template <typename ...X>
    constexpr auto operator()(X ...) const
    { return list_t<typename X::type...>; }
};

template <typename T>
struct list_of<T, std::enable_if_t<is_homogeneous<T>::value>> {
    template <typename ...X>
    constexpr auto operator()(X ...x) const
    { return hana::make<hana::tuple_tag>(x...); } // would use an array
};

struct _list {
    template <typename T, typename ...X>
    static constexpr auto of(X ...x)
    { return list_of<T>{}(x...); }

    struct Any;
    template <typename ...X>
    constexpr auto operator()(X ...x) const
    { return of<Any>(x...); }
};

constexpr _list list{};

int main() {
    list(1, 2, 3); // normal version

    // uses a list_t internally
    list.of<hana::type_tag>(hana::type_c<int>, hana::type_c<void>, hana::type_c<char>);

    // uses an array internally
    list.of<int>(1, 2, 3);
}

max_stars_repo_name: qicosmos/hana

max_issues_repo_name: qicosmos/hana

max_forks_repo_name: qicosmos/hana


text:
/*!
 * \file ReportVerifier.cpp
 * \brief Test for ReportVerifier functionality
 */

#include "sparta/report/db/ReportVerifier.hpp"
#include "simdb/ObjectManager.hpp"
#include "simdb/TableRef.hpp"
#include "simdb/utils/ObjectQuery.hpp"
#include "simdb/impl/sqlite/SQLiteConnProxy.hpp"
#include "sparta/report/db/Schema.hpp"
#include "sparta/utils/SpartaTester.hpp"

#include <boost/algorithm/string/replace.hpp>
#include <fstream>

TEST_INIT;

using sparta::SpartaTester;
namespace db = sparta::db;

#define PRINT_ENTER_TEST \
  std::cout << std::endl; \
  std::cout << "*************************************************************" \
            << "*** Beginning '" << __FUNCTION__ << "'" \
            << "*************************************************************" \
            << std::endl;

std::string makeLoremText()
{
    std::ostringstream lorem;
    lorem << "Lorem ipsum dolor sit amet, consectetur adipiscing elit,    " << std::endl;
    lorem << "sed do eiusmod tempor incididunt ut labore et dolore magna  " << std::endl;
    lorem << "aliqua. Ut enim ad minim veniam, quis nostrud exercitation  " << std::endl;
    lorem << "ullamco laboris nisi ut aliquip ex ea commodo consequat.    " << std::endl;
    lorem << "Duis aute irure dolor in reprehenderit in voluptate velit   " << std::endl;
    lorem << "esse cillum dolore eu fugiat nulla pariatur. Excepteur sint " << std::endl;
    lorem << "occaecat cupidatat non proident, sunt in culpa qui officia  " << std::endl;
    lorem << "deserunt mollit anim id est laborum.                        " << std::endl;
    return lorem.str();
}

void spartaTesterEquivalentFiles()
{
    PRINT_ENTER_TEST

    std::ostringstream cerr;
    std::unique_ptr<SpartaTester> tester =
        SpartaTester::makeTesterWithUserCError(cerr);

    const std::string fname1 = "lorem1.txt";
    const std::string fname2 = "lorem2.txt";
    const std::string header = "# foo=5,bar=asdf";

    {
        std::ofstream fout(fname1);
        fout << header << "\n";
        fout << makeLoremText();
    }

    {
        std::ofstream fout(fname2);
        fout << header << "\n";
        fout << makeLoremText();
    }

    tester->expectFilesEqual(fname1, fname2, true, __LINE__, __FILE__, false);
    EXPECT_TRUE(cerr.str().empty());
    EXPECT_EQUAL(SpartaTester::getErrorCode(tester.get()), 0);
}

void spartaTesterDifferentFiles()
{
    PRINT_ENTER_TEST

    std::ostringstream cerr;
    std::unique_ptr<SpartaTester> tester =
        SpartaTester::makeTesterWithUserCError(cerr);

    const std::string fname1 = "lorem1.txt";
    const std::string fname2 = "lorem2.txt";
    const std::string header = "# foo=5,bar=asdf";

    {
        std::ofstream fout(fname1);
        fout << header << "\n";
        fout << makeLoremText();
    }

    {
        std::ofstream fout(fname2);
        std::string lorem = makeLoremText();
        boost::replace_all(lorem, "aliqua", "aliquip");
        boost::replace_all(lorem, "consequat", "consectetur");
        fout << header << "\n";
        fout << lorem;
    }

    tester->expectFilesEqual(fname1, fname2, true, __LINE__, __FILE__, false);
    EXPECT_FALSE(cerr.str().empty());
    EXPECT_NOTEQUAL(SpartaTester::getErrorCode(tester.get()), 0);
}

void testVerificationTables()
{
    PRINT_ENTER_TEST

    simdb::ObjectManager obj_mgr(".");

    simdb::Schema schema;
    db::buildSimulationDatabaseSchema(schema);

    std::unique_ptr<simdb::DbConnProxy> db_proxy(new simdb::SQLiteConnProxy);
    obj_mgr.createDatabaseFromSchema(schema, std::move(db_proxy));

    const std::string dest_file = "AccuracyCheckedDBs/abcd-1234/out2.csv";
    const simdb::DatabaseID sim_info_id = 14;
    const bool passed = false;
    const bool is_timeseries = true;

    auto verif_tbl = obj_mgr.getTable("ReportVerificationResults");

    verif_tbl->createObjectWithArgs("DestFile", dest_file,
                                    "SimInfoID", sim_info_id,
                                    "Passed", (int)passed,
                                    "IsTimeseries", (int)is_timeseries);

    simdb::ObjectQuery query(obj_mgr, "ReportVerificationResults");
    query.addConstraints("Passed", simdb::constraints::equal, 0);
    EXPECT_EQUAL(query.countMatches(), 1);

    std::string record_dest_file;
    simdb::DatabaseID record_sim_info_id;
    int record_passed;
    int record_is_timeseries;

    query.writeResultIterationsTo("DestFile", &record_dest_file,
                                  "SimInfoID", &record_sim_info_id,
                                  "Passed", &record_passed,
                                  "IsTimeseries", &record_is_timeseries);

    EXPECT_EQUAL(query.countMatches(), 1);
    query.executeQuery()->getNext();

    EXPECT_EQUAL(record_dest_file, dest_file);
    EXPECT_EQUAL(record_sim_info_id, sim_info_id);
    EXPECT_EQUAL(record_passed, (int)passed);
    EXPECT_EQUAL(record_is_timeseries, (int)is_timeseries);
}

int main()
{
    spartaTesterEquivalentFiles();
    spartaTesterDifferentFiles();
    testVerificationTables();

    REPORT_ERROR;
    return ERROR_CODE;
}


text:
// Copyright (c) 2011-2013 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#define BOOST_TEST_MODULE Bitcoin Test Suite

#include "main.h"
#include "bitcoin_main.h"
#include "txdb.h"
#include "bitcoin_txdb.h"
#include "ui_interface.h"
#include "util.h"
#ifdef ENABLE_WALLET
#include "db.h"
#include "bitcoin_db.h"
#include "wallet.h"
#include "bitcoin_wallet.h"
#endif

#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>

#ifdef ENABLE_WALLET
uint64_t bitcoin_nAccountingEntryNumber = 0;
//Bitcoin_CDBEnv bitcoin_bitdb;

uint64_t bitcredit_nAccountingEntryNumber = 0;
Credits_CDBEnv bitcredit_bitdb("credits_database", "credits_db.log");

uint64_t deposit_nAccountingEntryNumber = 0;
Credits_CDBEnv deposit_bitdb("deposit_database", "deposit_db.log");
#endif

Bitcoin_CWallet* bitcoin_pwalletMain;
Credits_CWallet* bitcredit_pwalletMain;
Credits_CWallet* deposit_pwalletMain;

extern void noui_connect();

struct TestingSetup {
    Credits_CCoinsViewDB *bitcredit_pcoinsdbview;
    Bitcoin_CCoinsViewDB *bitcoin_pcoinsdbview;
    boost::filesystem::path pathTemp;
    boost::thread_group threadGroup;

    TestingSetup() {
        fPrintToDebugLog = false; // don't want to write to debug.log file
        noui_connect();
#ifdef ENABLE_WALLET
        bitcoin_bitdb.MakeMock();
        bitcredit_bitdb.MakeMock();
        deposit_bitdb.MakeMock();
#endif
        pathTemp = GetTempPath() / strprintf("test_credits_%lu_%i", (unsigned long)GetTime(), (int)(GetRand(100000)));
        boost::filesystem::create_directories(pathTemp);
        mapArgs["-datadir"] = pathTemp.string();

        bitcredit_pblocktree = new Credits_CBlockTreeDB(1 << 20, true);
        bitcredit_pcoinsdbview = new Credits_CCoinsViewDB(1 << 23, true);
        credits_pcoinsTip = new Credits_CCoinsViewCache(*bitcredit_pcoinsdbview);

        bitcoin_pblocktree = new Bitcoin_CBlockTreeDB(1 << 20, true);
        bitcoin_pcoinsdbview = new Bitcoin_CCoinsViewDB(1 << 23, true);
        bitcoin_pcoinsTip = new Bitcoin_CCoinsViewCache(*bitcoin_pcoinsdbview);

        Bitcoin_InitBlockIndex();
        Credits_InitBlockIndex();

#ifdef ENABLE_WALLET
        bool fFirstRun;
        bitcoin_pwalletMain = new Bitcoin_CWallet("bitcoin_wallet.dat");
        bitcoin_pwalletMain->LoadWallet(fFirstRun);
        Bitcoin_RegisterWallet(bitcoin_pwalletMain);

        bitcredit_pwalletMain = new Credits_CWallet("credits_wallet.dat", &bitcredit_bitdb);
        bitcredit_pwalletMain->LoadWallet(fFirstRun, bitcredit_nAccountingEntryNumber);
        Bitcredit_RegisterWallet(bitcredit_pwalletMain);

        deposit_pwalletMain = new Credits_CWallet("deposit_wallet.dat", &deposit_bitdb);
        deposit_pwalletMain->LoadWallet(fFirstRun, deposit_nAccountingEntryNumber);
        Bitcredit_RegisterWallet(deposit_pwalletMain);
#endif
        bitcredit_nScriptCheckThreads = 3;
        for (int i=0; i < bitcredit_nScriptCheckThreads-1; i++)
            threadGroup.create_thread(&Bitcredit_ThreadScriptCheck);
        Bitcredit_RegisterNodeSignals(Credits_NetParams()->GetNodeSignals());
    }
    ~TestingSetup()
    {
        threadGroup.interrupt_all();
        threadGroup.join_all();
        Bitcredit_UnregisterNodeSignals(Credits_NetParams()->GetNodeSignals());
#ifdef ENABLE_WALLET
        delete bitcoin_pwalletMain;
        bitcoin_pwalletMain = NULL;
        delete bitcredit_pwalletMain;
        bitcredit_pwalletMain = NULL;
        delete deposit_pwalletMain;
        deposit_pwalletMain = NULL;
#endif
        delete credits_pcoinsTip;
        delete bitcredit_pcoinsdbview;
        delete bitcredit_pblocktree;

        delete bitcoin_pcoinsTip;
        delete bitcoin_pcoinsdbview;
        delete bitcoin_pblocktree;
#ifdef ENABLE_WALLET
        bitcoin_bitdb.Flush(true);
        bitcredit_bitdb.Flush(true);
        deposit_bitdb.Flush(true);
#endif
        boost::filesystem::remove_all(pathTemp);
    }
};

BOOST_GLOBAL_FIXTURE(TestingSetup);

void Shutdown(void* parg)
{
  exit(0);
}

void StartShutdown()
{
  exit(0);
}

bool ShutdownRequested()
{
  return false;
}

max_stars_repo_name: gabriel-eiger/bitcredit

max_issues_repo_name: gabriel-eiger/bitcredit

max_forks_repo_name: gabriel-eiger/bitcredit