text:
#include <iostream>
#include <cmath>
#include <chrono>
#include <limits>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/core/eigen.hpp>

#include "pose2d.h"
#include "Ransac.hh"
#include "PoseRANSAC.hh"

//#define USE_QUATERNION
#define USE_ROTATION_MATRIX

namespace pose2d
{
/* Matrix coefficients for the non-depth case matrix to be solved by SVD */
   inline double tx_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (R(1, 2) + R(1, 0) * xp1 + R(1, 1) * yp1 - R(2, 2) * yq1 + (-(R(2, 0) * xp1) - R(2, 1) * yp1) * yq1);
      //return (R(1,2) + R(1,0)*xq1 - R(2,2)*yp1 - R(2,0)*xq1*yp1 + R(1,1)*yq1 - R(2,1)*yp1*yq1);
   }

   inline double ty_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//----------------------------------------------------------------------------------------------------
   {
      return (-R(0, 2) - R(0, 0) * xp1 + R(2, 2) * xq1 + R(2, 0) * xp1 * xq1 - R(0, 1) * yp1 + R(2, 1) * xq1 * yp1);
      //return ((-R(0,2) + R(2,2)*xp1 - R(0,0)*xq1 + R(2,0)*xp1*xq1 - R(0,1)*yq1 + R(2,1)*xp1*yq1));
   }

   inline double tz_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (-(R(1, 2) * xq1) - R(1, 0) * xp1 * xq1 - R(1, 1) * xq1 * yp1 +
              (R(0, 2) + R(0, 0) * xp1 + R(0, 1) * yp1) * yq1);
//   return (-(R(1,2)*xp1) - R(1,0)*xp1*xq1 + R(0,2)*yp1 + R(0,0)*xq1*yp1 - R(1,1)*xp1*yq1 + R(0,1)*yp1*yq1);
   }

/* RHS values for depth solution*/
   inline double b0(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double y_1)
   {
      return d_0 *
             (-R(1, 0) * x_0 - R(1, 1) * y_0 - R(1, 2) + R(2, 0) * x_0 * y_1 + R(2, 1) * y_0 * y_1 + R(2, 2) * y_1);
   }

   inline double b1(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double x_1)
   {
      return -d_0 *
             (-R(0, 0) * x_0 - R(0, 1) * y_0 - R(0, 2) + R(2, 0) * x_0 * x_1 + R(2, 1) * x_1 * y_0 + R(2, 2) * x_1);
   }

   inline double b2(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0,
                    const double x_1, const double y_1)
   {
      return d_0 *
             (-R(0, 0) * x_0 * y_1 - R(0, 1) * y_0 * y_1 - R(0, 2) * y_1 + R(1, 0) * x_0 * x_1 + R(1, 1) * x_1 * y_0 +
              R(1, 2) * x_1);
   }

   inline double b0(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQx = Qw * Qx;
      return d * (-Qw2 * y_1 - 2 * QwQx * y_0 * y_1 - 2 * QwQx + 2 * Qw * Qy * x_0 * y_1 + Qx2 * y_1 -
                  2 * Qx * Qz * x_0 * y_1 +
                  Qy2 * y_1 - 2 * Qy * Qz * y_0 * y_1 + 2 * Qy * Qz - Qz2 * y_1 + 2 * x_0 * (Qw * Qz + Qx * Qy) +
                  y_0 * (Qw2 -
                         Qx2 + Qy2 - Qz2));
   }

   inline double b1(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQy = Qw * Qy, QxQz = Qx * Qz;
      return -d *
             (-Qw2 * x_1 - 2 * Qw * Qx * x_1 * y_0 + 2 * QwQy * x_0 * x_1 + 2 * QwQy - 2 * Qw * Qz * y_0 + Qx2 * x_1 +
              2 * Qx * Qy * y_0 - 2 * QxQz * x_0 * x_1 + 2 * QxQz + Qy2 * x_1 - 2 * Qy * Qz * x_1 * y_0 - Qz2 * x_1 +
              x_0 * (Qw2 + Qx2 - Qy2 - Qz2));
   }

   inline double b2(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQz = Qw * Qz, QxQy = Qx * Qy;
      return -d *
             (-Qw2 * x_0 * y_1 - 2 * Qw * Qx * x_1 - 2 * Qw * Qy * y_1 + 2 * QwQz * x_0 * x_1 + 2 * QwQz * y_0 * y_1 -
              Qx2 * x_0 * y_1 + 2 * QxQy * x_0 * x_1 - 2 * QxQy * y_0 * y_1 - 2 * Qx * Qz * y_1 + Qy2 * x_0 * y_1 +
              2 * Qy * Qz * x_1 + Qz2 * x_0 * y_1 + x_1 * y_0 * (Qw2 - Qx2 + Qy2 - Qz2));

   }

   inline Eigen::Vector3d homogeneous(Eigen::Matrix3d A)
   //---------------------------------------------------------------------------------------------------------------------------
   {
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      return V.col(V.cols() - 1);
   }

   inline Eigen::Quaterniond rotation(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                      const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const Eigen::Vector3d& train_g,
             const Eigen::Vector3d query_g, const cv::Mat& intrinsics,
             Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d KI = K.inverse();
      //   std::cout << K << std::endl << KI << std::endl;
      //   if (std::isnan(KI(0,0))) KI = K;

      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
      //   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      Eigen::Matrix3d R = Q.toRotationMatrix();
      size_t m = pts.size();
      Eigen::MatrixXd A(m, 3);
      for (size_t row = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
         A.row(row) << tx_coeff(xt1, yt1, xq1, yq1, R),
               ty_coeff(xt1, yt1, xq1, yq1, R),
               tz_coeff(xt1, yt1, xq1, yq1, R);
      }

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto V = svd.matrixV();
      translation = V.col(V.cols() - 1);
//   assert(mut::check_essential(R, translation));
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, depth, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      size_t m = pts.size();
//   if ( (! std::isnan(depth)) && (m > 6 ) )
//   {
//      pose_ransac(pts, train_g, query_g, depth, K, Q, translation, 3);
//      return;
//   }
      Eigen::Matrix3d KI = K.inverse();
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera

      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::MatrixXd A(3 * m, 3);
      Eigen::VectorXd b(m * 3);
      for (size_t row = 0, ri = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
#ifdef USE_ROTATION_MATRIX
         A.row(ri) << 0, 1, -yq1;
         b(ri++) = b0(R, xt1, yt1, depth, yq1);
         A.row(ri) << -1, 0, xq1;
         b(ri++) = b1(R, xt1, yt1, depth, xq1);
         A.row(ri) << yq1, -xq1, 0;
         b(ri++) = b2(R, xt1, yt1, depth, xq1, yq1);
#endif
#ifdef USE_QUATERNION
         A.row(ri) << 0, -1, yq1;
         b(ri++) = b0(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, yq1, d);
         A.row(ri) <<  1, 0, -xq1;
         b(ri++) = b1(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, d);
         A.row(ri) << -yq1, xq1, 0;
         b(ri++) = b2(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, yq1, d);
#endif
      }
   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
//      Eigen::ColPivHouseholderQR<Eigen::MatrixXd> MQR(A);
//      translation = MQR.solve(b);
   translation = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //-----------------------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (no depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, -1, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;
         ransac->Estimate(pts, &best_model, &summary);
         confidence = summary.confidence;
         if (confidence > 0)
         {
            Q = best_model.rotation;
            translation = best_model.translation;
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DRansacEstimator estimator(K, Q);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel> > results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;
      }
#endif
      return confidence;
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const cv::Mat& intrinsics,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, depth, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const Eigen::Matrix3d& K,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void *RANSAC_params, int samples)
   //---------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (with depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, depth, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;

         if (ransac->Estimate(pts, &best_model, &summary))
         {
            confidence = summary.confidence;
            if (confidence > 0)
            {
               Q = best_model.rotation;
               translation = best_model.translation;
            }
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DDepthRansacEstimator estimator(K, Q, depth);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel>> results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;

//      for (size_t k=0; k<results.size(); k++)
//      {
//         std::pair<double, Grav2DRansacModel> pp = results[k];
//         std::cout << "RANSAC Result " << pp.second.translation.transpose() << " ";
//         std::vector<size_t> inliers = inlier_indices[k];
//         for (size_t inlier : inliers)
//            std::cout << train_img_pts[inlier] << " -> " << query_image_pts[inlier] << " | ";
//         std::cout << std::endl;
//      }
      }
#endif
      return confidence;
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//----------------------------------------------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }
}

ext: cc

max_stars_repo_name: donaldmunro/PlanarTrainer

max_issues_repo_name: donaldmunro/PlanarTrainer

max_forks_repo_name: donaldmunro/PlanarTrainer


text:
// Copyright 2020 Josh Pieper, jjp@pobox.com.
//
// 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 <linux/input.h>

#include <atomic>
#include <fstream>
#include <functional>
#include <thread>

#include <fmt/format.h>

#include <boost/algorithm/string.hpp>
#include <boost/asio.hpp>
#include <boost/signals2.hpp>

#include "mjlib/base/clipp_archive.h"
#include "mjlib/base/clipp.h"
#include "mjlib/base/fail.h"
#include "mjlib/base/time_conversions.h"
#include "mjlib/io/repeating_timer.h"
#include "mjlib/multiplex/stream_asio_client_builder.h"
#include "mjlib/telemetry/file_writer.h"

#include "base/linux_input.h"
#include "base/telemetry_log_registrar.h"
#include "mech/moteus.h"

namespace pl = std::placeholders;

namespace mjmech {
namespace mech {

namespace {
using Value = mjlib::multiplex::Format::Value;

struct Context {
  mjlib::io::SharedStream stream;
  boost::asio::streambuf* streambuf = nullptr;
  std::string* prefix = nullptr;
  std::string* name = nullptr;
  std::string* data = nullptr;
  mjlib::io::ErrorCallback callback;

  auto AtLeast(int size) {
    return [this, size](auto, auto) -> size_t  {
      return std::max<int>(0, size - streambuf->size());
    };
  }

  Context(mjlib::io::SharedStream stream_in,
          boost::asio::streambuf* streambuf_in,
          std::string* prefix_in,
          std::string* name_in,
          std::string* data_in,
          mjlib::io::ErrorCallback callback_in)
      : stream(stream_in),
        streambuf(streambuf_in),
        prefix(prefix_in),
        name(name_in),
        data(data_in),
        callback(std::move(callback_in)) {}
};

void AsyncHandleData(const mjlib::base::error_code& ec,
                     std::shared_ptr<Context> ctx) {
  mjlib::base::FailIf(ec);

  std::istream istr(ctx->streambuf);
  istr.read(&(*ctx->data)[0], ctx->data->size());

  boost::asio::post(
      ctx->stream->get_executor(),
      std::bind(std::move(ctx->callback), mjlib::base::error_code()));
}

void AsyncHandleSize(const mjlib::base::error_code& ec,
                     std::shared_ptr<Context> ctx) {
  mjlib::base::FailIf(ec);

  std::istream istr(ctx->streambuf);

  uint32_t size = 0;
  istr.read(reinterpret_cast<char*>(&size), sizeof(size));
  MJ_ASSERT(istr.gcount() == sizeof(size));

  ctx->data->resize(size);

  // Now read the data.
  boost::asio::async_read(
      *ctx->stream,
      *ctx->streambuf,
      ctx->AtLeast(size),
      std::bind(&AsyncHandleData, pl::_1, ctx));
}

void AsyncReadBinary(std::shared_ptr<Context> ctx);

void AsyncHandleLine(const mjlib::base::error_code& ec,
                     std::shared_ptr<Context> ctx) {
  mjlib::base::FailIf(ec);

  std::istream istr(ctx->streambuf);

  std::string line;
  std::getline(istr, line);
  boost::trim(line);
  if (line.substr(0, 2) == "OK") {
    // We just skip these.
    AsyncReadBinary(ctx);
    return;
  }
  const auto spacepos = line.find(' ');
  if (spacepos == std::string::npos) {
    throw mjlib::base::system_error::einval(
          "Malformed response: " + line);
  }

  *ctx->prefix = line.substr(0, spacepos);
  *ctx->name = line.substr(spacepos + 1);

  boost::asio::async_read(
      *ctx->stream,
      *ctx->streambuf,
      ctx->AtLeast(4),
      std::bind(&AsyncHandleSize, pl::_1, ctx));
}

void AsyncReadBinary(std::shared_ptr<Context> ctx) {
  boost::asio::async_read_until(
      *ctx->stream,
      *ctx->streambuf,
      "\r\n",
      std::bind(AsyncHandleLine, pl::_1, ctx));
}

void AsyncReadBinary(
    mjlib::io::SharedStream stream,
    boost::asio::streambuf* streambuf,
    std::string* prefix,
    std::string* name,
    std::string* data,
    mjlib::io::ErrorCallback cbk) {
  auto ctx = std::make_shared<Context>(
      stream, streambuf, prefix, name, data, std::move(cbk));
  AsyncReadBinary(ctx);
}

struct Options {
  std::string joystick;
  mjlib::multiplex::StreamAsioClientBuilder::Options stream;
  double period_s = 0.05;
  double max_torque_Nm = 1.0;

  std::string load_cell;
  double load_cell_tare_g = 110.0;
  double torque_scale = 0.00980665;

  std::string power_supply;

  std::string log;

  template <typename Archive>
  void Serialize(Archive* a) {
    a->Visit(MJ_NVP(joystick));
    a->Visit(MJ_NVP(stream));
    a->Visit(MJ_NVP(period_s));
    a->Visit(MJ_NVP(max_torque_Nm));
    a->Visit(MJ_NVP(load_cell));
    a->Visit(MJ_NVP(load_cell_tare_g));
    a->Visit(MJ_NVP(torque_scale));
    a->Visit(MJ_NVP(power_supply));
    a->Visit(MJ_NVP(log));
  }
};

class PowerSupply {
 public:
  struct Data {
    double voltage = 0.0;
    double current_A = 0.0;
    double power_W = 0.0;
  };

  using Signal = boost::signals2::signal<void (const Data*)>;

  PowerSupply(const boost::asio::any_io_executor& executor,
              const std::string& path)
      : executor_(executor),
        path_(path) {
    if (!path.empty()) {
      thread_ = std::thread(std::bind(&PowerSupply::Run, this));
    }
  }

  ~PowerSupply() {
    done_.store(true);
    if (thread_.joinable()) {
      thread_.join();
    }
  }

  Signal* signal() { return &signal_; }

  void Run() {
    // We can't use any buffered IO, because the tmc device works on a
    // per-read-write call, so we have to know exactly how many
    // syscalls we are making.
    const int fd = ::open(path_.c_str(), O_RDWR);
    mjlib::base::system_error::throw_if(
        fd < 0, fmt::format("Error opening '{}'", path_));

    auto write = [&](const std::string& str) {
      const int r = ::write(fd, str.data(), str.size());
      mjlib::base::system_error::throw_if(
          r < 0, fmt::format("Error writing"));
    };

    char buf[256] = {};
    auto read = [&]() {
      const int r = ::read(fd, buf, sizeof(buf));
      if (r < 0) { return std::string(); }
      return std::string(buf, r);
    };

    while (!done_.load()) {
      ::usleep(100000);

      Data data;

      write("MEAS:CURR?\n");
      data.current_A = std::stod(read());

      write("MEAS:VOLT?\n");
      data.voltage = std::stod(read());

      data.power_W = data.voltage * data.current_A;

      boost::asio::post(
          executor_,
          [this, data]() {
            this->signal_(&data);
          });
    }

    ::close(fd);
  }

  boost::asio::any_io_executor executor_;
  std::string path_;
  std::thread thread_;
  Signal signal_;

  std::atomic<bool> done_{false};
};

class LoadCell {
 public:
  LoadCell(const boost::asio::any_io_executor& executor,
           const std::string& path) {
    if (!path.empty()) {
      port_.emplace(executor, path);
      port_->set_option(boost::asio::serial_port::baud_rate(115200));
      StartRead();
    }
  }

  void StartRead() {
    boost::asio::async_read_until(
        *port_, streambuf_,
        "\n", [this](auto ec, auto size) {
          this->HandleRead(ec, size);
        });
  }

  void HandleRead(const mjlib::base::error_code& ec, size_t) {
    mjlib::base::FailIf(ec);

    std::istream istr(&streambuf_);
    while (!istr.eof()) {
      std::string line;
      std::getline(istr, line);
      try {
        const double load_g = std::stod(line);
        Emit(load_g);
      } catch (std::invalid_argument&) {
      }
    }
    StartRead();
  }

  void Emit(double load_g) {
    load_signal_(load_g);
  }

  boost::signals2::signal<void (double)>* signal() { return &load_signal_; }

 private:
  std::optional<boost::asio::serial_port> port_;
  boost::asio::streambuf streambuf_;
  boost::signals2::signal<void (double)> load_signal_;
};

class Application {
 public:
  Application(boost::asio::io_context& context,
              const Options& options)
      : executor_(context.get_executor()),
        options_(options),
        timer_(executor_),
        flush_timer_(executor_),
        linux_input_(executor_, options.joystick),
        asio_client_(executor_, options.stream),
        load_cell_(executor_, options.load_cell),
        power_supply_(executor_, options.power_supply),
        file_writer_(),
        log_registrar_(context, &file_writer_) {
    if (!options.log.empty()) {
      file_writer_.Open(options.log);
    }

    load_cell_.signal()->connect(
        std::bind(&Application::HandleLoad, this, pl::_1));
    power_supply_.signal()->connect(
        std::bind(&Application::HandlePowerSupply, this, pl::_1));

    log_registrar_.Register("torque_test", &signal_);
  }

  void Start() {
    asio_client_.AsyncStart(std::bind(&Application::HandleStart, this, pl::_1));
  }

  void HandleStart(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    // Start by sending a stop command to reset any state the servo
    // might have.
    Stop();

    timer_.start(mjlib::base::ConvertSecondsToDuration(options_.period_s),
                 std::bind(&Application::HandleTimer, this, pl::_1));

    debug_stream_ = asio_client_.MakeTunnel(1, 1, {});
    remaining_schemas_ = monitor_schemas_;
    InitSchema();

    StartRead();
    UpdateDisplay();
  }

  void Stop() {
    MJ_ASSERT(outstanding_ == false);

    request_.resize(1);
    auto& item = request_[0];
    item = {};
    item.id = 1;
    item.request.WriteSingle(
        moteus::kMode, Value(static_cast<int8_t>(moteus::Mode::kStopped)));

    outstanding_ = true;
    reply_ = {};
    asio_client_.AsyncTransmit(
        &request_, &reply_,
        std::bind(&Application::HandleStop, this, pl::_1));
  }

  void InitSchema() {
    boost::asio::async_write(
        *debug_stream_,
        boost::asio::buffer("tel stop\r\n"),
        [this](auto ec, auto) {
          mjlib::base::FailIf(ec);
          flush_timer_.expires_from_now(boost::posix_time::milliseconds(200));
          flush_timer_.async_wait([this](auto) { debug_stream_->cancel(); });
          this->FlushRead();
        });
  }

  void FlushRead() {
    debug_stream_->async_read_some(
        boost::asio::buffer(flush_buffer_),
        [this](auto ec, auto size) {
          if (ec == boost::asio::error::operation_aborted) {
            this->StartSchemaRead();
          } else {
            mjlib::base::FailIf(ec);
            FlushRead();
          }
        });
  }

  void StartSchemaRead() {
    if (remaining_schemas_.empty()) {
      // We're all done.  Now just set everything we're monitoring to
      // spew at 10Hz.
      std::ostringstream ostr;
      for (const auto& schema : monitor_schemas_) {
        ostr << fmt::format("tel rate {} 100\n", schema);
      }
      write_buffer_ = ostr.str();
      boost::asio::async_write(
          *debug_stream_,
          boost::asio::buffer(write_buffer_),
          std::bind(&Application::HandleSchemaDone, this, pl::_1));
    } else {
      // Request the next.
      write_buffer_ = fmt::format("tel schema {}\n", remaining_schemas_.back());
      remaining_schemas_.pop_back();
      boost::asio::async_write(
          *debug_stream_,
          boost::asio::buffer(write_buffer_),
          std::bind(&Application::HandleSchemaRequest, this, pl::_1));
    }
  }

  void HandleSchemaRequest(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    AsyncReadBinary(debug_stream_,
                    &debug_streambuf_,
                    &schema_prefix_,
                    &schema_name_,
                    &schema_data_,
                    std::bind(&Application::HandleSchemaResponse, this, pl::_1));
  }

  void HandleSchemaResponse(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    if (schema_prefix_ != "schema") {
      throw mjlib::base::system_error::einval(
          "Unknown response: " + schema_prefix_);
    }
    debug_ids_[schema_name_] = file_writer_.AllocateIdentifier(schema_name_);
    file_writer_.WriteSchema(debug_ids_[schema_name_], schema_data_);
    StartSchemaRead();
  }

  void HandleSchemaDone(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    StartReadDebugData();
  }

  void StartReadDebugData() {
    AsyncReadBinary(debug_stream_,
                    &debug_streambuf_,
                    &schema_prefix_,
                    &schema_name_,
                    &schema_data_,
                    std::bind(&Application::HandleDataRead, this, pl::_1));
  }

  void HandleDataRead(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    if (schema_prefix_ != "emit") {
      throw mjlib::base::system_error::einval("Unknown emit: " + schema_prefix_);
    }

    file_writer_.WriteData({}, debug_ids_[schema_name_], schema_data_);

    StartReadDebugData();
  }

  void HandleStop(const mjlib::base::error_code& ec) {
    outstanding_ = false;
    mjlib::base::FailIf(ec);
  }

  void HandleTimer(const mjlib::base::error_code& ec) {
    if (ec == boost::asio::error::operation_aborted) { return; }
    mjlib::base::FailIf(ec);

    if (outstanding_) { return; }

    outstanding_ = true;

    const float command_Nm = GetCommandTorqueNm();

    request_.resize(1);
    auto& item = request_[0];
    item = {};
    item.id = 1;
    item.request.WriteSingle(
        moteus::kMode, Value(static_cast<int8_t>(moteus::Mode::kPosition)));
    item.request.WriteMultiple(
        moteus::kCommandPosition,
        {Value(0.0f),  // position
              Value(0.0f),  // velocity
              Value(command_Nm),
              Value(0.0f),  // kp_scale
              Value(0.0f),  // kd_scale
              });

    item.request.ReadMultiple(moteus::kMode, 5, moteus::kFloat);
    item.request.ReadMultiple(moteus::kVoltage, 3, moteus::kFloat);

    current_reply_ = {};
    asio_client_.AsyncTransmit(
        &request_, &current_reply_,
        std::bind(&Application::HandleClient, this, pl::_1));
  }

  void StartRead() {
    linux_input_.AsyncRead(
        &event_, std::bind(
            &Application::HandleJoystick, this, pl::_1));
  }

  void HandleJoystick(const mjlib::base::error_code& ec) {
    mjlib::base::FailIf(ec);

    UpdateDisplay();

    StartRead();
  }

  void HandleClient(const mjlib::base::error_code& ec) {
    outstanding_ = false;

    if (ec == boost::asio::error::operation_aborted) { return; }
    mjlib::base::FailIf(ec);

    UpdateDisplay();
  }

  double GetCommandTorqueNm() {
    return linux_input_.abs_info(ABS_Y).scaled() *
        options_.max_torque_Nm;
  }

  void HandleLoad(double load_g) {
    measured_torque_Nm_ = (load_g - options_.load_cell_tare_g) *
                          options_.torque_scale;
    UpdateDisplay();
  }

  void HandlePowerSupply(const PowerSupply::Data* data) {
    power_supply_data_ = *data;
    UpdateDisplay();
  }

  void UpdateDisplay() {
    double command = GetCommandTorqueNm();

    auto get = [&](auto moteus_reg) -> double {
      for (const auto& item : current_reply_) {
        if (item.reg == moteus_reg) {
          return std::get<float>(std::get<Value>(item.value));
        }
      }
      return std::numeric_limits<double>::signaling_NaN();
    };

    Data data;
    data.timestamp = mjlib::io::Now(executor_.context());
    data.command_Nm = -command;
    data.measured.torque_Nm = measured_torque_Nm_;
    data.measured.voltage = power_supply_data_.voltage;
    data.measured.current_A = power_supply_data_.current_A;
    data.measured.power_W = power_supply_data_.power_W;

    data.qdd100.mode = get(moteus::kMode);
    data.qdd100.position_deg = get(moteus::kPosition) * 360.0;
    data.qdd100.velocity_dps = get(moteus::kVelocity) * 360.0;
    data.qdd100.torque_Nm = get(moteus::kTorque);
    data.qdd100.q_current_A = get(moteus::kQCurrent);
    data.qdd100.temperature_C = get(moteus::kTemperature);

    std::cout << fmt::format(
        " cmd={:5.1f}Nm  mt={:5.1f}Nm  mp={:4.1f}W  qdd100[ mode={:4} "
        "pos={:7.1f}deg vel={:6.1f}dps torque={:5.1f}Nm i={:4.1f}A "
        "t={:3.0f}C ]  \r",
        command,
        measured_torque_Nm_,
        power_supply_data_.power_W,
        MapMode(data.qdd100.mode),
        data.qdd100.position_deg,
        data.qdd100.velocity_dps,
        data.qdd100.torque_Nm,
        data.qdd100.q_current_A,
        data.qdd100.temperature_C);
    std::cout.flush();

    signal_(&data);
  }

  std::string MapMode(float value) {
    if (!std::isfinite(value)) { return "unk"; }
    return mode_text_.at(static_cast<int>(value));
  }

  struct Data {
    boost::posix_time::ptime timestamp;
    double command_Nm = 0.0;

    struct Measured {
      double torque_Nm = 0.0;
      double voltage = 0.0;
      double current_A = 0.0;
      double power_W = 0.0;

      template <typename Archive>
      void Serialize(Archive* a) {
        a->Visit(MJ_NVP(torque_Nm));
        a->Visit(MJ_NVP(voltage));
        a->Visit(MJ_NVP(current_A));
        a->Visit(MJ_NVP(power_W));
      }
    };

    Measured measured;

    struct Qdd100 {
      double mode = 0.0;
      double position_deg = 0.0;
      double velocity_dps = 0.0;
      double torque_Nm = 0.0;
      double q_current_A = 0.0;
      double temperature_C = 0.0;

      template <typename Archive>
      void Serialize(Archive* a) {
        a->Visit(MJ_NVP(position_deg));
        a->Visit(MJ_NVP(velocity_dps));
        a->Visit(MJ_NVP(torque_Nm));
        a->Visit(MJ_NVP(q_current_A));
        a->Visit(MJ_NVP(temperature_C));
      }
    };

    Qdd100 qdd100;

    template <typename Archive>
    void Serialize(Archive* a) {
      a->Visit(MJ_NVP(timestamp));
      a->Visit(MJ_NVP(command_Nm));
      a->Visit(MJ_NVP(measured));
      a->Visit(MJ_NVP(qdd100));
    }
  };

  boost::asio::any_io_executor executor_;
  const Options options_;
  mjlib::io::RepeatingTimer timer_;
  mjlib::io::DeadlineTimer flush_timer_;
  base::LinuxInput linux_input_;
  mjlib::multiplex::StreamAsioClientBuilder asio_client_;
  LoadCell load_cell_;
  PowerSupply power_supply_;

  mjlib::telemetry::FileWriter file_writer_;
  base::TelemetryLogRegistrar log_registrar_;

  base::LinuxInput::Event event_;

  using AsioClient = mjlib::multiplex::AsioClient;

  AsioClient::Request request_;
  AsioClient::Reply current_reply_;
  AsioClient::Reply reply_;
  bool outstanding_ = false;

  double measured_torque_Nm_ = 0.0;
  PowerSupply::Data power_supply_data_;

  boost::signals2::signal<void (const Data*)> signal_;

  const std::map<int, std::string> mode_text_ = {
    { 0, "stop" },
    { 1, "flt" },
    { 2, "" },
    { 3, "" },
    { 4, "" },
    { 5, "pwm" },
    { 6, "volt" },
    { 7, "foc" },
    { 8, "vdq" },
    { 9, "cur" },
    { 10, "pos" },
    { 11, "tmt" },
    { 12, "zero" },
  };

  mjlib::io::SharedStream debug_stream_;
  boost::asio::streambuf debug_streambuf_;

  std::string write_buffer_;
  std::map<std::string, uint64_t> debug_ids_;
  std::string schema_prefix_;
  std::string schema_name_;
  std::string schema_data_;

  const std::vector<std::string> monitor_schemas_ = {
    "servo_stats",
    "servo_cmd",
    "servo_control",
  };
  std::vector<std::string> remaining_schemas_;

  char flush_buffer_[256] = {};
};

}

int do_main(int argc, char** argv) {
  Options options;

  auto group = mjlib::base::ClippArchive().Accept(&options).group();
  mjlib::base::ClippParse(argc, argv, group);

  boost::asio::io_context context;
  Application application(context, options);

  application.Start();

  boost::asio::signal_set signals(context, SIGINT, SIGTERM);
  signals.async_wait([&](auto _1, auto _2) {
      context.stop();
    });

  try {
    context.run();
  } catch (std::runtime_error& e) {
    // We catch what errors we can to give cleanup a chance to happen.
    std::cerr << "ERROR: " << e.what() << "\n";
    return 1;
  }
  return 0;
}

}
}

int main(int argc, char** argv) {
  return mjmech::mech::do_main(argc, argv);
}


text:
//
// Copyright 2022 DMetaSoul
//
// 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 <boost/dll/runtime_symbol_info.hpp>
#include <boost/process/system.hpp>
#include <boost/process/search_path.hpp>
#include <serving/test_utils.h>
#include <serving/py_preprocessing_process.h>

using namespace metaspore::serving;

TEST(PyPreprocessingProcessTestSuite, LaunchProcessTest) {
    auto prog_dir = boost::dll::program_location().parent_path();
    auto conf_dir = prog_dir / "testing_preprocessor_conf";
    PyPreprocessingProcess proc;
    proc.set_python_executable(boost::process::search_path("python").string());
    proc.set_virtual_env_dir((prog_dir / "testing_preprocessor_venv").string());
    proc.set_requirement_file((conf_dir / "requirements.txt").string());
    proc.set_service_script_file((prog_dir / "preprocessor_service.py").string());
    proc.set_preprocessor_config_dir(conf_dir.string());
    proc.set_preprocessor_listen_addr("unix://" + (conf_dir / "listen_addr.sock").string());
    absl::Status status = proc.launch();
    int rc = boost::process::system(prog_dir / "testing_preprocessor_venv" / "bin" / "python",
                                    conf_dir / "test_example_preprocessor.py");
    ASSERT_EQ(rc, 0);
    ASSERT_TRUE(status.ok());
}

int main(int argc, char **argv) { return run_all_tests(argc, argv); }

ext: cpp

max_stars_repo_name: meta-soul/MetaSpore

max_issues_repo_name: meta-soul/MetaSpore

max_forks_repo_name: meta-soul/MetaSpore


text:
// Copyright 2020 The Beam Team
//
// 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 "ipfs_imp.h"
#include "utility/logger.h"
#include <boost/filesystem.hpp>

// TODO:IPFS check
// 2021/11/16 20:13:48 failed to sufficiently increase receive buffer size (was: 208 kiB, wanted: 2048 kiB, got: 416 kiB).
// See https://github.com/lucas-clemente/quic-go/wiki/UDP-Receive-Buffer-Size for details.
namespace beam::wallet
{
    IPFSService::Ptr IPFSService::AnyThread_create(HandlerPtr handler)
    {
        return std::make_shared<imp::IPFSService>(std::move(handler));
    }
}

namespace beam::wallet::imp
{
    namespace asio = boost::asio;

    IPFSService::IPFSService(HandlerPtr handler)
        : _handler(std::move(handler))
    {
    }

    IPFSService::~IPFSService()
    {
        assert(!_node);
        assert(!_thread.joinable());
        assert(!_ios_guard);

        if (_thread.joinable()) {
            std::terminate();
        }
    }

    void IPFSService::ServiceThread_start(asio_ipfs::config config)
    {
        std::scoped_lock lock(_mutex);
        if (_thread.joinable())
        {
            throw std::runtime_error("IPFS Service is already running");
        }

        const auto fullPath = boost::filesystem::system_complete(config.repo_root);

        //
        // boost::filesystem::weakly_canonical(fullPath) doesn't throw exception
        // if something goes wrong but SIGABORT, check error and throw manually
        //
        boost::system::error_code ec;
        const auto canonicalPath = boost::filesystem::weakly_canonical(fullPath, ec);
        if (ec.failed())
        {
            throw boost::filesystem::filesystem_error("IPFS service failed to form canonical path", ec);
        }

        config.repo_root = canonicalPath.string();
        LOG_INFO() << "Starting IPFS Service. Repo path is " << config.repo_root;

        //
        // Startup sequence, we run it sync, in main thread. May be need to make async
        // in next versions, but for now it greatly simplifies the flow
        //
        {
            std::string error;
            asio::spawn(_ios, [&](boost::asio::yield_context yield) {
                try
                {
                    if (config.bootstrap.empty())
                    {
                        #if defined(BEAM_TESTNET)
                            #error ("Define Testnet IPFS bootstrap")
                        #elif defined(BEAM_MAINNET)
                            #error ("Define Mainnet IPFS bootstrap")
                        #else
                            config.bootstrap.emplace_back("/ip4/3.19.141.112/tcp/38041/p2p/12D3KooWFrigFK9gVvCr7YDNNAAxDxmeyLDtR1tYvHcaXxuCcKpt");
                        #endif
                    }

                    _node = asio_ipfs::node::build(_ios, config, std::move(yield));

                    // TODO:IPFS ensure if connected
                    // TODO:IPFS lower connect timeout
                    assert(_node);
                }
                catch (const boost::system::system_error &err)
                {
                    error = err2str(err);
                }
            });
            _ios.run();

            if (!error.empty())
            {
                assert(false);
                throw std::runtime_error(error);
            }

            if (!_node)
            {
                assert(false);
                throw std::runtime_error("_node is somewhy empty");
            }

            // since it has been running need to reset context
            _ios.reset();
        }

        //
        // Here we know that everything is OK, and we're ready for a real
        // threaded startup. Save data & spawn an infinitely running thread
        //
        _myid = _node->id();
        _ios_guard = std::make_unique<IOSGuard>(_ios.get_executor());
        _thread = MyThread([this, repo = config.repo_root]()
        {
            _ios.run();
        });
    }

    void IPFSService::ServiceThread_stop()
    {
        std::scoped_lock lock(_mutex);
        if (!_thread.joinable())
        {
            assert(false);
            throw std::runtime_error("IPFS service thread already stopped");
        }

        LOG_INFO() << "Stopping IPFS Service...";
        _node.reset();
        _ios_guard->reset();
        _ios_guard.reset();

        assert(_thread.joinable());
        _thread.join();
        _ios.reset();
        LOG_INFO() << "IPFS Services stopped.";
    }

    void IPFSService::AnyThread_add(std::vector<uint8_t>&& data, std::function<void (std::string&&)>&& res, Err&& err)
    {
        if (data.empty())
        {
            AnyThreaad_retErr(std::move(err), "Empty data buffer cannot be added to IPFS");
            return;
        }

        call_ipfs(0, std::move(res), std::move(err),[this, data = std::move(data)]
        (boost::asio::yield_context yield, std::function<void()>& cancel) -> auto
        {
            return _node->add(&data[0], data.size(), cancel, std::move(yield));
        });
    }

    void IPFSService::AnyThread_get(const std::string& hash, uint32_t timeout, std::function<void (std::vector<uint8_t>&&)>&& res, Err&& err)
    {
        if (hash.empty())
        {
            AnyThreaad_retErr(std::move(err), "Cannot get data via an empty hash");
            return;
        }

        call_ipfs(timeout, std::move(res), std::move(err), [this, hash]
        (boost::asio::yield_context yield, std::function<void()>& cancel) -> auto
        {
            return _node->cat(hash, cancel, std::move(yield));
        });
    }

    void IPFSService::AnyThread_pin(const std::string& hash, uint32_t timeout, std::function<void ()>&& res, Err&& err)
    {
        if (hash.empty())
        {
            AnyThreaad_retErr(std::move(err), "Cannot get data via an empty hash");
            return;
        }

        call_ipfs(timeout, std::move(res), std::move(err), [this, hash]
        (boost::asio::yield_context yield, std::function<void()>& cancel) -> JustVoid
        {
            _node->pin(hash, cancel, std::move(yield));
            return JustVoid{};
        });
    }

    void IPFSService::AnyThread_unpin(const std::string& hash, uint32_t timeout, std::function<void ()>&& res, Err&& err)
    {
        if (hash.empty())
        {
            AnyThreaad_retErr(std::move(err), "Cannot unpin an empty hash");
            return;
        }

        call_ipfs(timeout, std::move(res), std::move(err), [this, hash]
        (boost::asio::yield_context yield, std::function<void()>& cancel) -> JustVoid
        {
            _node->unpin(hash, cancel, std::move(yield));
            return JustVoid{};
        });
    }

    void IPFSService::AnyThread_gc(uint32_t timeout, std::function<void ()>&& res, Err&& err)
    {
        call_ipfs(timeout, std::move(res), std::move(err), [this]
        (boost::asio::yield_context yield, std::function<void()>& cancel) -> JustVoid
        {
            _node->gc(cancel, std::move(yield));
            return JustVoid{};
        });
    }

    void IPFSService::AnyThreaad_retToClient(std::function<void()>&& what)
    {
        _handler->pushToClient(std::move(what));
    }
}

max_stars_repo_name: iKapitonau/beam

max_issues_repo_name: iKapitonau/beam

max_forks_repo_name: iKapitonau/beam


text:
#ifndef __C_AUTOM8_MOCHAD_DEVICE_SYSTEM_HPP__
#define __C_AUTOM8_MOCHAD_DEVICE_SYSTEM_HPP__

#include <boost/shared_ptr.hpp>

#include <autom8/util/signal_handler.hpp>
#include <autom8/device/x10/x10_device_system.hpp>
#include <autom8/device/x10/x10_device.hpp>
#include <autom8/device/device_model.hpp>
#include <autom8/device/x10/mochad/mochad_controller.hpp>

namespace autom8 {
    class mochad_device_system: public x10_device_system, public signal_handler {
    public:
        mochad_device_system();
        virtual ~mochad_device_system();

        virtual std::string description() { return "mochad (cm11a/cm15a/cm19)"; }
        virtual device_model& model();
        virtual bool send_device_message(command_type message_type, const char* message_params);
        virtual std::string controller_type() const { return "mochad"; }
        virtual void requery_device_status(const std::string& address);

        virtual void on_message_received(std::string);

    private:
        void on_device_removed(database_id id);
        void on_device_updated(database_id id);

        device_list devices_;
        mochad_controller controller_;
        device_model_ptr model_;
        device_factory_ptr factory_;
        std::string last_house_unit_;
    };
}

#endif

ext: hpp


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

#ifdef WITH_DEVICE_TREZOR_WEBUSB
#include <libusb.h>
#endif

#include <algorithm>
#include <functional>
#include <boost/endian/conversion.hpp>
#include <boost/asio/io_service.hpp>
#include <boost/asio/ip/udp.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <boost/format.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include "common/apply_permutation.h"
#include "transport.hpp"
#include "messages/messages-common.pb.h"

#undef MKEcoin_DEFAULT_LOG_CATEGORY
#define MKEcoin_DEFAULT_LOG_CATEGORY "device.trezor.transport"

using namespace std;
using json = rapidjson::Document;

namespace hw{
namespace trezor{

  bool t_serialize(const std::string & in, std::string & out){
    out = in;
    return true;
  }

  bool t_serialize(const epee::wipeable_string & in, std::string & out){
    out.assign(in.data(), in.size());
    return true;
  }

  bool t_serialize(const json_val & in, std::string & out){
    rapidjson::StringBuffer sb;
    rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
    in.Accept(writer);
    out = sb.GetString();
    return true;
  }

  std::string t_serialize(const json_val & in){
    std::string ret;
    t_serialize(in, ret);
    return ret;
  }

  bool t_deserialize(const std::string & in, std::string & out){
    out = in;
    return true;
  }

  bool t_deserialize(std::string & in, epee::wipeable_string & out){
    out = epee::wipeable_string(in);
    return true;
  }

  bool t_deserialize(const std::string & in, json & out){
    if (out.Parse(in.c_str()).HasParseError()) {
      throw exc::CommunicationException("JSON parse error");
    }
    return true;
  }

  static std::string json_get_string(const rapidjson::Value & in){
    return std::string(in.GetString());
  }

  uint64_t pack_version(uint32_t major, uint32_t minor, uint32_t patch)
  {
    // packing (major, minor, patch) to 64 B: 16 B | 24 B | 24 B
    const unsigned bits_1 = 16;
    const unsigned bits_2 = 24;
    const uint32_t mask_1 = (1 << bits_1) - 1;
    const uint32_t mask_2 = (1 << bits_2) - 1;
    CHECK_AND_ASSERT_THROW_MES(major <= mask_1 && minor <= mask_2 && patch <= mask_2, "Version numbers overflow packing scheme");
    return patch | (((uint64_t)minor) << bits_2) | (((uint64_t)major) << (bits_1 + bits_2));
  }

  typedef struct {
    uint16_t trezor_type;
    uint16_t id_vendor;
    uint16_t id_product;
  } trezor_usb_desc_t;

  static trezor_usb_desc_t TREZOR_DESC_T1 = {1, 0x534C, 0x0001};
  static trezor_usb_desc_t TREZOR_DESC_T2 = {2, 0x1209, 0x53C1};
  static trezor_usb_desc_t TREZOR_DESC_T2_BL = {3, 0x1209, 0x53C0};

  static trezor_usb_desc_t TREZOR_DESCS[] = {
      TREZOR_DESC_T1,
      TREZOR_DESC_T2,
      TREZOR_DESC_T2_BL,
  };

  static size_t TREZOR_DESCS_LEN = sizeof(TREZOR_DESCS)/sizeof(TREZOR_DESCS[0]);

  static ssize_t get_device_idx(uint16_t id_vendor, uint16_t id_product){
    for(size_t i = 0; i < TREZOR_DESCS_LEN; ++i){
      if (TREZOR_DESCS[i].id_vendor == id_vendor && TREZOR_DESCS[i].id_product == id_product){
        return i;
      }
    }

    return -1;
  }

  static bool is_device_supported(ssize_t device_idx){
    CHECK_AND_ASSERT_THROW_MES(device_idx < (ssize_t)TREZOR_DESCS_LEN, "Device desc idx too big");
    if (device_idx < 0){
      return false;
    }

#ifdef TREZOR_1_SUPPORTED
    return true;
#else
    return TREZOR_DESCS[device_idx].trezor_type != 1;
#endif
  }

  //
  // Helpers
  //

#define PROTO_HEADER_SIZE 6

  static size_t message_size(const google::protobuf::Message &req){
#if GOOGLE_PROTOBUF_VERSION < 3006001
    return size_t(req.ByteSize());
#else
    return req.ByteSizeLong();
#endif
  }

  static size_t serialize_message_buffer_size(size_t msg_size) {
    return PROTO_HEADER_SIZE + msg_size;  // tag 2B + len 4B
  }

  static void serialize_message_header(void * buff, uint16_t tag, uint32_t len){
    uint16_t wire_tag = boost::endian::native_to_big(static_cast<uint16_t>(tag));
    uint32_t wire_len = boost::endian::native_to_big(static_cast<uint32_t>(len));
    memcpy(buff, (void *) &wire_tag, 2);
    memcpy((uint8_t*)buff + 2, (void *) &wire_len, 4);
  }

  static void deserialize_message_header(const void * buff, uint16_t & tag, uint32_t & len){
    uint16_t wire_tag;
    uint32_t wire_len;
    memcpy(&wire_tag, buff, 2);
    memcpy(&wire_len, (uint8_t*)buff + 2, 4);

    tag = boost::endian::big_to_native(wire_tag);
    len = boost::endian::big_to_native(wire_len);
  }

  static void serialize_message(const google::protobuf::Message &req, size_t msg_size, uint8_t * buff, size_t buff_size) {
    auto msg_wire_num = MessageMapper::get_message_wire_number(req);
    const auto req_buffer_size = serialize_message_buffer_size(msg_size);
    if (req_buffer_size > buff_size){
      throw std::invalid_argument("Buffer too small");
    }

    serialize_message_header(buff, msg_wire_num, msg_size);
    if (!req.SerializeToArray(buff + 6, msg_size)){
      throw exc::EncodingException("Message serialization error");
    }
  }

  //
  // Communication protocol
  //

#define REPLEN 64

  void ProtocolV1::write(Transport & transport, const google::protobuf::Message & req){
    const auto msg_size = message_size(req);
    const auto buff_size = serialize_message_buffer_size(msg_size) + 2;

    epee::wipeable_string req_buff;
    epee::wipeable_string chunk_buff;

    req_buff.resize(buff_size);
    chunk_buff.resize(REPLEN);

    uint8_t * req_buff_raw = reinterpret_cast<uint8_t *>(req_buff.data());
    uint8_t * chunk_buff_raw = reinterpret_cast<uint8_t *>(chunk_buff.data());

    req_buff_raw[0] = '#';
    req_buff_raw[1] = '#';

    serialize_message(req, msg_size, req_buff_raw + 2, buff_size - 2);

    size_t offset = 0;

    // Chunk by chunk upload
    while(offset < buff_size){
      auto to_copy = std::min((size_t)(buff_size - offset), (size_t)(REPLEN - 1));

      chunk_buff_raw[0] = '?';
      memcpy(chunk_buff_raw + 1, req_buff_raw + offset, to_copy);

      // Pad with zeros
      if (to_copy < REPLEN - 1){
        memset(chunk_buff_raw + 1 + to_copy, 0, REPLEN - 1 - to_copy);
      }

      transport.write_chunk(chunk_buff_raw, REPLEN);
      offset += REPLEN - 1;
    }
  }

  void ProtocolV1::read(Transport & transport, std::shared_ptr<google::protobuf::Message> & msg, messages::MessageType * msg_type){
    epee::wipeable_string chunk_buff;
    chunk_buff.resize(REPLEN);
    char * chunk_buff_raw = chunk_buff.data();

    // Initial chunk read
    size_t nread = transport.read_chunk(chunk_buff_raw, REPLEN);
    if (nread != REPLEN){
      throw exc::CommunicationException("Read chunk has invalid size");
    }

    if (memcmp(chunk_buff_raw, "?##", 3) != 0){
      throw exc::CommunicationException("Malformed chunk");
    }

    uint16_t tag;
    uint32_t len;
    nread -= 3 + 6;
    deserialize_message_header(chunk_buff_raw + 3, tag, len);

    epee::wipeable_string data_acc(chunk_buff_raw + 3 + 6, nread);
    data_acc.reserve(len);

    while(nread < len){
      const size_t cur = transport.read_chunk(chunk_buff_raw, REPLEN);
      if (chunk_buff_raw[0] != '?'){
        throw exc::CommunicationException("Chunk malformed");
      }

      data_acc.append(chunk_buff_raw + 1, cur - 1);
      nread += cur - 1;
    }

    if (msg_type){
      *msg_type = static_cast<messages::MessageType>(tag);
    }

    if (nread < len){
      throw exc::CommunicationException("Response incomplete");
    }

    std::shared_ptr<google::protobuf::Message> msg_wrap(MessageMapper::get_message(tag));
    if (!msg_wrap->ParseFromArray(data_acc.data(), len)){
      throw exc::CommunicationException("Message could not be parsed");
    }

    msg = msg_wrap;
  }

  static void assert_port_number(uint32_t port)
  {
    CHECK_AND_ASSERT_THROW_MES(port >= 1024 && port < 65535, "Invalid port number: " << port);
  }

  Transport::Transport(): m_open_counter(0) {

  }

  bool Transport::pre_open(){
    if (m_open_counter > 0){
      MTRACE("Already opened, count: " << m_open_counter);
      m_open_counter += 1;
      return false;

    } else if (m_open_counter < 0){
      MTRACE("Negative open value: " << m_open_counter);

    }

    // Caller should set m_open_counter to 1 after open
    m_open_counter = 0;
    return true;
  }

  bool Transport::pre_close(){
    m_open_counter -= 1;

    if (m_open_counter < 0){
      MDEBUG("Already closed. Counter " << m_open_counter);

    } else if (m_open_counter == 0) {
      return true;

    }

    return false;
  }

  //
  // Bridge transport
  //

  const char * BridgeTransport::PATH_PREFIX = "bridge:";

  BridgeTransport::BridgeTransport(
        boost::optional<std::string> device_path,
        boost::optional<std::string> bridge_host):
    m_device_path(device_path),
    m_bridge_host(bridge_host ? bridge_host.get() : DEFAULT_BRIDGE),
    m_response(boost::none),
    m_session(boost::none),
    m_device_info(boost::none)
    {
      const char *env_bridge_port = nullptr;
      if (!bridge_host && (env_bridge_port = getenv("TREZOR_BRIDGE_PORT")) != nullptr)
      {
        uint16_t bridge_port;
        CHECK_AND_ASSERT_THROW_MES(epee::string_tools::get_xtype_from_string(bridge_port, env_bridge_port), "Invalid bridge port: " << env_bridge_port);
        assert_port_number(bridge_port);

        m_bridge_host = std::string("127.0.0.1:") + boost::lexical_cast<std::string>(env_bridge_port);
        MDEBUG("Bridge host: " << m_bridge_host);
      }

      m_http_client.set_server(m_bridge_host, boost::none, epee::net_utils::ssl_support_t::e_ssl_support_disabled);
    }

  std::string BridgeTransport::get_path() const {
    if (!m_device_path){
      return "";
    }

    std::string path(PATH_PREFIX);
    return path + m_device_path.get();
  }

  void BridgeTransport::enumerate(t_transport_vect & res) {
    json bridge_res;
    std::string req;

    bool req_status = invoke_bridge_http("/enumerate", req, bridge_res, m_http_client);
    if (!req_status){
      throw exc::CommunicationException("Bridge enumeration failed");
    }

    for(rapidjson::Value::ConstValueIterator itr = bridge_res.Begin(); itr != bridge_res.End(); ++itr){
      auto element = itr->GetObject();
      auto t = std::make_shared<BridgeTransport>(boost::make_optional(json_get_string(element["path"])));

      auto itr_vendor = element.FindMember("vendor");
      auto itr_product = element.FindMember("product");
      if (itr_vendor != element.MemberEnd() && itr_product != element.MemberEnd()
        && itr_vendor->value.IsNumber() && itr_product->value.IsNumber()){
        try {
          const auto id_vendor = (uint16_t) itr_vendor->value.GetUint64();
          const auto id_product = (uint16_t) itr_product->value.GetUint64();
          const auto device_idx = get_device_idx(id_vendor, id_product);
          if (!is_device_supported(device_idx)){
            MDEBUG("Device with idx " << device_idx << " is not supported. Vendor: " << id_vendor << ", product: " << id_product);
            continue;
          }
        } catch(const std::exception &e){
          MERROR("Could not detect vendor & product: " << e.what());
        }
      }

      t->m_device_info.emplace();
      t->m_device_info->CopyFrom(*itr, t->m_device_info->GetAllocator());
      res.push_back(t);
    }
  }

  void BridgeTransport::open() {
    if (!pre_open()){
      return;
    }

    if (!m_device_path){
      throw exc::CommunicationException("Coud not open, empty device path");
    }

    std::string uri = "/acquire/" + m_device_path.get() + "/null";
    std::string req;
    json bridge_res;
    bool req_status = invoke_bridge_http(uri, req, bridge_res, m_http_client);
    if (!req_status){
      throw exc::CommunicationException("Failed to acquire device");
    }

    m_session = boost::make_optional(json_get_string(bridge_res["session"]));
    m_open_counter = 1;
  }

  void BridgeTransport::close() {
    if (!pre_close()){
      return;
    }

    MTRACE("Closing Trezor:BridgeTransport");
    if (!m_device_path || !m_session){
      throw exc::CommunicationException("Device not open");
    }

    std::string uri = "/release/" + m_session.get();
    std::string req;
    json bridge_res;
    bool req_status = invoke_bridge_http(uri, req, bridge_res, m_http_client);
    if (!req_status){
      throw exc::CommunicationException("Failed to release device");
    }

    m_session = boost::none;
  }

  void BridgeTransport::write(const google::protobuf::Message &req) {
    m_response = boost::none;

    const auto msg_size = message_size(req);
    const auto buff_size = serialize_message_buffer_size(msg_size);
    epee::wipeable_string req_buff;
    req_buff.resize(buff_size);

    uint8_t * req_buff_raw = reinterpret_cast<uint8_t *>(req_buff.data());

    serialize_message(req, msg_size, req_buff_raw, buff_size);

    std::string uri = "/call/" + m_session.get();
    epee::wipeable_string res_hex;
    epee::wipeable_string req_hex = epee::to_hex::wipeable_string(epee::span<const std::uint8_t>(req_buff_raw, buff_size));

    bool req_status = invoke_bridge_http(uri, req_hex, res_hex, m_http_client);
    if (!req_status){
      throw exc::CommunicationException("Call method failed");
    }

    m_response = res_hex;
  }

  void BridgeTransport::read(std::shared_ptr<google::protobuf::Message> & msg, messages::MessageType * msg_type) {
    if (!m_response){
      throw exc::CommunicationException("Could not read, no response stored");
    }

    boost::optional<epee::wipeable_string> bin_data = m_response->parse_hexstr();
    if (!bin_data){
      throw exc::CommunicationException("Response is not well hexcoded");
    }

    uint16_t msg_tag;
    uint32_t msg_len;
    deserialize_message_header(bin_data->data(), msg_tag, msg_len);
    if (bin_data->size() != msg_len + 6){
      throw exc::CommunicationException("Response is not well hexcoded");
    }

    if (msg_type){
      *msg_type = static_cast<messages::MessageType>(msg_tag);
    }

    std::shared_ptr<google::protobuf::Message> msg_wrap(MessageMapper::get_message(msg_tag));
    if (!msg_wrap->ParseFromArray(bin_data->data() + 6, msg_len)){
      throw exc::EncodingException("Response is not well hexcoded");
    }
    msg = msg_wrap;
  }

  const boost::optional<json> & BridgeTransport::device_info() const {
    return m_device_info;
  }

  std::ostream& BridgeTransport::dump(std::ostream& o) const {
    return o << "BridgeTransport<path=" << (m_device_path ? get_path() : "None")
             << ", info=" << (m_device_info ? t_serialize(m_device_info.get()) : "None")
             << ", session=" << (m_session ? m_session.get() : "None")
             << ">";
  }

  //
  // UdpTransport
  //
  const char * UdpTransport::PATH_PREFIX = "udp:";
  const char * UdpTransport::DEFAULT_HOST = "127.0.0.1";
  const int UdpTransport::DEFAULT_PORT = 21324;

  static void parse_udp_path(std::string &host, int &port, std::string path)
  {
    if (boost::starts_with(path, UdpTransport::PATH_PREFIX))
    {
      path = path.substr(strlen(UdpTransport::PATH_PREFIX));
    }

    auto delim = path.find(':');
    if (delim == std::string::npos) {
      host = path;
    } else {
      host = path.substr(0, delim);
      port = std::stoi(path.substr(delim + 1));
    }
  }

  UdpTransport::UdpTransport(boost::optional<std::string> device_path,
                             boost::optional<std::shared_ptr<Protocol>> proto) :
      m_io_service(), m_deadline(m_io_service)
  {
    m_device_host = DEFAULT_HOST;
    m_device_port = DEFAULT_PORT;
    const char *env_trezor_path = nullptr;

    if (device_path) {
      parse_udp_path(m_device_host, m_device_port, device_path.get());
    } else if ((env_trezor_path = getenv("TREZOR_PATH")) != nullptr && boost::starts_with(env_trezor_path, UdpTransport::PATH_PREFIX)){
      parse_udp_path(m_device_host, m_device_port, std::string(env_trezor_path));
      MDEBUG("Applied TREZOR_PATH: " << m_device_host << ":" << m_device_port);
    } else {
      m_device_host = DEFAULT_HOST;
    }

    assert_port_number((uint32_t)m_device_port);
    if (m_device_host != "localhost" && m_device_host != DEFAULT_HOST){
      throw std::invalid_argument("Local endpoint allowed only");
    }

    m_proto = proto ? proto.get() : std::make_shared<ProtocolV1>();
  }

  std::string UdpTransport::get_path() const {
    std::string path(PATH_PREFIX);
    return path + m_device_host + ":" + std::to_string(m_device_port);
  }

  void UdpTransport::require_socket(){
    if (!m_socket){
      throw exc::NotConnectedException("Socket not connected");
    }
  }

  bool UdpTransport::ping(){
    return ping_int();
  }

  bool UdpTransport::ping_int(boost::posix_time::time_duration timeout){
    require_socket();
    try {
      std::string req = "PINGPING";
      char res[8];

      m_socket->send_to(boost::asio::buffer(req.c_str(), req.size()), m_endpoint);
      receive(res, 8, nullptr, false, timeout);

      return memcmp(res, "PONGPONG", 8) == 0;

    } catch(...){
      return false;
    }
  }

  void UdpTransport::enumerate(t_transport_vect & res) {
    std::shared_ptr<UdpTransport> t = std::make_shared<UdpTransport>();
    bool t_works = false;

    try{
      t->open();
      t_works = t->ping();
    } catch(...) {

    }
    t->close();
    if (t_works){
      res.push_back(t);
    }
  }

  void UdpTransport::open() {
    if (!pre_open()){
      return;
    }

    udp::resolver resolver(m_io_service);
    udp::resolver::query query(udp::v4(), m_device_host, std::to_string(m_device_port));
    m_endpoint = *resolver.resolve(query);

    m_socket.reset(new udp::socket(m_io_service));
    m_socket->open(udp::v4());

    m_deadline.expires_at(boost::posix_time::pos_infin);
    check_deadline();

    m_proto->session_begin(*this);
    m_open_counter = 1;
  }

  void UdpTransport::close() {
    if (!pre_close()){
      return;
    }

    MTRACE("Closing Trezor:UdpTransport");
    if (!m_socket) {
      throw exc::CommunicationException("Socket is already closed");
    }

    m_proto->session_end(*this);
    m_socket->close();
    m_socket = nullptr;
  }

  std::shared_ptr<Transport> UdpTransport::find_debug() {
#ifdef WITH_TREZOR_DEBUGGING
    std::shared_ptr<UdpTransport> t = std::make_shared<UdpTransport>();
    t->m_proto = std::make_shared<ProtocolV1>();
    t->m_device_host = m_device_host;
    t->m_device_port = m_device_port + 1;
    return t;
#else
    MINFO("Debug link is disabled in production");
    return nullptr;
#endif
  }

  void UdpTransport::write_chunk(const void * buff, size_t size){
    require_socket();

    if (size != 64){
      throw exc::CommunicationException("Invalid chunk size");
    }

    auto written = m_socket->send_to(boost::asio::buffer(buff, size), m_endpoint);
    if (size != written){
      throw exc::CommunicationException("Could not send the whole chunk");
    }
  }

  size_t UdpTransport::read_chunk(void * buff, size_t size){
    require_socket();
    if (size < 64){
      throw std::invalid_argument("Buffer too small");
    }

    ssize_t len;
    while(true) {
      try {
        boost::system::error_code ec;
        len = receive(buff, size, &ec, true);
        if (ec == boost::asio::error::operation_aborted) {
          continue;
        } else if (ec) {
          throw exc::CommunicationException(std::string("Comm error: ") + ec.message());
        }

        if (len != 64) {
          throw exc::CommunicationException("Invalid chunk size");
        }

        break;

      } catch(exc::CommunicationException const& e){
        throw;
      } catch(std::exception const& e){
        MWARNING("Error reading chunk, reason: " << e.what());
        throw exc::CommunicationException(std::string("Chunk read error: ") + std::string(e.what()));
      }
    }

    return static_cast<size_t>(len);
  }

  ssize_t UdpTransport::receive(void * buff, size_t size, boost::system::error_code * error_code, bool no_throw, boost::posix_time::time_duration timeout){
    boost::system::error_code ec;
    boost::asio::mutable_buffer buffer = boost::asio::buffer(buff, size);

    require_socket();

    // Set a deadline for the asynchronous operation.
    m_deadline.expires_from_now(timeout);

    // Set up the variables that receive the result of the asynchronous
    // operation. The error code is set to would_block to signal that the
    // operation is incomplete. Asio guarantees that its asynchronous
    // operations will never fail with would_block, so any other value in
    // ec indicates completion.
    ec = boost::asio::error::would_block;
    std::size_t length = 0;

    // Start the asynchronous operation itself. The handle_receive function
    // used as a callback will update the ec and length variables.
    m_socket->async_receive_from(boost::asio::buffer(buffer), m_endpoint,
                                 std::bind(&UdpTransport::handle_receive, std::placeholders::_1, std::placeholders::_2, &ec, &length));

    // Block until the asynchronous operation has completed.
    do {
      m_io_service.run_one();
    }
    while (ec == boost::asio::error::would_block);

    if (error_code){
      *error_code = ec;
    }

    if (no_throw){
      return length;
    }

    // Operation result
    if (ec == boost::asio::error::operation_aborted){
      throw exc::TimeoutException();

    } else if (ec) {
      MWARNING("Reading from UDP socket failed: " << ec.message());
      throw exc::CommunicationException();

    }

    return length;
  }

  void UdpTransport::write(const google::protobuf::Message &req) {
    m_proto->write(*this, req);
  }

  void UdpTransport::read(std::shared_ptr<google::protobuf::Message> & msg, messages::MessageType * msg_type) {
    m_proto->read(*this, msg, msg_type);
  }

  void UdpTransport::check_deadline(){
    if (!m_socket){
      return;  // no active socket.
    }

    // Check whether the deadline has passed. We compare the deadline against
    // the current time since a new asynchronous operation may have moved the
    // deadline before this actor had a chance to run.
    if (m_deadline.expires_at() <= boost::asio::deadline_timer::traits_type::now())
    {
      // The deadline has passed. The outstanding asynchronous operation needs
      // to be cancelled so that the blocked receive() function will return.
      //
      // Please note that cancel() has portability issues on some versions of
      // Microsoft Windows, and it may be necessary to use close() instead.
      // Consult the documentation for cancel() for further information.
      m_socket->cancel();

      // There is no longer an active deadline. The expiry is set to positive
      // infinity so that the actor takes no action until a new deadline is set.
      m_deadline.expires_at(boost::posix_time::pos_infin);
    }

    // Put the actor back to sleep.
    m_deadline.async_wait(boost::bind(&UdpTransport::check_deadline, this));
  }

  void UdpTransport::handle_receive(const boost::system::error_code &ec, std::size_t length,
                                    boost::system::error_code *out_ec, std::size_t *out_length) {
    *out_ec = ec;
    *out_length = length;
  }

  std::ostream& UdpTransport::dump(std::ostream& o) const {
    return o << "UdpTransport<path=" << get_path()
             << ", socket_alive=" << (m_socket ? "true" : "false")
             << ">";
  }

#ifdef WITH_DEVICE_TREZOR_WEBUSB

  static bool is_trezor1(libusb_device_descriptor * info){
    return info->idVendor == TREZOR_DESC_T1.id_vendor && info->idProduct == TREZOR_DESC_T1.id_product;
  }

  static bool is_trezor2(libusb_device_descriptor * info){
    return info->idVendor == TREZOR_DESC_T2.id_vendor && info->idProduct == TREZOR_DESC_T2.id_product;
  }

  static bool is_trezor2_bl(libusb_device_descriptor * info){
    return info->idVendor == TREZOR_DESC_T2_BL.id_vendor && info->idProduct == TREZOR_DESC_T2_BL.id_product;
  }

  static ssize_t get_trezor_dev_id(libusb_device_descriptor *info){
    CHECK_AND_ASSERT_THROW_MES(info, "Empty device descriptor");
    return get_device_idx(info->idVendor, info->idProduct);
  }

  static void set_libusb_log(libusb_context *ctx){
    CHECK_AND_ASSERT_THROW_MES(ctx, "Null libusb context");

    // http://libusb.sourceforge.net/api-1.0/group__libusb__lib.html
#if defined(LIBUSB_API_VERSION) && (LIBUSB_API_VERSION >= 0x01000106)
#  define TREZOR_LIBUSB_SET_DEBUG(ctx, level) libusb_set_option(ctx,  LIBUSB_OPTION_LOG_LEVEL, level)
#else
#  define TREZOR_LIBUSB_SET_DEBUG(ctx, level) libusb_set_debug(ctx, level)
#endif

    if (ELPP->vRegistry()->allowed(el::Level::Debug, MKEcoin_DEFAULT_LOG_CATEGORY))
      TREZOR_LIBUSB_SET_DEBUG(ctx, 3);
    else if (ELPP->vRegistry()->allowed(el::Level::Warning, MKEcoin_DEFAULT_LOG_CATEGORY))
      TREZOR_LIBUSB_SET_DEBUG(ctx, 2);
    else if (ELPP->vRegistry()->allowed(el::Level::Error, MKEcoin_DEFAULT_LOG_CATEGORY))
      TREZOR_LIBUSB_SET_DEBUG(ctx, 1);

#undef TREZOR_LIBUSB_SET_DEBUG
  }

  static int get_libusb_ports(libusb_device *dev, std::vector<uint8_t> &path){
    uint8_t tmp_path[16];
    int r = libusb_get_port_numbers(dev, tmp_path, sizeof(tmp_path));
    CHECK_AND_ASSERT_MES(r != LIBUSB_ERROR_OVERFLOW, -1, "Libusb path array too small");
    CHECK_AND_ASSERT_MES(r >= 0, -1, "Libusb path array error");

    path.resize(r);
    for (int i = 0; i < r; i++){
      path[i] = tmp_path[i];
    }

    return 0;
  }

  static std::string get_usb_path(uint8_t bus_id, const std::vector<uint8_t> &path){
    std::stringstream ss;
    ss << WebUsbTransport::PATH_PREFIX << (boost::format("%03d") % ((int)bus_id));
    for(uint8_t port : path){
      ss << ":" << ((int) port);
    }
    return ss.str();
  }

  const char * WebUsbTransport::PATH_PREFIX = "webusb:";

  WebUsbTransport::WebUsbTransport(
      boost::optional<libusb_device_descriptor*> descriptor,
      boost::optional<std::shared_ptr<Protocol>> proto
  ): m_usb_session(nullptr), m_usb_device(nullptr), m_usb_device_handle(nullptr),
     m_bus_id(-1), m_device_addr(-1)
  {
    if (descriptor){
      libusb_device_descriptor * desc = new libusb_device_descriptor;
      memcpy(desc, descriptor.get(), sizeof(libusb_device_descriptor));
      this->m_usb_device_desc.reset(desc);
    }

    m_proto = proto ? proto.get() : std::make_shared<ProtocolV1>();

#ifdef WITH_TREZOR_DEBUGGING
    m_debug_mode = false;
#endif
  }

  WebUsbTransport::~WebUsbTransport(){
    if (m_usb_device){
      close();
    }

    if (m_usb_session) {
      libusb_exit(m_usb_session);
      m_usb_session = nullptr;
    }
  }

  void WebUsbTransport::require_device() const{
    if (!m_usb_device_desc){
      throw std::runtime_error("No USB device specified");
    }
  }

  void WebUsbTransport::require_connected() const{
    require_device();
    if (!m_usb_device_handle){
      throw std::runtime_error("USB Device not opened");
    }
  }

  void WebUsbTransport::enumerate(t_transport_vect & res) {
    int r;
    libusb_device **devs;
    libusb_context *ctx = nullptr;

    r = libusb_init(&ctx);
    CHECK_AND_ASSERT_THROW_MES(r >= 0, "Unable to init libusb");

    set_libusb_log(ctx);

    ssize_t cnt = libusb_get_device_list(ctx, &devs);
    if (cnt < 0){
      libusb_exit(ctx);
      throw std::runtime_error("Unable to enumerate libusb devices");
    }

    MTRACE("Libusb devices: " << cnt);

    for(ssize_t i = 0; i < cnt; i++) {
      libusb_device_descriptor desc{};
      r = libusb_get_device_descriptor(devs[i], &desc);
      if (r < 0){
        MERROR("Unable to get libusb device descriptor " << i);
        continue;
      }

      const auto trezor_dev_idx = get_trezor_dev_id(&desc);
      if (!is_device_supported(trezor_dev_idx)){
        continue;
      }

      MTRACE("Found Trezor device: " << desc.idVendor << ":" << desc.idProduct << " dev_idx " << (int)trezor_dev_idx);

      auto t = std::make_shared<WebUsbTransport>(boost::make_optional(&desc));
      t->m_bus_id = libusb_get_bus_number(devs[i]);
      t->m_device_addr = libusb_get_device_address(devs[i]);

      // Port resolution may fail. Non-critical error, just addressing precision is decreased.
      get_libusb_ports(devs[i], t->m_port_numbers);

      res.push_back(t);
    }

    libusb_free_device_list(devs, 1);
    libusb_exit(ctx);
  }

  std::string WebUsbTransport::get_path() const {
    if (!m_usb_device_desc){
      return "";
    }

    return get_usb_path(static_cast<uint8_t>(m_bus_id), m_port_numbers);
  };

  void WebUsbTransport::open() {
    if (!pre_open()){
      return;
    }
    const int interface = get_interface();

#define TREZOR_DESTROY_SESSION() do { libusb_exit(m_usb_session); m_usb_session = nullptr; } while(0)

    int r;
    libusb_device **devs = nullptr;

    if (m_usb_session) {
      TREZOR_DESTROY_SESSION();
    }

    r = libusb_init(&m_usb_session);
    CHECK_AND_ASSERT_THROW_MES(r >= 0, "Unable to init libusb");
    set_libusb_log(m_usb_session);

    bool found = false;
    int open_res = 0;

    ssize_t cnt = libusb_get_device_list(m_usb_session, &devs);
    if (cnt < 0){
      TREZOR_DESTROY_SESSION();
      throw std::runtime_error("Unable to enumerate libusb devices");
    }

    for (ssize_t i = 0; i < cnt; i++) {
      libusb_device_descriptor desc{};
      r = libusb_get_device_descriptor(devs[i], &desc);
      if (r < 0){
        MERROR("Unable to get libusb device descriptor " << i);
        continue;
      }

      const auto trezor_dev_idx = get_trezor_dev_id(&desc);
      if (!is_device_supported(trezor_dev_idx)){
        continue;
      }

      auto bus_id = libusb_get_bus_number(devs[i]);
      std::vector<uint8_t> path;

      // Port resolution may fail. Non-critical error, just addressing precision is decreased.
      get_libusb_ports(devs[i], path);

      MTRACE("Found Trezor device: " << desc.idVendor << ":" << desc.idProduct
                                     << ", dev_idx: " << (int)trezor_dev_idx
                                     << ". path: " << get_usb_path(bus_id, path));

      if (bus_id == m_bus_id && path == m_port_numbers) {
        found = true;
        m_usb_device = devs[i];
        open_res = libusb_open(m_usb_device, &m_usb_device_handle);
        break;
      }
    }

    libusb_free_device_list(devs, 1);

    if (!found){
      TREZOR_DESTROY_SESSION();
      throw exc::DeviceAcquireException("Device not found");

    } else if (found && open_res != 0) {
      m_usb_device_handle = nullptr;
      m_usb_device = nullptr;
      TREZOR_DESTROY_SESSION();
      throw exc::DeviceAcquireException("Unable to open libusb device");
    }

    r = libusb_claim_interface(m_usb_device_handle, interface);

    if (r != 0){
      libusb_close(m_usb_device_handle);
      m_usb_device_handle = nullptr;
      m_usb_device = nullptr;
      TREZOR_DESTROY_SESSION();
      throw exc::DeviceAcquireException("Unable to claim libusb device");
    }

    m_open_counter = 1;
    m_proto->session_begin(*this);

#undef TREZOR_DESTROY_SESSION
  };

  void WebUsbTransport::close() {
    if (!pre_close()){
      return;
    }

    MTRACE("Closing Trezor:WebUsbTransport");
    m_proto->session_end(*this);

    int r = libusb_release_interface(m_usb_device_handle, get_interface());
    if (r != 0){
      MERROR("Could not release libusb interface: " << r);
    }

    m_usb_device = nullptr;
    if (m_usb_device_handle) {
      libusb_close(m_usb_device_handle);
      m_usb_device_handle = nullptr;
    }

    if (m_usb_session) {
      libusb_exit(m_usb_session);
      m_usb_session = nullptr;
    }
  };

  std::shared_ptr<Transport> WebUsbTransport::find_debug() {
#ifdef WITH_TREZOR_DEBUGGING
    require_device();
    auto t = std::make_shared<WebUsbTransport>(boost::make_optional(m_usb_device_desc.get()));
    t->m_bus_id = m_bus_id;
    t->m_device_addr = m_device_addr;
    t->m_port_numbers = m_port_numbers;
    t->m_debug_mode = true;
    return t;
#else
      MINFO("Debug link is disabled in production");
      return nullptr;
#endif
    }

  int WebUsbTransport::get_interface() const{
    const int INTERFACE_NORMAL = 0;
#ifdef WITH_TREZOR_DEBUGGING
    const int INTERFACE_DEBUG = 1;
    return m_debug_mode ? INTERFACE_DEBUG : INTERFACE_NORMAL;
#else
    return INTERFACE_NORMAL;
#endif
  }

  unsigned char WebUsbTransport::get_endpoint() const{
    const unsigned char ENDPOINT_NORMAL = 1;
#ifdef WITH_TREZOR_DEBUGGING
    const unsigned char ENDPOINT_DEBUG = 2;
    return m_debug_mode ? ENDPOINT_DEBUG : ENDPOINT_NORMAL;
#else
    return ENDPOINT_NORMAL;
#endif
  }

  void WebUsbTransport::write(const google::protobuf::Message &req) {
    m_proto->write(*this, req);
  };

  void WebUsbTransport::read(std::shared_ptr<google::protobuf::Message> & msg, messages::MessageType * msg_type) {
    m_proto->read(*this, msg, msg_type);
  };

  void WebUsbTransport::write_chunk(const void * buff, size_t size) {
    require_connected();
    if (size != REPLEN){
      throw exc::CommunicationException("Invalid chunk size: ");
    }

    unsigned char endpoint = get_endpoint();
    endpoint = (endpoint & ~LIBUSB_ENDPOINT_DIR_MASK) | LIBUSB_ENDPOINT_OUT;

    int transferred = 0;
    int r = libusb_interrupt_transfer(m_usb_device_handle, endpoint, (unsigned char*)buff, (int)size, &transferred, 0);
    CHECK_AND_ASSERT_THROW_MES(r == 0, "Unable to transfer, r: " << r);
    if (transferred != (int)size){
      throw exc::CommunicationException("Could not transfer chunk");
    }
  };

  size_t WebUsbTransport::read_chunk(void * buff, size_t size) {
    require_connected();
    unsigned char endpoint = get_endpoint();
    endpoint = (endpoint & ~LIBUSB_ENDPOINT_DIR_MASK) | LIBUSB_ENDPOINT_IN;

    int transferred = 0;
    int r = libusb_interrupt_transfer(m_usb_device_handle, endpoint, (unsigned char*)buff, (int)size, &transferred, 0);
    CHECK_AND_ASSERT_THROW_MES(r == 0, "Unable to transfer, r: " << r);
    if (transferred != (int)size){
      throw exc::CommunicationException("Could not read the chunk");
    }

    return transferred;
  };

  std::ostream& WebUsbTransport::dump(std::ostream& o) const {
    o << "WebUsbTransport<path=" << get_path()
             << ", vendorId=" << (m_usb_device_desc ? std::to_string(m_usb_device_desc->idVendor) : "?")
             << ", productId=" << (m_usb_device_desc ? std::to_string(m_usb_device_desc->idProduct) : "?")
             << ", deviceType=";

    if (m_usb_device_desc){
      if (is_trezor1(m_usb_device_desc.get()))
        o << "TrezorOne";
      else if (is_trezor2(m_usb_device_desc.get()))
        o << "TrezorT";
      else if (is_trezor2_bl(m_usb_device_desc.get()))
        o << "TrezorT BL";
    } else {
      o << "?";
    }

    return o << ">";
  };

#endif  // WITH_DEVICE_TREZOR_WEBUSB

  void enumerate(t_transport_vect & res){
    BridgeTransport bt;
    try{
      bt.enumerate(res);
    } catch (const std::exception & e){
      MERROR("BridgeTransport enumeration failed:" << e.what());
    }

#ifdef WITH_DEVICE_TREZOR_WEBUSB
    hw::trezor::WebUsbTransport btw;
    try{
      btw.enumerate(res);
    } catch (const std::exception & e){
      MERROR("WebUsbTransport enumeration failed:" << e.what());
    }
#endif

#ifdef WITH_DEVICE_TREZOR_UDP
    hw::trezor::UdpTransport btu;
    try{
      btu.enumerate(res);
    } catch (const std::exception & e){
      MERROR("UdpTransport enumeration failed:" << e.what());
    }
#endif
  }

  void sort_transports_by_env(t_transport_vect & res){
    const char *env_trezor_path = getenv("TREZOR_PATH");
    if (!env_trezor_path){
      return;
    }

    // Sort transports by the longest matching prefix with TREZOR_PATH
    std::string trezor_path(env_trezor_path);
    std::vector<size_t> match_idx(res.size());
    std::vector<size_t> path_permutation(res.size());

    for(size_t i = 0; i < res.size(); ++i){
      auto cpath = res[i]->get_path();
      std::string * s1 = &trezor_path;
      std::string * s2 = &cpath;

      // first has to be shorter in std::mismatch(). Returns first non-matching iterators.
      if (s1->size() >= s2->size()){
        std::swap(s1, s2);
      }

      const auto mism = std::mismatch(s1->begin(), s1->end(), s2->begin());
      match_idx[i] = mism.first - s1->begin();
      path_permutation[i] = i;
    }

    std::sort(path_permutation.begin(), path_permutation.end(), [&](const size_t i0, const size_t i1) {
      return match_idx[i0] > match_idx[i1];
    });

    tools::apply_permutation(path_permutation, res);
  }

  std::shared_ptr<Transport> transport(const std::string & path){
    if (boost::starts_with(path, BridgeTransport::PATH_PREFIX)){
      return std::make_shared<BridgeTransport>(path.substr(strlen(BridgeTransport::PATH_PREFIX)));

    } else if (boost::starts_with(path, UdpTransport::PATH_PREFIX)){
      return std::make_shared<UdpTransport>(path.substr(strlen(UdpTransport::PATH_PREFIX)));

    } else {
      throw std::invalid_argument("Unknown Trezor device path: " + path);

    }
  }

  void throw_failure_exception(const messages::common::Failure * failure) {
    if (failure == nullptr){
      throw std::invalid_argument("Failure message cannot be null");
    }

    boost::optional<std::string> message = failure->has_message() ? boost::make_optional(failure->message()) : boost::none;
    boost::optional<uint32_t> code = failure->has_code() ? boost::make_optional(static_cast<uint32_t>(failure->code())) : boost::none;
    if (!code){
      throw exc::proto::FailureException(code, message);
    }

    auto ecode = failure->code();
    if (ecode == messages::common::Failure_FailureType_Failure_UnexpectedMessage){
      throw exc::proto::UnexpectedMessageException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_ActionCancelled){
      throw exc::proto::CancelledException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_PinExpected){
      throw exc::proto::PinExpectedException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_PinInvalid){
      throw exc::proto::InvalidPinException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_NotEnoughFunds){
      throw exc::proto::NotEnoughFundsException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_NotInitialized){
      throw exc::proto::NotInitializedException(code, message);
    } else if (ecode == messages::common::Failure_FailureType_Failure_FirmwareError){
      throw exc::proto::FirmwareErrorException(code, message);
    } else {
      throw exc::proto::FailureException(code, message);
    }
  }

  GenericMessage::GenericMessage(messages::MessageType m_type, const shared_ptr<google::protobuf::Message> &m_msg)
        : m_type(m_type), m_msg(m_msg), m_empty(false) {}

  std::ostream& operator<<(std::ostream& o, hw::trezor::Transport const& t){
    return t.dump(o);
  }

  std::ostream& operator<<(std::ostream& o, std::shared_ptr<hw::trezor::Transport> const& t){
    if (!t){
      return o << "None";
    }

    return t->dump(o);
  }

}
}

max_stars_repo_name: MKEcoin/MkecoinTest

max_issues_repo_name: MKEcoin/MkecoinTest

max_forks_repo_name: MKEcoin/MkecoinTest