/* -*-c++-*--------------------------------------------------------------------
 * 2017 Bernd Pfrommer bernd.pfrommer@gmail.com
 *      Kartik Mohta
 */
#include "multicam_calibration/calibrator.h"
#include "multicam_calibration/utils.h"
#include <ceres/ceres.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/range/irange.hpp>
#include <fstream>
#include <iomanip>
#include <algorithm>

#define CERES_PUBLIC_INTERNAL_CONFIG_H_
#include <ceres/ceres.h>

//#define DEBUG_PARAMS

namespace multicam_calibration {
  using utils::Mat;
  using utils::Vec;
  using utils::DynVec;
  using utils::rotation_matrix;
  using boost::irange;

  // find number of intrinsics for all cameras
  static unsigned int get_num_intrinsics(const CalibDataVec &cdv) {
    unsigned int extrinsicsBase(0);
    for (const auto &cd : cdv) {
      // for each camera we have fx, fy, cx, cy + distortion coefficients;
      extrinsicsBase += cd.intrinsics.intrinsics.size() + cd.intrinsics.distortion_coeffs.size();
    }
    return (extrinsicsBase);
  }

  // maps parameters to parameter blocks
  // ------ parameter block layout --------
  //
  // params[0]          = intrinsics for cam 0
  //  ....
  // params[ncam-1]     = intrinsics for cam n-1
  //
  // params[ncam]       = extrinsics cam1->cam0 (if cam1 exists)
  //     ...
  // params[2 * ncam-2] = extrinsics camn->cam0
  // params[2 * ncam-1]   = frame_tf

  static std::vector<double *>
  params_to_blocks(const CalibDataVec &cdv, std::vector<double> &params,
                   unsigned int frame_num) {
    std::vector<double *> v;
    unsigned int num_cameras = cdv.size();
    unsigned int i = 0;
    unsigned int offset = 0;
    for (const auto &cd : cdv) {
      v.push_back(&params[offset]); // intrinsics are the first block
      // for each camera we have fx, fy, cx, cy + distortion coefficients;
      offset += cd.intrinsics.intrinsics.size() + cd.intrinsics.distortion_coeffs.size();
      i++;
    }
    // number of intrinsic parameters for all cameras
    unsigned int numIntrinsics = offset;
    // add the extrinsics between cam0 and the rest
    unsigned int totNumCameraParams = numIntrinsics + 6 * (num_cameras - 1);
    for (const auto iext : irange(0u, num_cameras - 1)) {
      v.push_back(&params[numIntrinsics + 6 * iext]);
    }
    const auto R_vec_offset = totNumCameraParams + 6 * frame_num;
    v.push_back(&params[R_vec_offset]);
    return (v);
  }

  struct FrameResidual
  {
    // Input is the list of world points and image points for the current frame in
    // each of the cameras
    FrameResidual(const std::vector<FrameWorldPoints> &world_points,
                  const std::vector<FrameImagePoints> &image_points,
                  const std::vector<std::vector<bool>> &masks,
                  const CalibDataVec &cams) :
      world_points_(world_points), image_points_(image_points), masks_(masks),
      cams_(cams) {
      }

    template <typename T>
    bool operator()(T const *const *params, T *residual) const
      {
        unsigned int frame_tf_idx   = cams_.size() * 2 - 1;
        const Vec<T, 3> R_vec_frame = Eigen::Map<const Vec<T, 3>>(&params[frame_tf_idx][0]);
        const Mat<T, 3, 3>  R_frame = rotation_matrix(R_vec_frame);
        const Vec<T, 3>     t_frame = Eigen::Map<const Vec<T, 3>>(&params[frame_tf_idx][3]);

        unsigned int residual_count = 0;
        unsigned int intrinsics_offset = 0;
        for(const auto cam_idx : irange(0u, (unsigned int)cams_.size()))
          {
            const CalibrationData &cam = cams_[cam_idx];
            Mat<T, 3, 3> K = Mat<T, 3, 3>::Identity();
            K(0, 0) = params[cam_idx][0];
            K(1, 1) = params[cam_idx][1];
            K(0, 2) = params[cam_idx][2];
            K(1, 2) = params[cam_idx][3];
            unsigned int numDist = cam.intrinsics.distortion_coeffs.size();
            DynVec<T> D(numDist);
            for(const auto i : irange(0u, numDist)) {
              D(i) = params[cam_idx][cam.intrinsics.intrinsics.size() + i];
            }
            if (cam.active) {
              Mat<T, 3, 3> R_cam;
              Vec<T, 3> t_cam;
              if(cam_idx == 0) {
                R_cam = Mat<T, 3, 3>::Identity();
                t_cam = Vec<T, 3>::Zero();
              } else {
                // apply camera extrinsics wrt camera 0
                unsigned int ext_param_num = cams_.size() + cam_idx - 1;
                const Vec<T, 3> R_vec_cam =
                  Eigen::Map<const Vec<T, 3>>(&params[ext_param_num][0]);
                R_cam = rotation_matrix(R_vec_cam); // rotation vector
                t_cam = Eigen::Map<const Vec<T, 3>>(&params[ext_param_num][3]);
              }

              const Mat<T, 3, 3> R = R_cam * R_frame;
              const Vec<T, 3>    t = R_cam * t_frame + t_cam;
              vector_aligned<Point2<T>> projected_points;
              if (cam.intrinsics.distortion_model == "equidistant") {
                projected_points = utils::project_frame_equidistant(world_points_[cam_idx], R, t, K, D);
              } else if (cam.intrinsics.distortion_model == "radtan") {
                projected_points = utils::project_frame_radtan(world_points_[cam_idx], R, t, K, D);
              } else {
                std::cout << "ERROR: unknown distortion model: " << cam.intrinsics.distortion_model << std::endl;
                return (false);
              }
              for(const auto i : irange<size_t>(0, projected_points.size())) {
                if (masks_[cam_idx][i]) {
                  residual[residual_count++] =
                    projected_points[i].x - T(image_points_[cam_idx][i].x);
                  residual[residual_count++] =
                    projected_points[i].y - T(image_points_[cam_idx][i].y);
                }
              }
            }
            intrinsics_offset += cam.intrinsics.intrinsics.size() +
              cam.intrinsics.distortion_coeffs.size();
          }
        return true;
      }

  private:
    const std::vector<FrameWorldPoints> world_points_;
    const std::vector<FrameImagePoints> image_points_;
    const std::vector<std::vector<bool>> masks_;
    CalibDataVec cams_;
  };

  static std::vector<double> transform_to_rvec_tvec(const CameraExtrinsics &tf) {
    std::vector<double> v;
    Eigen::AngleAxisd axisAngle(tf.block<3,3>(0, 0));
    Eigen::Vector3d rvec = axisAngle.axis() * axisAngle.angle();
    v.push_back(rvec(0));
    v.push_back(rvec(1));
    v.push_back(rvec(2));
    // Translation
    Eigen::Vector3d T = tf.block<3,1>(0, 3);
    v.push_back(T(0));
    v.push_back(T(1));
    v.push_back(T(2));
    return (v);
  }

#ifdef DEBUG_PARAMS
  static void print_params(const std::vector<double> &p, const CalibDataVec &cams) {
    using utils::rotation_matrix;
    using utils::vec_to_str;
    int off(0);
    std::cout << "------------------- parameters: -------------------" << std::endl;
    unsigned int intrinsics_offset(0);
    unsigned int extrinsics_base = get_num_intrinsics(cams);
    for(const auto cam_idx : irange<size_t>(0ul, cams.size())) {
      const CalibrationData &cam = cams[cam_idx];
      unsigned int off = intrinsics_offset;
      std::cout << "--------- camera # " << cam_idx << std::endl;
      std::cout << "fx: " << p[off]   << " fy: " << p[off+1] << std::endl;
      std::cout << "cx: " << p[off+2] << " cy: " << p[off+3] << std::endl;
      std::cout << "distortion coeff:";
      for(const auto k : irange(0u, (unsigned int)cam.intrinsics.distortion_coeffs.size())) {
        std::cout << " " << p[off + cam.intrinsics.intrinsics.size() + k];
      }
      std::cout << std::endl;
      if (cam_idx > 0) {
        off = extrinsics_base + 6 * (cam_idx - 1);
        const Vec<double, 3> rvec = Eigen::Map<const Vec<double, 3>>(&p[off]);
        Mat<double, 3, 3> R = rotation_matrix(rvec);
        const Vec<double, 3> t = Eigen::Map<const Vec<double, 3>>(&p[off + 3]);
        //std::cout << "T_cam_cam0:  rot: " << "[" << vec_to_str(&R(0,0), 9) << "] trans: ["
        //<< vec_to_str(&t(0), 3) << "]" << std::endl;
      }
      intrinsics_offset += cam.intrinsics.intrinsics.size() +
        cam.intrinsics.distortion_coeffs.size();

    }
#if 0
    off = extrinsics_base + 6 * (cams.size() - 1);
    std::cout << "--------- camera poses ----------------" << std::endl;
    for (; off < (int)p.size(); off += 6) {
      const Vec<double, 3> rvec = Eigen::Map<const Vec<double, 3>>(&p[off]);
      Mat<double, 3, 3> R = rotation_matrix(rvec);
      const Vec<double, 3> t  = Eigen::Map<const Vec<double, 3>>(&p[off + 3]);
      std::cout << "rot: " << "[" << vec_to_str(&R(0,0), 9) << "] trans: ["
                << vec_to_str(&t(0), 3) << "]" << std::endl;
    }
#endif
  }
#endif

  void Calibrator::showCameraStatus() const {
    for (const auto &cam: calibrationData_) {
      ROS_INFO("%-10s: fix intr: %d, fix extr: %d, active: %d",
               cam.name.c_str(), cam.fixIntrinsics,
               cam.fixExtrinsics, cam.active);
    }
  }

  void Calibrator::setCameras(const CalibDataVec &calibData) {
    calibrationData_ = calibData;
  }

  void Calibrator::addPoints(int frameNum, const CamWorldPoints &wp, const CamImagePoints &ip,
                             const CameraExtrinsics &cam0PoseGuess) {
    if (worldPoints_.empty()) {
      worldPoints_.resize(calibrationData_.size());
      imagePoints_.resize(calibrationData_.size());
      masks_.resize(calibrationData_.size());
    }
    if (wp.size() != calibrationData_.size() ||
        ip.size() != calibrationData_.size()) {
      std::cerr << "number of cams != width of array!" << std::endl;
      return;
    }
    for (unsigned int cam_idx  = 0; cam_idx < calibrationData_.size(); cam_idx++) {
      FrameWorldPoints fwp(wp[cam_idx].begin(), wp[cam_idx].end());
      worldPoints_[cam_idx].push_back(fwp);
      FrameImagePoints fip(ip[cam_idx].begin(), ip[cam_idx].end());
      imagePoints_[cam_idx].push_back(fip);
      masks_[cam_idx].push_back(std::vector<bool>(fip.size(), true));
    }
    cam0PoseGuess_.push_back(cam0PoseGuess);
  }

  void Calibrator::initializeVariables(std::vector<double> *param_ptr) {
    std::vector<double> &params = *param_ptr;
    const unsigned int num_cameras = calibrationData_.size();

    // initialize intrinsics
    for (const auto cam_idx : irange(0u, num_cameras)) {
      const CameraIntrinsics &ci = calibrationData_[cam_idx].intrinsics;
      params.insert(params.end(), std::begin(ci.intrinsics),
                    std::end(ci.intrinsics));
      params.insert(params.end(), std::begin(ci.distortion_coeffs),
                    std::end(ci.distortion_coeffs));
    }
    // initialize T_cn_cnm1, only for cameras > 0
    CameraExtrinsics T_cnm1_c0 = CameraExtrinsics::Identity();
    for(const auto cam_idx : irange(1u, num_cameras)) {
      // ------------ relative to camera 0 -----------------
      CameraExtrinsics T_cn_c0 = calibrationData_[cam_idx].T_cn_cnm1 * T_cnm1_c0;
      std::vector<double> rvec_tvec = transform_to_rvec_tvec(T_cn_c0);
      params.insert(params.end(), rvec_tvec.begin(), rvec_tvec.end());
      T_cnm1_c0 = T_cn_c0;
    }
    // the number of frames are the same across all cameras,
    // although not all frames must contain detected points
    const unsigned int num_frames = worldPoints_[0].size();
    // initialize pose guess for camera 0
    for(const auto i : irange(0u, num_frames))  {
      std::vector<double> rvec_tvec = transform_to_rvec_tvec(cam0PoseGuess_[i]);
      params.insert(params.end(), rvec_tvec.begin(), rvec_tvec.end());
    };
#ifdef DEBUG_PARAMS
    print_params(params, calibrationData_);
#endif
  }

  void Calibrator::runCalibration() {
    if (calibrationData_.empty() || worldPoints_.empty()) {
      ROS_ERROR("no data to run on!");
      return;
    }
    ROS_INFO_STREAM("camera status at start:");
    showCameraStatus();

    params_.clear();
    initializeVariables(&params_);
    ceres::Problem problem;
    setupOptimizationProblem(&problem, &params_);
    std::cout << "Num params: " << params_.size() << std::endl;
    std::cout << "Num residuals: " << problem.NumResiduals() << std::endl;

    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 200;
    options.num_threads = 4;
    options.function_tolerance = 1e-12;
    options.parameter_tolerance = 1e-12;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

#ifdef DEBUG_PARAMS
    print_params(params_, calibrationData_);
#endif
    std::cout << summary.FullReport() << std::endl;
  }

  void Calibrator::testCalibration(Residuals *resptr) {
    auto &res = *resptr; // residuals
    const unsigned int num_frames = worldPoints_[0].size();
    const unsigned int num_cameras = calibrationData_.size();

    for (const auto fnum : irange(0u, num_frames))  {
      std::vector<FrameWorldPoints> frame_world_points;
      std::vector<FrameImagePoints> frame_image_points;
      std::vector<std::vector<bool>> masks;
      size_t frame_num_points = getPointsAndMasks(fnum, &frame_world_points,
                                                  &frame_image_points, &masks);
      res.push_back(std::vector<std::vector<Vec2d>>()); // empty for this frame
      if (frame_num_points > 0) {
        const auto fr = FrameResidual(frame_world_points, frame_image_points,
                                      masks, calibrationData_);
        const unsigned numResiduals = 2 * frame_num_points;
        std::vector<double> residuals(numResiduals);
        // point offset to cam0 pose for this particular frame
        std::vector<double *> params = params_to_blocks(calibrationData_, params_, fnum);
        fr(&params[0], residuals.data());
        // unpack residuals into vector of vectors of vectors
        for (const auto cam_idx : irange(0u, num_cameras)) {
          res.back().push_back(std::vector<Vec2d>()); // empty for this camera
          if (calibrationData_[cam_idx].active) {
            size_t res_idx = 0;
            for (const auto pt_idx : irange(0u, (unsigned int)worldPoints_[cam_idx][fnum].size())) {
              if (masks_[cam_idx][fnum][pt_idx]) {
                res.back().back().push_back(Vec2d(residuals[res_idx * 2], residuals[res_idx * 2 + 1]));
                res_idx++;
              }
            }
          }
        }
      }
    }
  }

  size_t Calibrator::getPointsAndMasks(
    size_t fnum,
    std::vector<FrameWorldPoints> *frame_world_points,
    std::vector<FrameImagePoints> *frame_image_points,
    std::vector<std::vector<bool>> *masks) const {
    size_t frame_num_points{0};
    for (const auto cam_idx : irange(0ul, calibrationData_.size())) {
      frame_world_points->push_back(worldPoints_[cam_idx][fnum]);
      frame_image_points->push_back(imagePoints_[cam_idx][fnum]);
      masks->push_back(masks_[cam_idx][fnum]);
      if (calibrationData_[cam_idx].active) {
        const size_t nvalid = std::count(
          masks->back().begin(), masks->back().end(), true);
        frame_num_points += nvalid;
      }
    }
    return (frame_num_points);
  }

  void Calibrator::removeOutliers(double max_err) {
    ROS_INFO_STREAM("removing outliers with pixel error more than " << max_err);
    const double maxError2 = max_err * max_err;
    const size_t num_frames = worldPoints_[0].size();
    for (const auto fnum : irange(0ul, num_frames))  {
      std::vector<FrameWorldPoints> frame_world_points;
      std::vector<FrameImagePoints> frame_image_points;
      std::vector<std::vector<bool>> masks;

      size_t frame_num_points = getPointsAndMasks(fnum, &frame_world_points,
                                                  &frame_image_points, &masks);
      if (frame_num_points > 0) {
        const auto fr = FrameResidual(frame_world_points, frame_image_points,
                                      masks, calibrationData_);
        const unsigned numResiduals = 2 * frame_num_points;
        std::vector<double> residuals(numResiduals);
        // point offset to cam0 pose for this particular frame
        std::vector<double *> params = params_to_blocks(calibrationData_, params_, fnum);
        // compute residual for this frame
        fr(&params[0], residuals.data());
        // update mask
        size_t res_idx = 0;
        for (const auto cam_idx : irange(0ul, calibrationData_.size())) {
          if (calibrationData_[cam_idx].active) {
            for (const auto pt_idx :
                   irange(0u,(unsigned int)worldPoints_[cam_idx][fnum].size())) {
              if (masks_[cam_idx][fnum][pt_idx]) {
                const double dx2 = residuals[res_idx * 2] * residuals[res_idx * 2];
                const double dy2 = residuals[res_idx * 2 + 1] * residuals[res_idx * 2 + 1];
                if (dx2 + dy2 > maxError2) {
                  masks_[cam_idx][fnum][pt_idx] = false; // mark point as bad
                  ROS_INFO("dropping bad point cam: %1d, frame: %4d, pt: %3d, err: %6.2f",
                           cam_idx, fnum, pt_idx, std::sqrt(dx2 + dy2));
                }
                res_idx++;
              }
            }
          }
        }
      }
    }
  }

  CalibDataVec Calibrator::getCalibrationResults() const {
    CalibDataVec res;
    if (calibrationData_.empty() || worldPoints_.empty()) {
      return (res);
    }
    const CameraExtrinsics &T_cam0_imu = calibrationData_[0].T_cam_imu;
    const std::vector<double> &p = params_;

    CameraExtrinsics T_cn_cam0 = identity();
    unsigned int numIntrinsics = get_num_intrinsics(calibrationData_);
    unsigned int intrinsics_offset = 0;

    CameraExtrinsics T_cnm1_0   = CameraExtrinsics::Identity();
    for (unsigned int cam_idx = 0; cam_idx < calibrationData_.size(); cam_idx++) {
      CalibrationData cd(calibrationData_[cam_idx]);
      CameraExtrinsics T_cn_0   = CameraExtrinsics::Identity();
      if (cam_idx > 0) {
        unsigned int off = numIntrinsics + 6 * (cam_idx - 1);
        const Vec<double, 3> rvec = Eigen::Map<const Vec<double, 3>>(&p[off]);
        Mat<double, 3, 3>       R = rotation_matrix(rvec);
        const Vec<double, 3>    t = Eigen::Map<const Vec<double, 3>>(&p[off + 3]);

        T_cn_0.block<3,3>(0, 0) = R;
        T_cn_0.block<3,1>(0, 3) = t;
        T_cn_0(3,3) = 1.0;

        cd.T_cn_cnm1 = T_cn_0 * T_cnm1_0.inverse();

        T_cn_cam0 = cd.T_cn_cnm1 * T_cn_cam0;
        if (isNonZero(T_cam0_imu)) {
          cd.T_cam_imu = T_cn_0 * T_cam0_imu;
        }
      }

      T_cnm1_0 = T_cn_0;

      // offset for intrinsics
      for (unsigned int i = 0; i < cd.intrinsics.intrinsics.size(); i++) {
        cd.intrinsics.intrinsics[i] = p[intrinsics_offset + i];
      }
      for (unsigned int i = 0; i < cd.intrinsics.distortion_coeffs.size(); i++) {
        cd.intrinsics.distortion_coeffs[i] = p[intrinsics_offset +
                                               cd.intrinsics.intrinsics.size() + i];
      }
      res.push_back(cd);
      intrinsics_offset += cd.intrinsics.intrinsics.size() + cd.intrinsics.distortion_coeffs.size();
    }
    return (res);
  }

  void Calibrator::setupOptimizationProblem(ceres::Problem *prob, std::vector<double> *vars) {
    std::vector<double> &params = *vars;
    ceres::Problem     &problem = *prob;

    const unsigned int num_frames = worldPoints_[0].size();
    const unsigned int num_cameras = calibrationData_.size();

    //
    // Create one FrameResidual for each frame
    //
    for(const auto i : irange(0u, num_frames))  {
      std::vector<FrameWorldPoints> frame_world_points;
      std::vector<FrameImagePoints> frame_image_points;
      std::vector<std::vector<bool>> masks;
      size_t frame_num_points = getPointsAndMasks(i, &frame_world_points,
                                                  &frame_image_points, &masks);
      if (frame_num_points == 0) {
        // none of the active cameras sees any points in this frame!
        continue;
      }
      auto cost_function = new ceres::DynamicAutoDiffCostFunction<FrameResidual>(
        new FrameResidual(frame_world_points, frame_image_points, masks, calibrationData_));
      std::vector<double *> v = params_to_blocks(calibrationData_, params, i);
      // per camera intrinsics
      for (const auto cam_idx : irange(0ul, calibrationData_.size())) {
        const CalibrationData &cd = calibrationData_[cam_idx];
        unsigned int nparam = cd.intrinsics.intrinsics.size() +
          cd.intrinsics.distortion_coeffs.size();
        cost_function->AddParameterBlock(nparam);
      }
      // (ncam-1) extrinsics
      for (const auto iext: irange(1ul, calibrationData_.size())) {
        (void)iext;
        cost_function->AddParameterBlock(6);
      }
      // Pose of cam0 wrt calib board
      cost_function->AddParameterBlock(6);

      // Reprojection error
      cost_function->SetNumResiduals(2 * frame_num_points);
      problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), v);

      // fix intrinsic parameters where requested
      for (const auto cam_idx : irange(0ul, calibrationData_.size())) {
        const CalibrationData &cd = calibrationData_[cam_idx];
        if (fixAllIntrinsics_ || cd.fixIntrinsics) {
          problem.SetParameterBlockConstant(v[cam_idx]);
        }
      }
      // fix extrinsics where requested
      for (const auto iext : irange(1ul, calibrationData_.size()))  {
        if (calibrationData_[iext].fixExtrinsics) {
          problem.SetParameterBlockConstant(v[iext + calibrationData_.size() - 1]);
        }
      }
    }
  }
}


#include <boost/test/unit_test.hpp>
#include <templ/solvers/temporal/SimpleTemporalNetwork.hpp>
#include <numeric/Combinatorics.hpp>
#include <graph_analysis/GraphIO.hpp>
#include <graph_analysis/WeightedEdge.hpp>

using namespace templ::solvers;
using namespace templ::solvers::temporal;

BOOST_AUTO_TEST_SUITE(simple_temporal_network)

BOOST_AUTO_TEST_CASE(value_propagation_1)
{
    {
        // [10,80] -- 38 --> [30,100]
        //    <------ -30 ----|
        // Expected after propagation
        // [10,70] -- 38 --> [48,100]
        SimpleTemporalNetwork stn;
        point_algebra::TimePoint::Ptr tp0(new point_algebra::TimePoint(10,80));
        point_algebra::TimePoint::Ptr tp1(new point_algebra::TimePoint(30,100));
        //point_algebra::TimePoint::Ptr tp2(new point_algebra::TimePoint(0,100));

        stn.addInterval(tp0, tp1, Bounds(30,38));
        // Using for loop to check stability of result
        for(int i = 0; i < 2; ++i)
        {
            graph_analysis::BaseGraph::Ptr baseGraph = stn.propagate();

            BOOST_REQUIRE_MESSAGE( tp0->getUpperBound() == 70, "Upper bound corrected after propagation: expected 70, actual " << tp0->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp1->getLowerBound() == 48, "Lower bound corrected after propagation: expected 48, actual " << tp1->getLowerBound());

        }

        // [10,80] -- 38 --> [30,100] -- 30 --> [70,100]
        //    <------ -30 ----|    <--- -10 ------|
        // Expected after propagation
        // [10,60] -- 38 --> [48,90] --- 30 --> [78,100]
        //    <------ -30 ----|    <--- -10 ------|
        point_algebra::TimePoint::Ptr tp2(new point_algebra::TimePoint(70,100));
        stn.addInterval(tp1,tp2, Bounds(10,30));
        {
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);
        }
        // Using for loop to check stability of result
        for(int i = 0; i < 2; ++i)
        {
            stn.propagate();
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();
            BOOST_REQUIRE_MESSAGE( tp0->getUpperBound() == 60, "Upper bound corrected after propagation: expected 60, actual " << tp0->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp1->getLowerBound() == 48, "Lower bound corrected after propagation: expected 48, actual " << tp1->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp1->getUpperBound() == 90, "Upper bound corrected after propagation: expected 90, actual " << tp1->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp2->getLowerBound() == 78, "Lower bound corrected after propagation: expected 78, actual " << tp2->getLowerBound());

            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);
        }
    }
}

BOOST_AUTO_TEST_CASE(value_propagation_2)
{
    {
        SimpleTemporalNetwork stn;

        point_algebra::TimePoint::Ptr tp0(new point_algebra::TimePoint(20,200));
        point_algebra::TimePoint::Ptr tp1(new point_algebra::TimePoint(0,150));
        point_algebra::TimePoint::Ptr tp2(new point_algebra::TimePoint(0,50));

        stn.addInterval(tp1, tp0, Bounds(10,40));
        stn.addInterval(tp2, tp1, Bounds(20,70));

        {
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);
        }

        // Using for loop to check stability of result
        for(int i = 0; i < 2; ++i)
        {
            stn.propagate();
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();

            BOOST_REQUIRE_MESSAGE( tp0->getLowerBound() == 110, "Lower bound corrected after propagation: expected 110, actual " << tp0->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp0->getUpperBound() == 200, "Upper bound corrected after propagation: expected 200, actual " << tp0->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp1->getLowerBound() == 70, "Lower bound corrected after propagation: expected 70, actual " << tp1->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp1->getUpperBound() == 150, "Upper bound corrected after propagation: expected 150, actual " << tp1->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp2->getLowerBound() == 0, "Lower bound corrected after propagation: expected 0, actual " << tp2->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp2->getUpperBound() == 50, "Upper bound corrected after propagation: expected 50, actual " << tp2->getUpperBound());

            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);

        }
    }
}

BOOST_AUTO_TEST_CASE(value_propagation_3)
{
    {
        SimpleTemporalNetwork stn;

        point_algebra::TimePoint::Ptr tp0(new point_algebra::TimePoint(10,20));
        point_algebra::TimePoint::Ptr tp1(new point_algebra::TimePoint(30,100));

        stn.addInterval(tp0, tp1, Bounds(30,38));

        {
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-before-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);
        }

        // Using for loop to check stability of result
        for(int i = 0; i < 2; ++i)
        {
            stn.propagate();
            graph_analysis::BaseGraph::Ptr baseGraph = stn.getDistanceGraph();

            BOOST_REQUIRE_MESSAGE( tp0->getLowerBound() == 10, "Lower bound corrected after propagation: expected 10, actual " << tp0->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp0->getUpperBound() == 20, "Upper bound corrected after propagation: expected 20, actual " << tp0->getUpperBound());
            BOOST_REQUIRE_MESSAGE( tp1->getLowerBound() == 48, "Lower bound corrected after propagation: expected 48, actual " << tp1->getLowerBound());
            BOOST_REQUIRE_MESSAGE( tp1->getUpperBound() == 100, "Upper bound corrected after propagation: expected 100, actual " << tp1->getUpperBound());

            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GEXF);
            graph_analysis::io::GraphIO::write("test_SimpleTemporalNetwork-after-domain_propagation", baseGraph, graph_analysis::representation::GRAPHVIZ);

        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <Eigen/Dense>
#include <algorithm>
#include "plugin/device/cpu/kernel/triu_cpu_kernel.h"
#include "plugin/device/cpu/hal/device/cpu_device_address.h"

namespace mindspore {
namespace kernel {
namespace {
constexpr size_t kTriuInputsNum = 1;
constexpr size_t kTriuOutputsNum = 1;
constexpr size_t kDim = 2;
}  // namespace

void TriuCpuKernelMod::InitKernel(const CNodePtr &kernel_node) {
  MS_EXCEPTION_IF_NULL(kernel_node);
  kernel_name_ = common::AnfAlgo::GetCNodeName(kernel_node);
  input_shape_ = common::AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 0);
  input_dtype_ = AnfAlgo::GetInputDeviceDataType(kernel_node, 0);

  input_dims_ = input_shape_.size();
  if (input_dims_ < kDim) {
    MS_LOG(EXCEPTION)
      << "For Triu, the input tensor's rank must be at least 2 for 'Triu' Op, but input tensor's rank is "
      << input_dims_ << ".";
  }
  if (common::AnfAlgo::HasNodeAttr("diagonal", kernel_node)) {
    diagonal_ = common::AnfAlgo::GetNodeAttr<int64_t>(kernel_node, "diagonal");
  }
}

template <typename T>
bool TriuCpuKernelMod::TriuCompute(const std::vector<AddressPtr> &inputs, const std::vector<AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kTriuInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kTriuOutputsNum, kernel_name_);

  auto input_addr = reinterpret_cast<T *>(inputs[0]->addr);
  auto output_addr = reinterpret_cast<T *>(outputs[0]->addr);

  size_t input_size = 1;
  for (size_t i = 0; i < input_dims_; ++i) {
    input_size *= input_shape_[i];
  }

  using MatrixMap = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>;

  auto matrix_width = input_shape_[input_dims_ - 2];
  auto matrix_height = input_shape_[input_dims_ - 1];
  auto matrix_size = matrix_width * matrix_height;
  auto matrixs_num = input_size / matrix_size;

  for (size_t k = 0; k < matrixs_num; ++k) {
    MatrixMap input(input_addr + k * matrix_size, matrix_width, matrix_height);
    MatrixMap output(output_addr + k * matrix_size, matrix_width, matrix_height);
    output = input.template triangularView<Eigen::Upper>();
    if (diagonal_ < 0) {
      for (size_t j = 0; j < matrix_height; j++) {
        for (size_t i = j + 1; i <= j - diagonal_ && i < matrix_width; i++) {
          output(i, j) = input(i, j);
        }
      }
    } else {
      for (size_t i = 0; i < matrix_width; i++) {
        for (size_t j = i; j < i + diagonal_ && j < matrix_height; j++) {
          output(i, j) = static_cast<T>(0.0);
        }
      }
    }
  }
  return true;
}

bool TriuCpuKernelMod::Launch(const std::vector<kernel::AddressPtr> &inputs, const std::vector<kernel::AddressPtr> &,
                              const std::vector<kernel::AddressPtr> &outputs) {
  switch (input_dtype_) {
    case kNumberTypeUInt8:
      return TriuCompute<uint8_t>(inputs, outputs);
    case kNumberTypeUInt16:
      return TriuCompute<uint16_t>(inputs, outputs);
    case kNumberTypeUInt32:
      return TriuCompute<uint32_t>(inputs, outputs);
    case kNumberTypeUInt64:
      return TriuCompute<uint64_t>(inputs, outputs);
    case kNumberTypeInt8:
      return TriuCompute<int8_t>(inputs, outputs);
    case kNumberTypeInt16:
      return TriuCompute<int16_t>(inputs, outputs);
    case kNumberTypeInt32:
      return TriuCompute<int32_t>(inputs, outputs);
    case kNumberTypeInt64:
      return TriuCompute<int64_t>(inputs, outputs);
    case kNumberTypeFloat16:
      return TriuCompute<float16>(inputs, outputs);
    case kNumberTypeFloat32:
      return TriuCompute<float>(inputs, outputs);
    case kNumberTypeFloat64:
      return TriuCompute<double>(inputs, outputs);
    case kNumberTypeBool:
      return TriuCompute<bool>(inputs, outputs);
    default:
      MS_LOG(ERROR) << "Unsupported data type.";
  }
  return true;
}

std::vector<KernelAttr> TriuCpuKernelMod::GetOpSupport() {
  static std::vector<KernelAttr> support_list = {
    KernelAttr().AddInputAttr(kNumberTypeUInt8).AddOutputAttr(kNumberTypeUInt8),
    KernelAttr().AddInputAttr(kNumberTypeUInt16).AddOutputAttr(kNumberTypeUInt16),
    KernelAttr().AddInputAttr(kNumberTypeUInt32).AddOutputAttr(kNumberTypeUInt32),
    KernelAttr().AddInputAttr(kNumberTypeUInt64).AddOutputAttr(kNumberTypeUInt64),
    KernelAttr().AddInputAttr(kNumberTypeInt8).AddOutputAttr(kNumberTypeInt8),
    KernelAttr().AddInputAttr(kNumberTypeInt16).AddOutputAttr(kNumberTypeInt16),
    KernelAttr().AddInputAttr(kNumberTypeInt32).AddOutputAttr(kNumberTypeInt32),
    KernelAttr().AddInputAttr(kNumberTypeInt64).AddOutputAttr(kNumberTypeInt64),
    KernelAttr().AddInputAttr(kNumberTypeFloat16).AddOutputAttr(kNumberTypeFloat16),
    KernelAttr().AddInputAttr(kNumberTypeFloat32).AddOutputAttr(kNumberTypeFloat32),
    KernelAttr().AddInputAttr(kNumberTypeFloat64).AddOutputAttr(kNumberTypeFloat64),
    KernelAttr().AddInputAttr(kNumberTypeBool).AddOutputAttr(kNumberTypeBool)};
  return support_list;
}

MS_KERNEL_FACTORY_REG(NativeCpuKernelMod, Triu, TriuCpuKernelMod);
}  // namespace kernel
}  // namespace mindspore


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;
typedef GraphTraits::vertex_descriptor Vertex;
typedef GraphTraits::edge_descriptor Edge;

void testcase()
{
  int n, m, k, l;
  std::cin >> n >> m >> k >> l;
  assert(n >= 1 && n <= 500 && m >= 0 && m <= 1e4 && k >= 1 && k <= 500 && l >= 1 && l <= 500);

  std::vector<int> station_counts_by_loc(n, 0);
  for (int i = 0; i < k; i++)
  {
    int x;
    std::cin >> x;
    assert(x >= 0 && x < n);
    station_counts_by_loc.at(x)++;
  }

  std::vector<int> photo_counts_by_loc(n, 0);
  for (int i = 0; i < l; i++)
  {
    int x;
    std::cin >> x;
    assert(x >= 0 && x < n);
    photo_counts_by_loc.at(x)++;
  }

  std::vector<std::vector<int>> alt_road_counts(n, std::vector<int>(n, 0));
  for (int i = 0; i < m; i++)
  {
    int x, y;
    std::cin >> x >> y;
    assert(x >= 0 && x < n && y >= 0 && y < n && x != y);
    alt_road_counts.at(x).at(y)++;
  }

  const int max_possible_flow = std::min(k, l);

  const int num_vertices = 2 + 2 * n;
  int next_free_vertex = 0;
  const Vertex source = next_free_vertex++;
  const Vertex sink = next_free_vertex++;
  auto intersection_at = [n, next_free_vertex](int i, bool post_photo_layer) -> Vertex {
    assert(i >= 0 && i < n);
    return next_free_vertex + (post_photo_layer ? n : 0) + i;
  };
  next_free_vertex += 2 * n;
  assert(next_free_vertex == num_vertices);
  Graph G(num_vertices);

  auto add_edge = [&G](int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  for (int i = 0; i < n; i++)
  {
    int count = station_counts_by_loc.at(i);
    if (count > 0)
    {
      add_edge(source, intersection_at(i, false), count);
      add_edge(intersection_at(i, true), sink, count);
    }
  }

  for (int i = 0; i < n; i++)
  {
    int count = photo_counts_by_loc.at(i);
    if (count > 0)
    {
      add_edge(intersection_at(i, false), intersection_at(i, true), count);
    }
  }

  for (int i = 0; i < n; i++)
  {
    for (int j = 0; j < n; j++)
    {
      int count = alt_road_counts.at(i).at(j);
      if (count == 0)
      {
        continue;
      }
      assert(count > 0);

      DEBUG(2, "alt roads between " << i << " and " << j << ": " << count);

      add_edge(intersection_at(i, false), intersection_at(j, false), max_possible_flow);
      add_edge(intersection_at(i, true), intersection_at(j, true), count);
    }
  }

  int flow = boost::push_relabel_max_flow(G, source, sink);
  assert(flow >= 0 && flow <= max_possible_flow);
  std::cout << flow << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"MinimumJerkInterpolation.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"
#include "left_hand.h"
#include <sensor_msgs/JointState.h>
#include <cstdlib>
#include<termios.h>
#include <iostream>
#include <QTime>

using namespace  std;
using namespace  Eigen;

void matrix_view(MatrixXd M){
    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {str+=QString::number(M(i,j));str+="   ";}
        qDebug()<<str;}qDebug()<<"";}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;qDebug()<<"";}

double move2pose(double max,double t_local,double T_start ,double T_end){
    double T_move=T_end-T_start;
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_end){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else{theta=max;}
    return theta;
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

int qc_offset[12];
int qra_offset[4];
int qla_offset[4];
bool qc_initial_bool;
void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        for (int i = 12; i < 16; ++i){
            qra_offset[i-12]=int(msg.position[i+1]);
        }

        for (int i = 20; i < 24; ++i){
            qla_offset[i-20]=int(msg.position[i+1]);
        }

        qc_initial_bool=false;

        ROS_INFO("Offset_feet=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);
        ROS_INFO("Offset_righthand=%d\t%d\t%d\t%d",qra_offset[0],qra_offset[1], qra_offset[2],qra_offset[3]);
        ROS_INFO("Offset_lefthand=%d\t%d\t%d\t%d",qla_offset[0],qla_offset[1], qla_offset[2],qla_offset[3]);
        ROS_INFO("Initialized!");
    }
}

VectorXd absolute_q0_r(7);VectorXd absolute_q0_l(7);
bool abs_initial_bool;

void abs_read(const sensor_msgs::JointState & msg){
    if (abs_initial_bool){

        VectorXd absolute_sensor_r(7);VectorXd absolute_sensor_l(7);
        for (int i = 0; i < 7; ++i) {
            absolute_sensor_r(i)=msg.position[i+13];            absolute_q0_r(i)=0;
            absolute_sensor_l(i)=msg.position[i+13+8];            absolute_q0_l(i)=0;
        }
        //right hand
        absolute_q0_r(0)=double(absolute_sensor_r(0)+863)/8192*2*M_PI;
        absolute_q0_r(1)=double(absolute_sensor_r(1)-(-889+2275))/8192*2*M_PI;
        absolute_q0_r(2)=-double(absolute_sensor_r(2)-2881)/8192*2*M_PI;
        absolute_q0_r(3)=double(absolute_sensor_r(3)-2058)/8192*2*M_PI;
        //    //left hand
        absolute_q0_l(0)=double(absolute_sensor_l(0)-705)/8192*2*M_PI;
        absolute_q0_l(1)=double(absolute_sensor_l(1)-(3642-2275))/8192*2*M_PI;
        absolute_q0_l(2)=-double(absolute_sensor_l(2)+344)/8192*2*M_PI;
        absolute_q0_l(3)=-double(absolute_sensor_l(3)-47)/8192*2*M_PI;
        qDebug()<<"absolutes:";
        matrix_view(absolute_sensor_r);matrix_view(absolute_sensor_l);
        qDebug()<<"positions:";
        VectorXd absolute_q0_r_deg(7);
        absolute_q0_r_deg=180/M_PI*absolute_q0_r;
        matrix_view(absolute_q0_r_deg);
        VectorXd absolute_q0_l_deg(7);
        absolute_q0_l_deg=180/M_PI*absolute_q0_l;
        matrix_view(absolute_q0_l_deg);

        abs_initial_bool=false;
    }

}

int main(int argc, char **argv)
{

    abs_initial_bool=true;
    qc_initial_bool=true;

    ros::init(argc, argv, "mynode");

    ros::NodeHandle nh;

    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
    ros::Subscriber abs_sensor = nh.subscribe("/surena/abs_joint_state", 1000, abs_read);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);

    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    VectorXd q_ra(7) ;VectorXd q_la(7);
q_ra.fill(0);
q_la.fill(0);

    int q_motor_r[8];int q_motor_l[8];
    for (int var = 0; var < 8; ++var) {
        q_motor_r[var]=0;q_motor_l[var]=0;
    }

    ros::Rate loop_rate(200);
    int count = 0;
    double time=0.0;

    VectorXd qr_home(7);
    VectorXd ql_home(7);

    qr_home<<10*M_PI/180,-10*M_PI/180,0,-20*M_PI/180, 0,0,0;
    ql_home<<10*M_PI/180, 10*M_PI/180,0,-20*M_PI/180, 0,0,0;

bool initializing=true;
    while (ros::ok())
    {

        if (abs_initial_bool) {
            ROS_INFO_ONCE("abs is initializing!");
            ros::spinOnce();
            continue;
        }

        if (qc_initial_bool) {
            ROS_INFO_ONCE("qc is initializing!");
            ros::spinOnce();
            continue;
        }

        if(initializing){
        ROS_INFO("press any key to start!");
        getch();
        initializing=false;
    }

        time=double(count)*.005;
        for (int i = 0; i < 4; ++i) {
            q_ra(i)=absolute_q0_r(i)+move2pose(qr_home(i)-absolute_q0_r(i),time,0,3);
            q_la(i)=absolute_q0_l(i)+move2pose(ql_home(i)-absolute_q0_l(i),time,0,3);
        }
        q_motor_r[0]=-int(10*(q_ra(0)-absolute_q0_r(0))*180/M_PI*120/60)+qra_offset[0];
        q_motor_r[1]= int(10*(q_ra(1)-absolute_q0_r(1))*180/M_PI*120/60)+qra_offset[1];
        q_motor_r[2]= -int(7*(q_ra(2)-absolute_q0_r(2))*180/M_PI*100/60)+qra_offset[2];
        q_motor_r[3]=  int(7*(q_ra(3)-absolute_q0_r(3))*180/M_PI*100/60)+qra_offset[3];

        q_motor_r[4]=0;
        q_motor_r[5]=0;
        q_motor_r[6]=0;
        q_motor_r[7]=9;

        q_motor_l[0]=int(10*(q_la(0)-absolute_q0_l(0))*180/M_PI*120/60)+qla_offset[0];
        q_motor_l[1]=int(10*(q_la(1)-absolute_q0_l(1))*180/M_PI*120/60)+qla_offset[1];
        q_motor_l[2]=-int(7*(q_la(2)-absolute_q0_l(2))*180/M_PI*100/60)+qla_offset[2];
        q_motor_l[3]=-int(7*(q_la(3)-absolute_q0_l(3))*180/M_PI*100/60)+qla_offset[3];

        q_motor_l[4]=0;
        q_motor_l[5]=0;
        q_motor_l[6]=0;
        q_motor_l[7]=9;

        msg.data.clear();
        for(int  i = 0;i < 12;i++)
        {
            msg.data.push_back(qc_offset[i]);
        }
        //right hand epose
        msg.data.push_back(q_motor_r[0]);//12 -y  a,z
        msg.data.push_back(q_motor_r[1]);//13 +x
        msg.data.push_back(q_motor_r[2]);//14 -z
        msg.data.push_back(q_motor_r[3]);//15 +y
        //right hand dynamixel + fingers
        msg.data.push_back(q_motor_r[4]);//16
        msg.data.push_back(q_motor_r[5]);//17
        msg.data.push_back(q_motor_r[6]);//18
        msg.data.push_back(q_motor_r[7]);//19
        //left hand epose
        msg.data.push_back(q_motor_l[0]);//20 +y
        msg.data.push_back(q_motor_l[1]);//21 +x
        msg.data.push_back(q_motor_l[2]);//22 -z
        msg.data.push_back(q_motor_l[3]);//23 -y
        //left hand dynamixel + fingers
        msg.data.push_back(q_motor_l[4]);//24
        msg.data.push_back(q_motor_l[5]);//25
        msg.data.push_back(q_motor_l[6]);//26
        msg.data.push_back(q_motor_l[7]);//27
        ++count;
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        if(time>3){break;}

    }

    return 0;
}


/**
 * @file gausslobattoparabolic.cc
 * @brief NPDE exam TEMPLATE CODE FILE
 * @author Oliver Rietmann
 * @date 22.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "gausslobattoparabolic.h"

#include <lf/assemble/assemble.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <functional>
#include <memory>
#include <utility>

namespace GaussLobattoParabolic {

/* SAM_LISTING_BEGIN_1 */
lf::assemble::COOMatrix<double> initMbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  //====================
  // Your code goes here
  // Replace this dummy assignment for M:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> M(N, N);
  //====================

  return M;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
lf::assemble::COOMatrix<double> initAbig(
    std::shared_ptr<const lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space) {
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  //====================
  // Your code goes here
  // Replace this dummy assignment for A:
  int N = dofh.NumDofs();
  lf::assemble::COOMatrix<double> A(N, N);
  //====================

  return A;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
RHSProvider::RHSProvider(const lf::assemble::DofHandler &dofh,
                         std::function<double(double)> g)
    : g_(std::move(g)) {
  //====================
  // Your code goes here
  //====================
}

Eigen::VectorXd RHSProvider::operator()(double t) const {
  //====================
  // Your code goes here
  // Replace this dummy return value:
  return Eigen::VectorXd(0);
  //====================
}
/* SAM_LISTING_END_3 */

}  // namespace GaussLobattoParabolic


#include <cppunit/extensions/HelperMacros.h>
#include "PhysicsTools/Utilities/interface/Simplify.h"
#include "PhysicsTools/Utilities/interface/Fraction.h"
#include "PhysicsTools/Utilities/interface/Variables.h"
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>

class testSimplifications : public CppUnit::TestFixture {
  CPPUNIT_TEST_SUITE(testSimplifications);
  CPPUNIT_TEST(checkAll);
  CPPUNIT_TEST_SUITE_END();

public:
  void setUp() {}
  void tearDown() {}
  void checkAll();
};

CPPUNIT_TEST_SUITE_REGISTRATION(testSimplifications);

void testSimplifications::checkAll() {
  using namespace funct;
  using namespace boost;
  BOOST_STATIC_ASSERT((is_same<
		       Sum<Numerical<2>, Numerical<3> >::type,
		       Numerical<5>
		       >::value));

  BOOST_STATIC_ASSERT((is_same<
		       Difference<Numerical<2>, Numerical<3> >::type,
		       Numerical<-1>
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Product<Numerical<2>, Numerical<3> >::type,
		       Numerical<6>
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Minus<Numerical<6> >::type,
		       Numerical<-6>
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Fraction<2,4>::type,
		       Fraction<1,2>::type
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Fraction<-1,2>::type,
		       Fraction<1,-2>::type
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Fraction<4,2>::type,
		       Numerical<2>
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Fraction<3,4>::type,
		       Fraction<21,28>::type
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Fraction<3,1>::type,
		       Numerical<3>
		       >::value));
  BOOST_STATIC_ASSERT((is_same<
		       Product<Minus<X>::type, Minus<Y>::type>::type,
		       Product<X, Y>::type
		       >::value));
  BOOST_STATIC_ASSERT((!Parametric<Power<X, Y>::type>::value));
  BOOST_STATIC_ASSERT((is_same<
		       Product<Power<X, Y>::type, Power<X, Z>::type>::type,
		       Power<X, Sum<Y, Z>::type>::type
		       >::value));
  BOOST_STATIC_ASSERT((!Parametric<Ratio<Sin<X>::type, Cos<X>::type>::type>::value));
  BOOST_STATIC_ASSERT((is_same<
		       Ratio<Sin<X>::type, Cos<X>::type>::type,
		       Tan<X>::type
		       >::value));
}


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

#include <boost/simd/arithmetic/functions/remquo.hpp>
#include <boost/simd/include/functions/simd/round2even.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF          ( remquo_
                                       , tag::cpu_
                                       , (A0)(A1)
                                       , ( boost::is_same
                                          < typename dispatch::meta::
                                                     as_integer<A0,signed>::type
                                          , A1
                                          >
                                         )
                                       , (generic_< floating_<A0> >)
                                         (generic_< floating_<A0> >)
                                         (generic_< integer_<A1> > )
                                       )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A0 const& a1,A1& a3) const
    {
      result_type a2;
      boost::simd::remquo(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( remquo_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type quo_t;
    typedef std::pair<A0,quo_t>                                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 const& a1) const
    {
      A0 first;
      quo_t second;
      boost::simd::remquo( a0, a1, first, second );
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF          ( remquo_, tag::cpu_
                                       , (A0)(A1)
                                       , ( boost::is_same
                                           < typename dispatch::meta::
                                                      as_integer<A0,signed>::type
                                           , A1
                                           >
                                         )
                                       , (generic_<floating_<A0> >)
                                         (generic_<floating_<A0> >)
                                         (generic_<floating_<A0> >)
                                         (generic_<integer_ <A1> >)
                                       )
  {
    typedef void result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0, A0 const& a1,A0& a2, A1& a3) const
    {
      A0 const d = round2even(a0/a1);

#if defined(BOOST_SIMD_NO_INVALIDS)
      a2 = if_allbits_else(is_eqz(a1), a0-d*a1);
#else
      a2 = if_allbits_else(l_or(is_invalid(a0), is_eqz(a1)), a0-d*a1);
#endif

      a3 = toint(d);
    }
  };
} } }

#endif


#ifndef PROBABILISTIC_HPP_INCLUDED
#define PROBABILISTIC_HPP_INCLUDED
#include <vector>
#include <memory>
#include <random>
#include <math.h>
#include <cassert>
#include <unordered_map>
#include <unordered_set>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include "../probabilistic/gp.hpp"
#include "../utilities/utilities.hpp"

namespace boat {
double normal_lnp(double x, double mean, double variance);

template <class Particle> class SemiParamericEngine;
extern std::string dag_model_fake_name;

template <class Particle>
class ProbEngine {
  friend class boost::serialization::access;
  friend class SemiParamericEngine<Particle>;
 public:
  ProbEngine(int num_particles = 50000);

  ProbEngine(std::vector<Particle> particles);

  ProbEngine& set_num_particles(int num_particles);  // Resamples if
                                                        // initialized
  int get_num_particles() const;

  ProbEngine single_particle_engine() const ;

  template <class... Args>
  void create_using_constructor(const Args&... args);

  template <class ForwardIt>
  void create_from_range(ForwardIt begin, ForwardIt end);

  template <class... Args>
  void create_from_engines(const ProbEngine<Args>&... args);

  const Particle& value() const;

  template <class... Args>
  void observe(const Args&... args);

  template <class... Args>
  void observe_deterministic(const Args&... args);

  template <class... Args>
  void observe_i(bool deterministic, const Args&... args);

  template <class Func>
  typename std::result_of<Func(Particle)>::type average(Func func) const;

  template <class Func>
  void execute(Func func);

  template <class... Args>
  double predict(const Args&... args) const;

  double get_marginal_log_likelihood() const;

  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);

  void resample();

  const Particle& sample() const;
  const Particle& first_particle() const;

private:
  void initialize() const;
  void check_ess();
  void check_constructed() const;
  std::vector<const Particle*> get_random_selection(int new_ns) const;
  void init_particles_for_average(int pfa) const;

  template <class T = Particle>
  void construction_helper(
      typename std::enable_if<std::is_default_constructible<T>::value>::type*
          dummy = nullptr) const;

  template <class T = Particle>
  void construction_helper(
      typename std::enable_if<!std::is_default_constructible<T>::value>::type*
          dummy = nullptr) const;

  template <class T, int... S>
  void cfe_helper(const T& inputs, seq<S...>);

  mutable bool initialized_;
  size_t num_particles_;
  double num_particles_inv_;
  mutable std::vector<double> log_likelihoods_;
  mutable std::vector<double> normalized_likelihoods_;
  mutable std::vector<Particle> particles_;
  mutable std::vector<const Particle*> particles_for_average_;
  mutable double mll_;
};

// Semi-parametric utilities

template <class Particle>
class SemiParamericEngine;

template <class Child>
class DAGModel;

struct Distrib {
  std::string name_;
  double mu_;
  double var_;
  friend std::ostream &operator<<(std::ostream& os, const Distrib& d){
    os << d.name_ << ":  mu " << d.mu_ << " std " << sqrt(d.var_);
    return os;
  }
};

template <class Child>
class SemiParametricModel {
  template <class Particle> friend class SemiParamericEngine;
public:
  template<class... Args>
  GaussianDistrib predict_distrib(const Args&... args) const;

  template<class... Args>
  double predict_mean(const Args&... args) const;

  template<class... Args>
  double observe(double result, const Args&... args) const;
protected:

  template <class... Args>
  double parametric(const Args&... args) const;

  template <class... Args>
  double parametric_noise(const Args&... args) const;

  template <class... Args>
  std::vector<double> to_vec(const Args&... args) const;

  void set_params(GPParams params);

//private:
  Child* crtp_this();
  const Child* crtp_this() const;

  double default_noise_;
  mutable TreedGPS gp_;
};

template <class Child>
class DAGModel {
public:
  DAGModel();

  void set_num_particles(int np);
  template <class... Args>
  void observe(const std::unordered_map<std::string, double>& measurements,
               const Args&... args);

  template <class... Args>
  std::vector<Distrib> predict(const Args&... args);

  template <class... Args>
  double expected_improvement(const std::string& target, double incumbent,
                              const Args&... args);

  std::unique_ptr<Child> sample();
  template <class... Args>
  double sample_predict(const std::string& target, const Args&... args);

protected:
  template <class Particle, class... Args>
  double output(const std::string& name, ProbEngine<Particle>& eng,
                const Args&... args);

  double output(const std::string& name, double value);
private:

  template <class... Args>
  void run_test_pass(const Args&... args);

  void clear_test_pass();
  //Output functions for each situation
  template <class Particle, class... Args>
  double output_test_pass(const std::string& name, ProbEngine<Particle>& eng,
                          const Args&... args);

  double output_test_pass(const std::string& name, double val);

  template <class Particle, class... Args>
  double output_predict(const std::string& name, ProbEngine<Particle>& eng,
                          const Args&... args);

  double output_predict(const std::string& name, double val);

  template <class Particle, class... Args>
  double output_ei(const std::string& name, ProbEngine<Particle>& eng,
                          const Args&... args);

  double output_ei(const std::string& name, double val);

  template <class Particle, class... Args>
  double output_sample_predict(const std::string& name,
                               ProbEngine<Particle>& eng,
                               const Args&... args);

  double output_sample_predict(const std::string& name, double val);

  template <class Particle, class... Args>
  double output_past_target(const std::string& name, ProbEngine<Particle>& eng,
                          const Args&... args);

  double output_past_target(const std::string& name, double val);

  template <class Particle, class... Args>
  double output_observe(const std::string& name, ProbEngine<Particle>& eng,
                          const Args&... args);

  double output_observe(const std::string& name, double val);

  void check_unset();
  void check_output_list(const std::string& name, void* eng_addr);
  // This following function resets the random number generator so
  // results are the same if we give the same input. This makes debugging easier
  void reset_generator();
  Child* crtp_this();
  const Child* crtp_this() const;

  bool is_sample_;
  int num_particles_;

  //Observe
  bool observe_;
  const std::unordered_map<std::string, double>* measurements_;
  std::unordered_set<std::string> observed_measurements_;

  // Predict
  bool predict_;
  std::vector<double> predict_mus_and_vars_;

  // Useful for the next two that have targets
  const std::string* target_;
  bool past_target_;

  //EI
  bool ei_;
  double incumbent_;
  double ei_res_;
  bool minimizing_;

  //Sample predict
  bool sample_predict_;
  std::unordered_map<void *, double> fixed_draws_;
  double sample_predict_value_;

  // Copying necessary engines
  bool test_pass_;
  std::unordered_set<std::string> need_observe_;
  std::unordered_map<void *, std::vector<std::string> > seen_outputs_;
  std::unordered_map<void*, std::shared_ptr<void> > duplicated_engines_;
  std::vector<std::pair<std::string, void*> > output_list_;
  int output_list_index_;
};

}
#include "probabilistic_t.hpp"

#endif  // PROBABILISTIC_HPP_INCLUDED


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

PCEEdge::PCEEdge(const Vector3d& l_cutting_line_s_, const Vector3d& l_cutting_line_e_, const Vector3d& cutting_surface_normal_,
	const Vector3d& l_u_vector_)
{
	cut.l_cutting_line_s = l_cutting_line_s_;
	cut.l_cutting_line_e=l_cutting_line_e_;
	cut.cutting_surface_normal = cutting_surface_normal_;
	cut.l_u_vector = l_u_vector_;

	if (Math::Functs::IsAlmostZero(Math::Functs::GetAngleBetween(Vector3d(0.0, 0.0, 1.0), cutting_surface_normal_) - Math::Math_PI / 2.0))
	{
		style = VerticalEnd;

		cutting_line_s = cut.l_cutting_line_s;
		cutting_line_e = cut.l_cutting_line_e;

		cut.offset_s = Vector2d(cut.l_cutting_line_s);
		cut.offset_e = Vector2d(cut.l_cutting_line_e);

		cut.u_cutting_line_s = cut.l_cutting_line_s + l_u_vector_;
		cut.u_cutting_line_e = cut.l_cutting_line_e + l_u_vector_;
	}
	else
	{
		style = TiltingEnd;
		cut.u_cutting_line_s = cut.l_cutting_line_s + l_u_vector_;
		cut.u_cutting_line_e = cut.l_cutting_line_e + l_u_vector_;

		if (Math::Functs::GetAngleBetween(cutting_surface_normal_, Vector3d(0.0, 0.0, 1.0)) < Math::Math_PI / 2.0)
		{
			cutting_line_s = cut.l_cutting_line_s;
			cutting_line_e = cut.l_cutting_line_e;
			cut.offset_s = Vector2d(cutting_line_s);
			cut.offset_e = Vector2d(cutting_line_e);
		}
		else
		{
			cutting_line_s = cut.u_cutting_line_s;
			cutting_line_e = cut.u_cutting_line_e;
			cut.offset_s = Vector2d(cutting_line_s);
			cut.offset_e = Vector2d(cutting_line_e);
		}
	}
}

PCEEdge::PCEEdge(const int& index_, const int& shape_index_, const PCECUT& cut_) :
	index(index_), part_index(shape_index_),cut(cut_)
{
	if (Math::Functs::IsAlmostZero(Math::Functs::GetAngleBetween(Vector3d(0.0, 0.0, 1.0), cut_.cutting_surface_normal) - Math::Math_PI / 2.0))
		style = VerticalEnd;
	else
		style = TiltingEnd;

	Set(cut_);
}

void PCEEdge::Set(const PCECUT& cut_)
{
	cut = cut_;
	cut.offset_c = (cut.offset_s + cut.offset_e) / 2.0;
	cut.offset_n[0] = cut.offset_s[1] - cut.offset_e[1];//-y
	cut.offset_n[1] = cut.offset_e[0] - cut.offset_s[0];//x
	Math::Functs::SetVectorLength(cut.offset_n, 1.0);

	cut.origin_c = (cut.origin_s + cut.origin_e) / 2.0;
	cut.origin_n[0] = cut.origin_s[1] - cut.origin_e[1];//-y
	cut.origin_n[1] = cut.origin_e[0] - cut.origin_s[0];//x
	Math::Functs::SetVectorLength(cut.origin_n, 1.0);
}


//
// Copyright (c) 2017-2018 CNRS
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  forwardKinematics(model,data_ref,q,v,a);
  computeForwardKinematicsDerivatives(model,data,q,v,a);

  for(size_t i = 1; i < (size_t)model.njoints; ++i)
  {
    BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i]));
    BOOST_CHECK(data.v[i].isApprox(data_ref.v[i]));
    BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i])));
    BOOST_CHECK(data.a[i].isApprox(data_ref.a[i]));
    BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i])));
  }

  computeJointJacobians(model,data_ref,q);
  BOOST_CHECK(data.J.isApprox(data_ref.J));

  computeJointJacobiansTimeVariation(model, data_ref, q, v);
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
}

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  computeForwardKinematicsDerivatives(model,data,q,v,a);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
  Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
  Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
  Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();

  getJointVelocityDerivatives(model,data,jointId,WORLD,
                              partial_dq,partial_dv);

  getJointVelocityDerivatives(model,data,jointId,LOCAL,
                              partial_dq_local,partial_dv_local);

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);

  BOOST_CHECK(partial_dv.isApprox(J_ref));
  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));

  // Check against finite differences
  Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
  Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
  Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
  Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
  const double alpha = 1e-6;

  // dvel/dv
  Eigen::VectorXd v_plus(v);
  Data data_plus(model);
  forwardKinematics(model,data_ref,q,v);
  Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
  Motion v0_local(data_ref.v[jointId]);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v_plus);

    partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
    partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
    v_plus[k] -= alpha;
  }

  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));

  // dvel/dq
  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
  forwardKinematics(model,data_ref,q,v);
  v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    forwardKinematics(model,data_plus,q_plus,v);

    partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
    partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));

  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));

//  computeJointJacobiansTimeVariation(model,data_ref,q,v);
//  Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.setZero();
//  getJointJacobianTimeVariation<WORLD>(model,data_ref,jointId,dJ_ref);
//  BOOST_CHECK(partial_dq.isApprox(dJ_ref));
//
//  std::cout << "partial_dq\n" << partial_dq << std::endl;
//  std::cout << "dJ_ref\n" << dJ_ref << std::endl;

}

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  computeForwardKinematicsDerivatives(model,data,q,v,a);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
  Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
  Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
  Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
  Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();

  getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getJointAccelerationDerivatives(model,data,jointId,LOCAL,
                                  v_partial_dq_local,
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);

  // Check v_partial_dq against getJointVelocityDerivatives
  {
    Data data_v(model);
    computeForwardKinematicsDerivatives(model,data_v,q,v,a);

    Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
    Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
    Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();

    getJointVelocityDerivatives(model,data_v,jointId,WORLD,
                                v_partial_dq_ref,v_partial_dv_ref);

    BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
    getJointVelocityDerivatives(model,data_v,jointId,LOCAL,
                                v_partial_dq_ref_local,v_partial_dv_ref);

    BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
  }

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);

  BOOST_CHECK(a_partial_da.isApprox(J_ref));
  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));

  // Check against finite differences
  Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
  Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
  const double alpha = 1e-8;

  Eigen::VectorXd v_plus(v), a_plus(a);
  Data data_plus(model);
  forwardKinematics(model,data_ref,q,v,a);

  // dacc/da
  Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId]));
  Motion a0_local(data_ref.a[jointId]);
  for(int k = 0; k < model.nv; ++k)
  {
    a_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v,a_plus);

    a_partial_da_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
    a_plus[k] -= alpha;
  }
  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));

  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_da,a_partial_da_local);
  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));

  // dacc/dv
  Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
  Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v_plus,a);

    a_partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
    v_plus[k] -= alpha;
  }

  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));

  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_dv,a_partial_dv_local);
  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));

  // dacc/dq
  a_partial_dq.setZero();
  a_partial_dv.setZero();
  a_partial_da.setZero();

  a_partial_dq_local.setZero();
  a_partial_dv_local.setZero();
  a_partial_da_local.setZero();

  Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
  Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();
//  a.setZero();

  computeForwardKinematicsDerivatives(model,data,q,v,a);
  getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getJointAccelerationDerivatives(model,data,jointId,LOCAL,
                                  v_partial_dq_local,
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);

  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
  forwardKinematics(model,data_ref,q,v,a);
  a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]);
  a0_local = data_ref.a[jointId];

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    forwardKinematics(model,data_plus,q_plus,v,a);

    a_partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));

  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));

}

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model,true);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  const Model::JointIndex jointId = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1);
  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
  Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
  Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();

  // LOCAL: da/dv == dJ/dt + dv/dq
//  {
//    Data::Matrix6x rhs(6,model.nv); rhs.setZero();
//
//    v_partial_dq.setZero();
//    a_partial_dq.setZero();
//    a_partial_dv.setZero();
//    a_partial_da.setZero();
//
//    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
//    computeForwardKinematicsDerivatives(model,data,q,v,a);
//
//    getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
//                                           v_partial_dq,
//                                           a_partial_dq,a_partial_dv,a_partial_da);
//
//    getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs);
//
//    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
//    getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId,
//                                       v_partial_dq_ref,v_partial_dv_ref);
//    rhs += v_partial_dq_ref;
//    BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
//
//    std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
//  }

  // WORLD: da/dv == dJ/dt + dv/dq
  {
    Data::Matrix6x rhs(6,model.nv); rhs.setZero();

    v_partial_dq.setZero();
    a_partial_dq.setZero();
    a_partial_dv.setZero();
    a_partial_da.setZero();

    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
    computeForwardKinematicsDerivatives(model,data,q,v,a);

    getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                    v_partial_dq,
                                    a_partial_dq,a_partial_dv,a_partial_da);

    getJointJacobianTimeVariation(model,data_ref,jointId,WORLD,rhs);

    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
    getJointVelocityDerivatives(model,data_ref,jointId,WORLD,
                                v_partial_dq_ref,v_partial_dv_ref);
    rhs += v_partial_dq_ref;
    BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));

//    std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
  }

//  // WORLD: da/dq == d/dt(dv/dq)
//  {
//    const double alpha = 1e-8;
//    Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
//
//    Data data_plus(model);
//    v_plus = v + alpha * a;
//    q_plus = integrate(model,q,alpha*v);
//
//    computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
//    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
//
//    Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
//    Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
//
//    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
//
//    v_partial_dq.setZero();
//    a_partial_dq.setZero();
//    a_partial_dv.setZero();
//    a_partial_da.setZero();
//
//    getJointVelocityDerivatives<WORLD>(model,data_ref,jointId,
//                                       v_partial_dq_ref,v_partial_dv_ref);
//    getJointVelocityDerivatives<WORLD>(model,data_plus,jointId,
//                                       v_partial_dq_plus,v_partial_dv_plus);
//
//    getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId,
//                                           v_partial_dq,
//                                           a_partial_dq,a_partial_dv,a_partial_da);
//
//    Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
//    {
//      Data data_fd(model);
//      VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero();
//      for(int k = 0; k < model.nv; ++k)
//      {
//        v_eps[k] += alpha;
//        q_fd = integrate(model,q,v_eps);
//        forwardKinematics(model,data_fd,q_fd,v,a);
//        a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) - data_ref.oa[jointId]).toVector()/alpha;
//        v_eps[k] = 0.;
//      }
//    }
//
//    Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
//    BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
//
//    std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
//    std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
//  }

  // LOCAL: da/dq == d/dt(dv/dq)
  {
    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);

    Data data_plus(model);
    v_plus = v + alpha * a;
    q_plus = integrate(model,q,alpha*v);

    computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);

    Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
    Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();

    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();

    v_partial_dq.setZero();
    a_partial_dq.setZero();
    a_partial_dv.setZero();
    a_partial_da.setZero();

    getJointVelocityDerivatives(model,data_ref,jointId,LOCAL,
                                       v_partial_dq_ref,v_partial_dv_ref);
    getJointVelocityDerivatives(model,data_plus,jointId,LOCAL,
                                v_partial_dq_plus,v_partial_dv_plus);

    getJointAccelerationDerivatives(model,data_ref,jointId,LOCAL,
                                    v_partial_dq,
                                    a_partial_dq,a_partial_dv,a_partial_da);

    Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
    BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));

//    std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
  }

}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/strong_type.hpp>

/// This example tries to see how close we can define money using strong_type
/// money should be close to strong_counter as it is close to duration.
/// However, there are some issues.
///     There is not a common currency (until we decide to have it)
///         Adding money with different currencies needs the currency result, and so needs to be lazy
///     Having a common currency means to do two operations to convert from two currencies.
///         The compiler could optimize it if the currency is know at compile time, but in general this is not the case.
///     Should we have implicit or explicit conversions between currencies?

#include <functional>
#include <type_traits>
#include "currency.hpp"
#include <experimental/fundamental/v2/config.hpp>

template <class Currency, class Rep = double>
struct money;

namespace detail
{
/// helper function that converts a money to a specific currency using the conversion_factor customization point
template <class Currency, class C, class R>
double to_currency(money<C, R> const& m)
{
    return m.count() * currency::conversion_factor(m.currency(), Currency{});
}
}

// fixme: Should be Rep, Currency?
// fixme: Should we add a Period parameter to count in Million Dollars, or in cents?
//      Don't having a period , forces almost to work with double.
//      Having a period allows to work with integers most of the time

template <class Currency, class Rep>
struct money
{
    money() = default;

    // todo: Add template <class Rep2> as duration
    explicit constexpr money(Rep v) : rep(v) {}

    constexpr money(money const& m) = default;

    // fixme: Should we restrict it as do duration and require a money_cast?
    // or just define an explicit constructor as does pair.
    template <class C, class R>
    constexpr money(money<C, R> m)
    : rep (detail::to_currency<Currency>(m))
    {
    }
    JASEL_CXX14_CONSTEXPR money& operator=(money const& m) = default;
#if 0
    template <class C, class R>
    constexpr money& operator=(money<C, R> m)
    {
        rep = detail::to_currency<Currency>(m);
        return *this;
    }
#endif

    // fixme: Should we replace it by count as duration does?
    constexpr Rep count() const noexcept { return rep; }
    constexpr Currency currency() const noexcept { return Currency{}; }

    // todo: add zero, min, max

    friend constexpr bool operator==(money m1, money m2) noexcept
    {
        return m1.count() == m2.count();
    }
    friend constexpr bool operator!=(money m1, money m2) noexcept
    {
        return ! (m1 == m2);
    }
    friend constexpr money operator+(money m1) noexcept
    {
        return m1 ;
    }
    friend constexpr money operator+(money m1, money m2) noexcept
    {
        return money(m1.count() + m2.count()) ;
    }

    JASEL_CXX14_CONSTEXPR money& operator++() noexcept
    {
        rep++;
        return *this ;
    }
    JASEL_CXX14_CONSTEXPR money operator++(int) noexcept
    {
        money tmp = *this;
        rep++;
        return tmp ;
    }
    JASEL_CXX14_CONSTEXPR money& operator+=(money m) noexcept
    {
        rep += m.count();
        return *this ;
    }

    friend constexpr money operator-(money m1, money m2) noexcept
    {
        return money(m1.count() - m2.count()) ;
    }
    friend constexpr money operator-(money m1) noexcept
    {
        return money(- m1.count()) ;
    }
    JASEL_CXX14_CONSTEXPR money& operator--() noexcept
    {
        rep--;
        return *this ;
    }
    JASEL_CXX14_CONSTEXPR money operator--(int) noexcept
    {
        money tmp = *this;
        rep--;
        return tmp ;
    }
    JASEL_CXX14_CONSTEXPR money& operator-=(money m) noexcept
    {
        rep -= m.count();
        return *this ;
    }

    friend constexpr money operator*(money m, double x) noexcept
    {
        return money(m.count() * x) ;
    }
    friend constexpr money operator*(double x, money m) noexcept
    {
        return m * x;
    }

    friend constexpr money operator/(money m, double x) noexcept
    {
        return money(m.count() / x) ;
    }
    JASEL_CXX14_CONSTEXPR money& operator/=(double x) noexcept
    {
        rep /= x;
        return *this;
    }
    friend JASEL_CXX14_CONSTEXPR money operator%(money m, int s) noexcept
    {
        using CR = typename std::common_type<Rep, int>::type;
        auto r = CR(m.count()) % s ;
        return money(r) ;
    }
#if defined __clang__
    // gcc doesn't accepts this
    template <class R1, class R2>
    friend constexpr money<Currency, typename std::common_type<R1,R2>::type> operator%(money<Currency, R1> m, money<Currency, R2> n) noexcept
    {
        return money(m.count() % n.count()) ;
    }
#endif
    JASEL_CXX14_CONSTEXPR money& operator%=(int x) noexcept
    {
        rep %= x;
        return *this;
    }

private:
    Rep rep;
};

namespace money_expr
{
template <class M1, class M2, class Op>
struct binary
{
    M1 m1;
    M2 m2;

    // fixme:: shouldn't this requires that the representation is convertible from the common_type?
    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>() {
        return money<C,R>( Op{}(detail::to_currency<C>(m1), detail::to_currency<C>(m2)));
    }
};

template <class M1, class M2>
struct add
{
    M1 m1;
    M2 m2;

    // fixme:: shouldn't this requires that the representation is convertible from the common_type?
    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( detail::to_currency<C>(m1) + detail::to_currency<C>(m2));
    }
};
template <class M1, class M2>
struct substract
{
    M1 m1;
    M2 m2;

    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( detail::to_currency<C>(m1) - detail::to_currency<C>(m2));
    }
};

template <class C, class M1, class M2>
struct divide
{
    M1 m1;
    M2 m2;

    template <class R>
    JASEL_CXX14_CONSTEXPR operator R()
    {
        return  detail::to_currency<C>(m1) / detail::to_currency<C>(m2);
    }
};

template <class M1, class M2>
struct modulo
{
    M1 m1;
    M2 m2;

    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( detail::to_currency<C>(m1) % detail::to_currency<C>(m2));
    }
};

}

template <class C1, class R1, class C2, class R2>
constexpr auto operator+(money<C1,R1> m1, money<C2,R2> m2) noexcept -> money_expr::add<money<C1,R1>, money<C2,R2>>
{
    return money_expr::add<money<C1,R1>, money<C2,R2>>{m1, m2} ;
    //return money_expr::binary<money<C1,R1>, money<C2,R2>, std::plus>{m1, m2} ;
}

template <class C1, class R1, class C2, class R2>
constexpr auto operator-(money<C1,R1> m1, money<C2,R2> m2) noexcept -> money_expr::substract<money<C1,R1>, money<C2,R2>>
{
    return money_expr::substract<money<C1,R1>, money<C2,R2>>{m1, m2} ;
}

template <class OSTREAM, class C, class R>
OSTREAM& operator<<(OSTREAM& os, money<C, R> m)
{
    os<< m.count() << m.currency().symbol();
    return os;
}

using dollars = money<currency::dollar>;
using euros = money<currency::euro>;

#include <boost/detail/lightweight_test.hpp>
#include <iostream>

int main()
{
    {
        dollars d(5);
        BOOST_TEST(long(d.count()) == 5);
    }
    {
        dollars d;
        d = dollars(5);
        BOOST_TEST(long(d.count()) == 5);
    }
    {
        dollars d;
        d = dollars(5);
        BOOST_TEST(d == dollars(5));
    }
    {
        dollars d1(5), d2(10);
        BOOST_TEST(d1+d2 == dollars(15));
    }
    {
        dollars d(5);
        BOOST_TEST(+d  == d);
    }
    {
        dollars d1(5), d2(10);
        BOOST_TEST(d1-d2 == dollars(-5));
    }
    {
        dollars d(5);
        BOOST_TEST(-d  == dollars(-5));
    }
    {
        dollars d(5);
        BOOST_TEST(d * 3  == dollars(15));
    }
    {
        dollars d(5);
        BOOST_TEST(3 * d  == dollars(15));
    }
    {
        dollars d(15);
        BOOST_TEST(d / 3  == dollars(5));
    }
    {
        using idollars = money<currency::dollar, int>;
        idollars d(16);
        BOOST_TEST(d % 3  == idollars(1));
    }
//    {
//        using CR = std::common_type_t<double,int>;
//        double d(16);
//        BOOST_TEST(CR(d) % 3  == 1);
//    }
    {
        dollars d(10);
        euros e(5);
        euros x = d + e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(25));
    }
    {
        euros e1(5);
        dollars d(10);
        euros e2(5);
        euros x = e1 + d + e2;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(30));
    }
    {
        dollars d(10);
        euros e(5);
        e =  d;
        BOOST_TEST(e == euros(20));
    }
    {
        dollars d(10);
        euros e(5);
        euros x = d - e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(15));
    }
    {
        dollars d(10);
        euros e(5);
        euros x = d - e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(15));
    }
    {
        euros e1(5);
        dollars d(10);
        euros e2(5);
        euros x = e1 - d - e2;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(-20));
    }
    return ::boost::report_errors();
}


// CxxSwizzle
// Copyright (c) 2013, Piotr Gwiazdowski <gwiazdorrr+github at gmail.com>

#include <boost/test/unit_test.hpp>
#include <array>
#include <limits>
#include "setup.h"

template <class TVec, class TScalar>
void test_operators(TVec a, TScalar b)
{
    size_t index = 0;

    auto negPredicate = [=](typename TVec::scalar_type x, typename TVec::scalar_type y) -> bool { return are_close(x, -y); };
    auto addPredicate = [=](typename TVec::scalar_type x, typename TVec::scalar_type y) -> bool { return are_close(x, y + b); };
    auto subPredicate = [=](typename TVec::scalar_type x, typename TVec::scalar_type y) -> bool { return are_close(x, y - b); };
    auto mulPredicate = [=](typename TVec::scalar_type x, typename TVec::scalar_type y) -> bool { return are_close(x, y * b); };
    auto divPredicate = [=](typename TVec::scalar_type x, typename TVec::scalar_type y) -> bool { return are_close(x, y / b); };

    BOOST_ASSERT( are_equal(-a, a, negPredicate) );

    BOOST_ASSERT( are_equal<TVec>(a + b, a, addPredicate) );
    BOOST_ASSERT( are_equal<TVec>(b + a, a, addPredicate) );
    BOOST_ASSERT( are_equal<TVec>(TVec(a) += b, a, addPredicate) );

    BOOST_ASSERT( are_equal<TVec>(a - b, a, subPredicate) );
    BOOST_ASSERT( are_equal<TVec>(-(b - a), a, subPredicate) );
    BOOST_ASSERT( are_equal<TVec>(TVec(a) -= b, a, subPredicate) );

    BOOST_ASSERT( are_equal<TVec>(a * b, a, mulPredicate) );
    BOOST_ASSERT( are_equal<TVec>(b * a, a, mulPredicate) );
    BOOST_ASSERT( are_equal<TVec>(TVec(a) *= b, a, mulPredicate) );

    BOOST_ASSERT( are_equal<TVec>(a / b, a, divPredicate) );
    BOOST_ASSERT( are_equal<TVec>(1 / (b / a), a, divPredicate) );
    BOOST_ASSERT( are_equal<TVec>(TVec(a) /= b, a, divPredicate) );
}

template <class TVec>
void test_oprators(TVec a)
{
    test_operators(a, double(1));
    test_operators(a, int(1));
    test_operators(a, float(1));
}

BOOST_AUTO_TEST_SUITE(Construction)

BOOST_AUTO_TEST_CASE(vec1_basic)
{
    // default
    vec1 a;
    BOOST_CHECK(a.x == 0.0f);
    // with a value
    vec1 b(666);
    BOOST_CHECK(b.x == 666.0f);
    // copy
    vec1 c(b);
    BOOST_CHECK(b.x == c.x);
    // from proxy
    vec1 d(c.x);
    BOOST_CHECK(c.x == d.x);
}

BOOST_AUTO_TEST_CASE(vec2_basic)
{
    BOOST_CHECK( are_all_equal( vec2(), 0) );
    BOOST_CHECK( !are_all_equal( vec2(), 666) );
    BOOST_CHECK( are_all_equal( vec2(666), 666) );

    // with a value
    vec2 b(1, 2);
    BOOST_CHECK(b.x == 1 && b.y == 2);
    BOOST_CHECK( !are_all_equal(b, 666) );

    // copy
    vec2 c(b);
    BOOST_CHECK(are_equal(b, c));
    // from proxy (reverse)
    vec2 d(c.yx);
    BOOST_CHECK(are_equal_reverse(c, d));
    c.yx = c;
}

BOOST_AUTO_TEST_CASE(vec3_basic)
{
    BOOST_CHECK( are_all_equal( vec3(), 0) );
    BOOST_CHECK( !are_all_equal( vec3(), 666) );
    BOOST_CHECK( are_all_equal( vec3(666), 666) );

    // with a value
    vec3 b(1, 2, 3);
    BOOST_CHECK(b.x == 1 && b.y == 2 && b.z == 3);
    BOOST_CHECK( !are_all_equal(b, 666) );

    // copy
    vec3 c(b);
    BOOST_CHECK(are_equal(b, c));
    // from proxy (reverse)
    vec3 d(c.zyx);
    BOOST_CHECK(are_equal_reverse(c, d));
}

BOOST_AUTO_TEST_CASE(vec4_basic)
{
    BOOST_CHECK( are_all_equal( vec4(), 0) );
    BOOST_CHECK( !are_all_equal( vec4(), 666) );
    BOOST_CHECK( are_all_equal( vec4(666), 666) );

    // with a value
    vec4 b(1, 2, 3, 4);
    BOOST_CHECK( b.x == 1 && b.y == 2 && b.z == 3 && b.w == 4 );
    BOOST_CHECK( !are_all_equal(b, 666) );

    // copy
    vec4 c(b);
    BOOST_CHECK(are_equal(b, c));
    // from proxy (reverse)
    vec4 d(c.wzyx);
    BOOST_CHECK(are_equal_reverse(c, d));
}

BOOST_AUTO_TEST_CASE(vec_proxy_test)
{
    vec4 v1(1, 2, 3, 4);

    // check if values make sense

    BOOST_CHECK(  are_equal<vec1>(vec1(1), vec1(v1.x)) );
    BOOST_CHECK(  are_equal<vec1>(vec1(4), vec1(v1.w)) );
    BOOST_CHECK( !are_equal<vec1>(vec1(1), vec1(v1.w)) );

    BOOST_CHECK(  are_equal<vec2>(vec2(1, 2), v1.xy) );
    BOOST_CHECK(  are_equal<vec2>(vec2(4, 3), v1.wz) );
    BOOST_CHECK(  are_equal<vec2>(vec2(1, 4), v1.xw) );
    BOOST_CHECK( !are_equal<vec2>(vec2(1, 2), v1.xw) );

    BOOST_CHECK(  are_equal<vec3>(vec3(1, 2, 3), v1.xyz) );
    BOOST_CHECK(  are_equal<vec3>(vec3(4, 3, 2), v1.wzy) );
    BOOST_CHECK(  are_equal<vec3>(vec3(1, 4, 4), v1.xww) );
    BOOST_CHECK( !are_equal<vec3>(vec3(1, 2, 3), v1.xww) );

    BOOST_CHECK(  are_equal<vec4>(vec4(1, 2, 3, 4), v1.xyzw) );
    BOOST_CHECK(  are_equal<vec4>(vec4(4, 3, 2, 1), v1.wzyx) );
    BOOST_CHECK(  are_equal<vec4>(vec4(3, 2, 2, 4), v1.zyyw) );
}

BOOST_AUTO_TEST_CASE(vec_test_operators)
{
    test_oprators( vec1(0) );
    test_oprators( vec2(1, 2) );
    test_oprators( vec3(1, 2, 3) );
    test_oprators( vec4(1, 2, 3, 4) );
}

BOOST_AUTO_TEST_CASE(vec1_test_construct)
{
    // if it compiles then great
    vec1 v1(1);
    vec2 v2(2, 3);
    vec3 v3(4, 5, 6);
    vec4 v4(7, 8, 9, 10);

    BOOST_ASSERT( are_equal(vec1(v1.x), v1)) ;
    BOOST_ASSERT( are_equal(vec1(v2.y), vec1(v2.yxyx)) );
    BOOST_ASSERT( are_equal(vec1(v3.x), vec1(v3)) );
    BOOST_ASSERT( are_equal(vec1(v4.w), vec1(v4.wwww)) );
}

BOOST_AUTO_TEST_CASE(vec2_test_construct)
{
    // if it compiles then great
    vec1 v1(1);
    vec2 v2(2, 3);
    vec3 v3(4, 5, 6);
    vec4 v4(7, 8, 9, 10);

    BOOST_ASSERT( are_equal(vec2(v1.x, v1.x), vec2(v1.xx)) );
    BOOST_ASSERT( are_equal(vec2(v2.x, v2.y), vec2(v2.x, v2.yx)) );
    BOOST_ASSERT( are_equal(vec2(v3.z, v3.y), vec2(v3.zyzz)) );
    BOOST_ASSERT( are_equal(vec2(v4.w, v4.w), vec2(v4.w, v4.wwxx)) );
}

BOOST_AUTO_TEST_CASE(vec3_test_construct)
{
    // if it compiles then great
    vec1 v1(1);
    vec2 v2(2, 3);
    vec3 v3(4, 5, 6);
    vec4 v4(7, 8, 9, 10);

    BOOST_ASSERT( are_equal(vec3(v1.x, v1.x, v1.x), vec3(v1.xxxx)) );
    BOOST_ASSERT( are_equal(vec3(v2.x, v2.y, v2.x), vec3(v2.x, v2.yx)) );
    BOOST_ASSERT( are_equal(vec3(v3.x, v3.y, v3.z), vec3(v3)) );
    BOOST_ASSERT( are_equal(vec3(v4.w, v4.w, v4.w), vec3(v4.w, v4.wwxx)) );
}

BOOST_AUTO_TEST_CASE(vec4_test_construct)
{
    // if it compiles then great
    vec1 v1(1);
    vec2 v2(2, 3);
    vec3 v3(4, 5, 6);
    vec4 v4(7, 8, 9, 10);

    BOOST_ASSERT( are_equal(vec4(v1.x, v1.x, v1.x, v1.x), vec4(v1.xxxx)) );
    BOOST_ASSERT( are_equal(vec4(v2.x, v2.y, v2.x, v2.x), vec4(v2.x, v2.y, v2.x, v2.xyxy)) );
    BOOST_ASSERT( are_equal(vec4(v3.x, v3.y, v3.z, v3.z), vec4(v3, v3.z)) );
    BOOST_ASSERT( are_equal(vec4(v4.w, v4.w, v4.w, v4.x), vec4(v4.w, v4.wwxx)) );
}

BOOST_AUTO_TEST_SUITE_END()