<commit_before>#include <gtest/gtest.h>

#include <boost/filesystem.hpp>

#include <Options.h>
#include <OptionsManager.h>

class TestOptions : public ::testing::Test {
protected:
    std::string filename;
    const Options& options;
    const OptionsManager optionsManager;

    TestOptions() :
        filename("../../Wangscape/example/example_options.json"),
        optionsManager(filename),
        options(optionsManager.getOptions())
    {
    };
    ~TestOptions() {};
};

TEST_F(TestOptions, TestOptionsValues)
{
    EXPECT_EQ(options.filename, filename) <<
        "Incorrect options filename";
    EXPECT_TRUE(boost::filesystem::equivalent(
        boost::filesystem::path(options.relativeOutputDirectory),
        boost::filesystem::path("../Wangscape/example/output"))) <<
        "Incorrect relative output directory";
    EXPECT_STREQ(options.outputDirectory.c_str(),
                 "output") <<
        "Incorrect output directory";
    EXPECT_STREQ(options.outputFilenames.tileDataFilename.c_str(),
                 "tiles.json") <<
        "Incorrect tile data filename";
    EXPECT_STREQ(options.outputFilenames.tilesetDataFilename.c_str(),
                 "tilesets.json") <<
        "Incorrect tileset data filename";
    EXPECT_STREQ(options.outputFilenames.tileGroupsFilename.c_str(),
                 "tile_groups.json") <<
        "Incorrect tile groups filename";
    EXPECT_STREQ(options.outputFilenames.terrainHypergraphFilename.c_str(),
                 "terrain_hypergraph.json") <<
        "Incorrect terrain hypergraph filename";
    EXPECT_EQ(options.tileFormat.resolution, 32) <<
        "Incorrect resolution";
    EXPECT_NE(options.terrains.find("g"), options.terrains.cend()) <<
        "Options did not load grass terrain info";
    EXPECT_NE(options.cliques.size(), 0) <<
        "Options did not load any cliques";
}<commit_msg>Change path to example_options.json in test<commit_after>#include <gtest/gtest.h>

#include <boost/filesystem.hpp>

#include <Options.h>
#include <OptionsManager.h>

class TestOptions : public ::testing::Test {
protected:
    std::string filename;
    const Options& options;
    const OptionsManager optionsManager;

    TestOptions() :
        filename("../../Wangscape/example/example_options.json"),
        optionsManager(filename),
        options(optionsManager.getOptions())
    {
    };
    ~TestOptions() {};
};

TEST_F(TestOptions, TestOptionsValues)
{
    EXPECT_EQ(options.filename, filename) <<
        "Incorrect options filename";
    EXPECT_TRUE(boost::filesystem::equivalent(
        boost::filesystem::path(options.relativeOutputDirectory),
        boost::filesystem::path("../../Wangscape/example/output"))) <<
        "Incorrect relative output directory";
    EXPECT_STREQ(options.outputDirectory.c_str(),
                 "output") <<
        "Incorrect output directory";
    EXPECT_STREQ(options.outputFilenames.tileDataFilename.c_str(),
                 "tiles.json") <<
        "Incorrect tile data filename";
    EXPECT_STREQ(options.outputFilenames.tilesetDataFilename.c_str(),
                 "tilesets.json") <<
        "Incorrect tileset data filename";
    EXPECT_STREQ(options.outputFilenames.tileGroupsFilename.c_str(),
                 "tile_groups.json") <<
        "Incorrect tile groups filename";
    EXPECT_STREQ(options.outputFilenames.terrainHypergraphFilename.c_str(),
                 "terrain_hypergraph.json") <<
        "Incorrect terrain hypergraph filename";
    EXPECT_EQ(options.tileFormat.resolution, 32) <<
        "Incorrect resolution";
    EXPECT_NE(options.terrains.find("g"), options.terrains.cend()) <<
        "Options did not load grass terrain info";
    EXPECT_NE(options.cliques.size(), 0) <<
        "Options did not load any cliques";
}
<|endoftext|>


<commit_before>/*
 *  Multi Marker Pose Estimation using ARToolkit
 *  Copyright (C) 2010, CCNY Robotics Lab
 *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
 *  William Morris <morris@ee.ccny.cuny.edu>
 *  Gautier Dumonteil <gautier.dumonteil@gmail.com>
 *  http://robotics.ccny.cuny.edu
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "ar_pose/ar_multi.h"
#include "ar_pose/object.h"

int main (int argc, char **argv)
{
  ros::init (argc, argv, "ar_single");
  ros::NodeHandle n;
  ar_pose::ARSinglePublisher ar_single (n);
  ros::spin ();
  return 0;
}

namespace ar_pose
{
  ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
  {
    std::string local_path;
    std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
	std::string default_path = "data/object_4x4";
    ros::NodeHandle n_param ("~");
    XmlRpc::XmlRpcValue xml_marker_center;

    // **** get parameters

    if (!n_param.getParam ("publish_tf", publishTf_))
      publishTf_ = true;
    ROS_INFO ("\tPublish transforms: %d", publishTf_);

    if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
      publishVisualMarkers_ = true;
    ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);

    if (!n_param.getParam ("threshold", threshold_))
      threshold_ = 100;
    ROS_INFO ("\tThreshold: %d", threshold_);

	//modifications to allow path list from outside the package
	n_param.param ("marker_pattern_list", local_path, default_path);
	if (local_path.compare(0,5,"data/") == 0){
	  //according to previous implementations, check if first 5 chars equal "data/"
	  sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
	}
	else{
	  //for new implementations, can pass a path outside the package_path
	  sprintf (pattern_filename_, "%s", local_path.c_str ());
	}
	ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);

    // **** subscribe

    ROS_INFO ("Subscribing to info topic");
    sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
    getCamInfo_ = false;

    // **** advertsie

    arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
    if(publishVisualMarkers_)
    {
		rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
	 }
  }

  ARSinglePublisher::~ARSinglePublisher (void)
  {
    //cvReleaseImage(&capture_); //Don't know why but crash when release the image
    arVideoCapStop ();
    arVideoClose ();
  }

  void ARSinglePublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
  {
    if (!getCamInfo_)
    {
      cam_info_ = (*cam_info);

      cam_param_.xsize = cam_info_.width;
      cam_param_.ysize = cam_info_.height;

      cam_param_.mat[0][0] = cam_info_.P[0];
      cam_param_.mat[1][0] = cam_info_.P[4];
      cam_param_.mat[2][0] = cam_info_.P[8];
      cam_param_.mat[0][1] = cam_info_.P[1];
      cam_param_.mat[1][1] = cam_info_.P[5];
      cam_param_.mat[2][1] = cam_info_.P[9];
      cam_param_.mat[0][2] = cam_info_.P[2];
      cam_param_.mat[1][2] = cam_info_.P[6];
      cam_param_.mat[2][2] = cam_info_.P[10];
      cam_param_.mat[0][3] = cam_info_.P[3];
      cam_param_.mat[1][3] = cam_info_.P[7];
      cam_param_.mat[2][3] = cam_info_.P[11];

      cam_param_.dist_factor[0] = cam_info_.K[2];       // x0 = cX from openCV calibration
      cam_param_.dist_factor[1] = cam_info_.K[5];       // y0 = cY from openCV calibration
      cam_param_.dist_factor[2] = -100*cam_info_.D[0];  // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...

      arInit ();

      ROS_INFO ("Subscribing to image topic");
      cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
      getCamInfo_ = true;
    }
  }

  void ARSinglePublisher::arInit ()
  {
    arInitCparam (&cam_param_);
    ROS_INFO ("*** Camera Parameter ***");
    arParamDisp (&cam_param_);

    // load in the object data - trained markers and associated bitmap files
    if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
      ROS_BREAK ();
    ROS_DEBUG ("Objectfile num = %d", objectnum);

    sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
    capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
  }

  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the
     * ARToolKit configure option (see config.h).*/
    /*try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    //cvConvertImage(capture,capture,CV_CVTIMG_FLIP);
    dataPtr = (ARUint8 *) capture_->imageData;*/

    cv_bridge::CvImagePtr cv_ptr;

    try
    {
      cv_ptr = cv_bridge::toCvCopy(image_msg,
          sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    dataPtr = (ARUint8 *) cv_ptr->image.data;

    // detect the markers in the video frame
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      ROS_BREAK ();
    }

    arPoseMarkers_.markers.clear ();
    // check for known patterns
    for (i = 0; i < objectnum; i++)
    {
      k = -1;
      for (j = 0; j < marker_num; j++)
      {
        if (object[i].id == marker_info[j].id)
        {
          if (k == -1)
            k = j;
          else                  // make sure you have the best pattern (highest confidence factor)
          if (marker_info[k].cf < marker_info[j].cf)
            k = j;
        }
      }
      if (k == -1)
      {
        object[i].visible = 0;
        continue;
      }

      // calculate the transform for each marker
      if (object[i].visible == 0)
      {
        arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      else
      {
        arGetTransMatCont (&marker_info[k], object[i].trans,
                           object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      object[i].visible = 1;

      double arQuat[4], arPos[3];

      //arUtilMatInv (object[i].trans, cam_trans);
      arUtilMat2QuatPos (object[i].trans, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];

      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" Object num %i ------------------------------------------------", i);
      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = image_msg->header.frame_id;
      ar_pose_marker.header.stamp = image_msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = pos[0];
      ar_pose_marker.pose.pose.position.y = pos[1];
      ar_pose_marker.pose.pose.position.z = pos[2];

      ar_pose_marker.pose.pose.orientation.x = quat[0];
      ar_pose_marker.pose.pose.orientation.y = quat[1];
      ar_pose_marker.pose.pose.orientation.z = quat[2];
      ar_pose_marker.pose.pose.orientation.w = quat[3];

      ar_pose_marker.confidence = round(marker_info->cf * 100);
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      // **** publish transform between camera and marker

      tf::Quaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      tf::Vector3 origin (pos[0], pos[1], pos[2]);
      tf::Transform t (rotation, origin);

      if (publishTf_)
      {
			tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name);
			broadcaster_.sendTransform(camToMarker);
      }

      // **** publish visual marker

      if (publishVisualMarkers_)
      {
        tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
        tf::Transform markerPose = t * m; // marker pose in the camera frame

        tf::poseTFToMsg (markerPose, rvizMarker_.pose);

        rvizMarker_.header.frame_id = image_msg->header.frame_id;
        rvizMarker_.header.stamp = image_msg->header.stamp;
        rvizMarker_.id = object[i].id;

        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.ns = "basic_shapes";
        rvizMarker_.type = visualization_msgs::Marker::CUBE;
        rvizMarker_.action = visualization_msgs::Marker::ADD;
        switch (i)
        {
          case 0:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 1.0f;
            rvizMarker_.color.a = 1.0;
            break;
          case 1:
            rvizMarker_.color.r = 1.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
            break;
          default:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 1.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
        }
        rvizMarker_.lifetime = ros::Duration (1.0);

        rvizMarkerPub_.publish (rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    arMarkerPub_.publish (arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }
}                               // end namespace ar_pose
<commit_msg>broadcasts pose to tf under name ar_tag<commit_after>/*
 *  Multi Marker Pose Estimation using ARToolkit
 *  Copyright (C) 2010, CCNY Robotics Lab
 *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
 *  William Morris <morris@ee.ccny.cuny.edu>
 *  Gautier Dumonteil <gautier.dumonteil@gmail.com>
 *  http://robotics.ccny.cuny.edu
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "ar_pose/ar_multi.h"
#include "ar_pose/object.h"

std::string OBJECT_NAME = "ar_tag";

int main (int argc, char **argv)
{
  ros::init (argc, argv, "ar_single");
  ros::NodeHandle n;
  ar_pose::ARSinglePublisher ar_single (n);
  ros::spin ();
  return 0;
}

namespace ar_pose
{
  ARSinglePublisher::ARSinglePublisher (ros::NodeHandle & n):n_ (n), it_ (n_)
  {
    std::string local_path;
    std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
	std::string default_path = "data/object_4x4";
    ros::NodeHandle n_param ("~");
    XmlRpc::XmlRpcValue xml_marker_center;

    // **** get parameters

    if (!n_param.getParam ("publish_tf", publishTf_))
      publishTf_ = true;
    ROS_INFO ("\tPublish transforms: %d", publishTf_);

    if (!n_param.getParam ("publish_visual_markers", publishVisualMarkers_))
      publishVisualMarkers_ = true;
    ROS_INFO ("\tPublish visual markers: %d", publishVisualMarkers_);

    if (!n_param.getParam ("threshold", threshold_))
      threshold_ = 100;
    ROS_INFO ("\tThreshold: %d", threshold_);

	//modifications to allow path list from outside the package
	n_param.param ("marker_pattern_list", local_path, default_path);
	if (local_path.compare(0,5,"data/") == 0){
	  //according to previous implementations, check if first 5 chars equal "data/"
	  sprintf (pattern_filename_, "%s/%s", package_path.c_str (), local_path.c_str ());
	}
	else{
	  //for new implementations, can pass a path outside the package_path
	  sprintf (pattern_filename_, "%s", local_path.c_str ());
	}
	ROS_INFO ("Marker Pattern Filename: %s", pattern_filename_);

    // **** subscribe

    ROS_INFO ("Subscribing to info topic");
    sub_ = n_.subscribe (cameraInfoTopic_, 1, &ARSinglePublisher::camInfoCallback, this);
    getCamInfo_ = false;

    // **** advertsie

    arMarkerPub_ = n_.advertise < ar_pose::ARMarkers > ("ar_pose_marker", 0);
    if(publishVisualMarkers_)
    {
		rvizMarkerPub_ = n_.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
	 }
  }

  ARSinglePublisher::~ARSinglePublisher (void)
  {
    //cvReleaseImage(&capture_); //Don't know why but crash when release the image
    arVideoCapStop ();
    arVideoClose ();
  }

  void ARSinglePublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
  {
    if (!getCamInfo_)
    {
      cam_info_ = (*cam_info);

      cam_param_.xsize = cam_info_.width;
      cam_param_.ysize = cam_info_.height;

      cam_param_.mat[0][0] = cam_info_.P[0];
      cam_param_.mat[1][0] = cam_info_.P[4];
      cam_param_.mat[2][0] = cam_info_.P[8];
      cam_param_.mat[0][1] = cam_info_.P[1];
      cam_param_.mat[1][1] = cam_info_.P[5];
      cam_param_.mat[2][1] = cam_info_.P[9];
      cam_param_.mat[0][2] = cam_info_.P[2];
      cam_param_.mat[1][2] = cam_info_.P[6];
      cam_param_.mat[2][2] = cam_info_.P[10];
      cam_param_.mat[0][3] = cam_info_.P[3];
      cam_param_.mat[1][3] = cam_info_.P[7];
      cam_param_.mat[2][3] = cam_info_.P[11];

      cam_param_.dist_factor[0] = cam_info_.K[2];       // x0 = cX from openCV calibration
      cam_param_.dist_factor[1] = cam_info_.K[5];       // y0 = cY from openCV calibration
      cam_param_.dist_factor[2] = -100*cam_info_.D[0];  // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
      cam_param_.dist_factor[3] = 1.0;                  // scale factor, should probably be >1, but who cares...

      arInit ();

      ROS_INFO ("Subscribing to image topic");
      cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
      getCamInfo_ = true;
    }
  }

  void ARSinglePublisher::arInit ()
  {
    arInitCparam (&cam_param_);
    ROS_INFO ("*** Camera Parameter ***");
    arParamDisp (&cam_param_);

    // load in the object data - trained markers and associated bitmap files
    if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
      ROS_BREAK ();
    ROS_DEBUG ("Objectfile num = %d", objectnum);

    sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
    capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
  }

  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the
     * ARToolKit configure option (see config.h).*/
    /*try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    //cvConvertImage(capture,capture,CV_CVTIMG_FLIP);
    dataPtr = (ARUint8 *) capture_->imageData;*/

    cv_bridge::CvImagePtr cv_ptr;

    try
    {
      cv_ptr = cv_bridge::toCvCopy(image_msg,
          sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    dataPtr = (ARUint8 *) cv_ptr->image.data;

    // detect the markers in the video frame
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      ROS_BREAK ();
    }

    arPoseMarkers_.markers.clear ();
    // check for known patterns
    for (i = 0; i < objectnum; i++)
    {
      k = -1;
      for (j = 0; j < marker_num; j++)
      {
        if (object[i].id == marker_info[j].id)
        {
          if (k == -1)
            k = j;
          else                  // make sure you have the best pattern (highest confidence factor)
          if (marker_info[k].cf < marker_info[j].cf)
            k = j;
        }
      }
      if (k == -1)
      {
        object[i].visible = 0;
        continue;
      }

      // calculate the transform for each marker
      if (object[i].visible == 0)
      {
        arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      else
      {
        arGetTransMatCont (&marker_info[k], object[i].trans,
                           object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      object[i].visible = 1;

      double arQuat[4], arPos[3];

      //arUtilMatInv (object[i].trans, cam_trans);
      arUtilMat2QuatPos (object[i].trans, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];

      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" Object num %i ------------------------------------------------", i);
      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = image_msg->header.frame_id;
      ar_pose_marker.header.stamp = image_msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = pos[0];
      ar_pose_marker.pose.pose.position.y = pos[1];
      ar_pose_marker.pose.pose.position.z = pos[2];

      ar_pose_marker.pose.pose.orientation.x = quat[0];
      ar_pose_marker.pose.pose.orientation.y = quat[1];
      ar_pose_marker.pose.pose.orientation.z = quat[2];
      ar_pose_marker.pose.pose.orientation.w = quat[3];

      ar_pose_marker.confidence = round(marker_info->cf * 100);
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      // **** publish transform between camera and marker

      tf::Quaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      tf::Vector3 origin (pos[0], pos[1], pos[2]);
      tf::Transform t (rotation, origin);

      if (publishTf_)
      {
			//tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name);
			tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, OBJECT_NAME);
			broadcaster_.sendTransform(camToMarker);
      }

      // **** publish visual marker

      if (publishVisualMarkers_)
      {
        tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
        tf::Transform markerPose = t * m; // marker pose in the camera frame

        tf::poseTFToMsg (markerPose, rvizMarker_.pose);

        rvizMarker_.header.frame_id = image_msg->header.frame_id;
        rvizMarker_.header.stamp = image_msg->header.stamp;
        rvizMarker_.id = object[i].id;

        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.ns = "basic_shapes";
        rvizMarker_.type = visualization_msgs::Marker::CUBE;
        rvizMarker_.action = visualization_msgs::Marker::ADD;
        switch (i)
        {
          case 0:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 1.0f;
            rvizMarker_.color.a = 1.0;
            break;
          case 1:
            rvizMarker_.color.r = 1.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
            break;
          default:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 1.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
        }
        rvizMarker_.lifetime = ros::Duration (1.0);

        rvizMarkerPub_.publish (rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    arMarkerPub_.publish (arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }
}                               // end namespace ar_pose
<|endoftext|>


<commit_before>// XInputHooker.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <SetupAPI.h>
#include <Xinput.h>
#include "MinHook.h"

typedef VOID(WINAPI *XInputEnable_t)(BOOL);
typedef DWORD(WINAPI *XInputGetState_t)(DWORD, XINPUT_STATE *);

typedef BOOL(WINAPI *SetupDiEnumDeviceInterfaces_t)(HDEVINFO, PSP_DEVINFO_DATA, const GUID *, DWORD, PSP_DEVICE_INTERFACE_DATA);
typedef BOOL(WINAPI *DeviceIoControl_t)(HANDLE, DWORD, LPVOID, DWORD, LPVOID, DWORD, LPDWORD, LPOVERLAPPED);

SetupDiEnumDeviceInterfaces_t fpSetupDiEnumDeviceInterfaces = nullptr;
DeviceIoControl_t fpDeviceIoControl = nullptr;

FILE *fLog = nullptr;
unsigned long counter = 0;

BOOL DetourSetupDiEnumDeviceInterfaces(
    HDEVINFO                  DeviceInfoSet,
    PSP_DEVINFO_DATA          DeviceInfoData,
    const GUID                *InterfaceClassGuid,
    DWORD                     MemberIndex,
    PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData
)
{
    printf("GUID = {%08lX-%04hX-%04hX-%02hhX%02hhX-%02hhX%02hhX%02hhX%02hhX%02hhX%02hhX}\n",
        InterfaceClassGuid->Data1, InterfaceClassGuid->Data2, InterfaceClassGuid->Data3,
        InterfaceClassGuid->Data4[0], InterfaceClassGuid->Data4[1], InterfaceClassGuid->Data4[2], InterfaceClassGuid->Data4[3],
        InterfaceClassGuid->Data4[4], InterfaceClassGuid->Data4[5], InterfaceClassGuid->Data4[6], InterfaceClassGuid->Data4[7]);

    return fpSetupDiEnumDeviceInterfaces(DeviceInfoSet, DeviceInfoData, InterfaceClassGuid, MemberIndex, DeviceInterfaceData);
}

BOOL WINAPI DetourDeviceIoControl(
    HANDLE       hDevice,
    DWORD        dwIoControlCode,
    LPVOID       lpInBuffer,
    DWORD        nInBufferSize,
    LPVOID       lpOutBuffer,
    DWORD        nOutBufferSize,
    LPDWORD      lpBytesReturned,
    LPOVERLAPPED lpOverlapped
)
{
    auto inBuffer = static_cast<PUCHAR>(lpInBuffer);
    auto packetCount = counter++;

    fprintf(fLog, "\n[%010lu] [%08lu] [%08lu] [I] - ", packetCount, dwIoControlCode, nInBufferSize);

    for (auto i = 0; i < nInBufferSize; i++)
    {
        fprintf(fLog, "%02X ", inBuffer[i]);
    }

    auto retval = fpDeviceIoControl(hDevice, dwIoControlCode, lpInBuffer, nInBufferSize, lpOutBuffer, nOutBufferSize, lpBytesReturned, lpOverlapped);

    auto outBuffer = static_cast<PUCHAR>(lpOutBuffer);

    fprintf(fLog, "\n[%010lu] [%08lu] [%08lu] [O] - ", packetCount, dwIoControlCode, *lpBytesReturned);

    for (auto i = 0; i < *lpBytesReturned; i++)
    {
        fprintf(fLog, "%02X ", outBuffer[i]);
    }

    return retval;
}

template <typename T>
inline MH_STATUS MH_CreateHookEx(LPVOID pTarget, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHook(pTarget, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

template <typename T>
inline MH_STATUS MH_CreateHookApiEx(
    LPCWSTR pszModule, LPCSTR pszProcName, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHookApi(
        pszModule, pszProcName, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

int main()
{
    fopen_s(&fLog, "xinput.log", "wt");
    fprintf(fLog, "[Time stamp] [I/O ctrl] [buffsize]\n");

    // Initialize MinHook.
    if (MH_Initialize() != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&SetupDiEnumDeviceInterfaces, &DetourSetupDiEnumDeviceInterfaces,
        reinterpret_cast<LPVOID*>(&fpSetupDiEnumDeviceInterfaces)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&SetupDiEnumDeviceInterfaces) != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&DeviceIoControl, &DetourDeviceIoControl,
        reinterpret_cast<LPVOID*>(&fpDeviceIoControl)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&DeviceIoControl) != MH_OK)
    {
        return 1;
    }

    auto mod = LoadLibrary(L"xinput1_3.dll");
    auto enable = reinterpret_cast<XInputEnable_t>(GetProcAddress(mod, "XInputEnable"));
    auto getState = reinterpret_cast<XInputGetState_t>(GetProcAddress(mod, "XInputGetState"));

    enable(TRUE);
    XINPUT_STATE state;

    printf("XInputGetState = 0x%X\n", getState(0, &state));

    // Disable the hook for MessageBoxW.
    if (MH_DisableHook(MH_ALL_HOOKS) != MH_OK)
    {
        return 1;
    }

    // Uninitialize MinHook.
    if (MH_Uninitialize() != MH_OK)
    {
        return 1;
    }

    return 0;
}

<commit_msg>Logging IOCTL in hex<commit_after>// XInputHooker.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <SetupAPI.h>
#include <Xinput.h>
#include "MinHook.h"

typedef VOID(WINAPI *XInputEnable_t)(BOOL);
typedef DWORD(WINAPI *XInputGetState_t)(DWORD, XINPUT_STATE *);

typedef BOOL(WINAPI *SetupDiEnumDeviceInterfaces_t)(HDEVINFO, PSP_DEVINFO_DATA, const GUID *, DWORD, PSP_DEVICE_INTERFACE_DATA);
typedef BOOL(WINAPI *DeviceIoControl_t)(HANDLE, DWORD, LPVOID, DWORD, LPVOID, DWORD, LPDWORD, LPOVERLAPPED);

SetupDiEnumDeviceInterfaces_t fpSetupDiEnumDeviceInterfaces = nullptr;
DeviceIoControl_t fpDeviceIoControl = nullptr;

FILE *fLog = nullptr;
unsigned long counter = 0;

BOOL DetourSetupDiEnumDeviceInterfaces(
    HDEVINFO                  DeviceInfoSet,
    PSP_DEVINFO_DATA          DeviceInfoData,
    const GUID                *InterfaceClassGuid,
    DWORD                     MemberIndex,
    PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData
)
{
    printf("GUID = {%08lX-%04hX-%04hX-%02hhX%02hhX-%02hhX%02hhX%02hhX%02hhX%02hhX%02hhX}\n",
        InterfaceClassGuid->Data1, InterfaceClassGuid->Data2, InterfaceClassGuid->Data3,
        InterfaceClassGuid->Data4[0], InterfaceClassGuid->Data4[1], InterfaceClassGuid->Data4[2], InterfaceClassGuid->Data4[3],
        InterfaceClassGuid->Data4[4], InterfaceClassGuid->Data4[5], InterfaceClassGuid->Data4[6], InterfaceClassGuid->Data4[7]);

    return fpSetupDiEnumDeviceInterfaces(DeviceInfoSet, DeviceInfoData, InterfaceClassGuid, MemberIndex, DeviceInterfaceData);
}

BOOL WINAPI DetourDeviceIoControl(
    HANDLE       hDevice,
    DWORD        dwIoControlCode,
    LPVOID       lpInBuffer,
    DWORD        nInBufferSize,
    LPVOID       lpOutBuffer,
    DWORD        nOutBufferSize,
    LPDWORD      lpBytesReturned,
    LPOVERLAPPED lpOverlapped
)
{
    auto inBuffer = static_cast<PUCHAR>(lpInBuffer);
    auto packetCount = counter++;

    fprintf(fLog, "\n[%010lu] [%08X] [%08lu] [I] - ", packetCount, dwIoControlCode, nInBufferSize);

    for (auto i = 0; i < nInBufferSize; i++)
    {
        fprintf(fLog, "%02X ", inBuffer[i]);
    }

    auto retval = fpDeviceIoControl(hDevice, dwIoControlCode, lpInBuffer, nInBufferSize, lpOutBuffer, nOutBufferSize, lpBytesReturned, lpOverlapped);

    auto outBuffer = static_cast<PUCHAR>(lpOutBuffer);

    fprintf(fLog, "\n[%010lu] [%08X] [%08lu] [O] - ", packetCount, dwIoControlCode, *lpBytesReturned);

    for (auto i = 0; i < *lpBytesReturned; i++)
    {
        fprintf(fLog, "%02X ", outBuffer[i]);
    }

    return retval;
}

template <typename T>
inline MH_STATUS MH_CreateHookEx(LPVOID pTarget, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHook(pTarget, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

template <typename T>
inline MH_STATUS MH_CreateHookApiEx(
    LPCWSTR pszModule, LPCSTR pszProcName, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHookApi(
        pszModule, pszProcName, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

int main()
{
    fopen_s(&fLog, "xinput.log", "wt");
    fprintf(fLog, "[Time stamp] [I/O ctrl] [buffsize]\n");

    // Initialize MinHook.
    if (MH_Initialize() != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&SetupDiEnumDeviceInterfaces, &DetourSetupDiEnumDeviceInterfaces,
        reinterpret_cast<LPVOID*>(&fpSetupDiEnumDeviceInterfaces)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&SetupDiEnumDeviceInterfaces) != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&DeviceIoControl, &DetourDeviceIoControl,
        reinterpret_cast<LPVOID*>(&fpDeviceIoControl)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&DeviceIoControl) != MH_OK)
    {
        return 1;
    }

    auto mod = LoadLibrary(L"xinput1_3.dll");
    auto enable = reinterpret_cast<XInputEnable_t>(GetProcAddress(mod, "XInputEnable"));
    auto getState = reinterpret_cast<XInputGetState_t>(GetProcAddress(mod, "XInputGetState"));

    enable(TRUE);
    XINPUT_STATE state;

    printf("XInputGetState = 0x%X\n", getState(0, &state));

    // Disable the hook for MessageBoxW.
    if (MH_DisableHook(MH_ALL_HOOKS) != MH_OK)
    {
        return 1;
    }

    // Uninitialize MinHook.
    if (MH_Uninitialize() != MH_OK)
    {
        return 1;
    }

    return 0;
}

<|endoftext|>


<commit_before>// XInputHooker.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <SetupAPI.h>
#include <Xinput.h>
#include "MinHook.h"

typedef BOOL(WINAPI *SetupDiEnumDeviceInterfaces_t)(HDEVINFO, PSP_DEVINFO_DATA, const GUID *, DWORD, PSP_DEVICE_INTERFACE_DATA);
typedef VOID(WINAPI *XInputEnable_t)(BOOL);
typedef DWORD(WINAPI *XInputGetState_t)(DWORD, XINPUT_STATE *);

SetupDiEnumDeviceInterfaces_t fpSetupDiEnumDeviceInterfaces = nullptr;

BOOL DetourSetupDiEnumDeviceInterfaces(
    HDEVINFO                  DeviceInfoSet,
    PSP_DEVINFO_DATA          DeviceInfoData,
    const GUID                *InterfaceClassGuid,
    DWORD                     MemberIndex,
    PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData
)
{
    printf("GUID = {%08lX-%04hX-%04hX-%02hhX%02hhX-%02hhX%02hhX%02hhX%02hhX%02hhX%02hhX}\n",
        InterfaceClassGuid->Data1, InterfaceClassGuid->Data2, InterfaceClassGuid->Data3,
        InterfaceClassGuid->Data4[0], InterfaceClassGuid->Data4[1], InterfaceClassGuid->Data4[2], InterfaceClassGuid->Data4[3],
        InterfaceClassGuid->Data4[4], InterfaceClassGuid->Data4[5], InterfaceClassGuid->Data4[6], InterfaceClassGuid->Data4[7]);

    return fpSetupDiEnumDeviceInterfaces(DeviceInfoSet, DeviceInfoData, InterfaceClassGuid, MemberIndex, DeviceInterfaceData);
}

template <typename T>
inline MH_STATUS MH_CreateHookEx(LPVOID pTarget, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHook(pTarget, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

template <typename T>
inline MH_STATUS MH_CreateHookApiEx(
    LPCWSTR pszModule, LPCSTR pszProcName, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHookApi(
        pszModule, pszProcName, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

int main()
{
    MH_STATUS stat;

    // Initialize MinHook.
    if (MH_Initialize() != MH_OK)
    {
        return 1;
    }

    //auto setupapi = LoadLibrary(L"setupapi.dll");

    if (MH_CreateHook(&SetupDiEnumDeviceInterfaces, &DetourSetupDiEnumDeviceInterfaces,
        reinterpret_cast<LPVOID*>(&fpSetupDiEnumDeviceInterfaces)) != MH_OK)
    {
        return 1;
    }

    // or you can use the new helper function like this.
    //if ((stat = MH_CreateHookApiEx(
    //    L"setupapi", "SetupDiEnumDeviceInterfaces", &DetourSetupDiEnumDeviceInterfaces, &fpSetupDiEnumDeviceInterfaces)) != MH_OK)
    //{
    //    return 1;
    //}

    if ((stat = MH_EnableHook(&SetupDiEnumDeviceInterfaces)) != MH_OK)
    {
        return 1;
    }

    auto mod = LoadLibrary(L"xinput1_3.dll");
    auto enable = reinterpret_cast<XInputEnable_t>(GetProcAddress(mod, "XInputEnable"));
    auto getState = reinterpret_cast<XInputGetState_t>(GetProcAddress(mod, "XInputGetState"));

    enable(TRUE);
    XINPUT_STATE state;
    auto retval = getState(0, &state);

    getchar();

    // Disable the hook for MessageBoxW.
    if (MH_DisableHook(MH_ALL_HOOKS) != MH_OK)
    {
        return 1;
    }

    // Uninitialize MinHook.
    if (MH_Uninitialize() != MH_OK)
    {
        return 1;
    }

    return 0;
}

<commit_msg>Removed unused code<commit_after>// XInputHooker.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <SetupAPI.h>
#include <Xinput.h>
#include "MinHook.h"

typedef VOID(WINAPI *XInputEnable_t)(BOOL);
typedef DWORD(WINAPI *XInputGetState_t)(DWORD, XINPUT_STATE *);

typedef BOOL(WINAPI *SetupDiEnumDeviceInterfaces_t)(HDEVINFO, PSP_DEVINFO_DATA, const GUID *, DWORD, PSP_DEVICE_INTERFACE_DATA);
typedef BOOL(WINAPI *DeviceIoControl_t)(HANDLE, DWORD, LPVOID, DWORD, LPVOID, DWORD, LPDWORD, LPOVERLAPPED);

SetupDiEnumDeviceInterfaces_t fpSetupDiEnumDeviceInterfaces = nullptr;
DeviceIoControl_t fpDeviceIoControl = nullptr;

BOOL DetourSetupDiEnumDeviceInterfaces(
    HDEVINFO                  DeviceInfoSet,
    PSP_DEVINFO_DATA          DeviceInfoData,
    const GUID                *InterfaceClassGuid,
    DWORD                     MemberIndex,
    PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData
)
{
    printf("GUID = {%08lX-%04hX-%04hX-%02hhX%02hhX-%02hhX%02hhX%02hhX%02hhX%02hhX%02hhX}\n",
        InterfaceClassGuid->Data1, InterfaceClassGuid->Data2, InterfaceClassGuid->Data3,
        InterfaceClassGuid->Data4[0], InterfaceClassGuid->Data4[1], InterfaceClassGuid->Data4[2], InterfaceClassGuid->Data4[3],
        InterfaceClassGuid->Data4[4], InterfaceClassGuid->Data4[5], InterfaceClassGuid->Data4[6], InterfaceClassGuid->Data4[7]);

    return fpSetupDiEnumDeviceInterfaces(DeviceInfoSet, DeviceInfoData, InterfaceClassGuid, MemberIndex, DeviceInterfaceData);
}

BOOL WINAPI DetourDeviceIoControl(
    HANDLE       hDevice,
    DWORD        dwIoControlCode,
    LPVOID       lpInBuffer,
    DWORD        nInBufferSize,
    LPVOID       lpOutBuffer,
    DWORD        nOutBufferSize,
    LPDWORD      lpBytesReturned,
    LPOVERLAPPED lpOverlapped
)
{
    printf("DeviceIoControl called (nOutBufferSize = %lu)\n", nOutBufferSize);

    return fpDeviceIoControl(hDevice, dwIoControlCode, lpInBuffer, nInBufferSize, lpOutBuffer, nOutBufferSize, lpBytesReturned, lpOverlapped);
}

template <typename T>
inline MH_STATUS MH_CreateHookEx(LPVOID pTarget, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHook(pTarget, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

template <typename T>
inline MH_STATUS MH_CreateHookApiEx(
    LPCWSTR pszModule, LPCSTR pszProcName, LPVOID pDetour, T** ppOriginal)
{
    return MH_CreateHookApi(
        pszModule, pszProcName, pDetour, reinterpret_cast<LPVOID*>(ppOriginal));
}

int main()
{
    // Initialize MinHook.
    if (MH_Initialize() != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&SetupDiEnumDeviceInterfaces, &DetourSetupDiEnumDeviceInterfaces,
        reinterpret_cast<LPVOID*>(&fpSetupDiEnumDeviceInterfaces)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&SetupDiEnumDeviceInterfaces) != MH_OK)
    {
        return 1;
    }

    if (MH_CreateHook(&DeviceIoControl, &DetourDeviceIoControl,
        reinterpret_cast<LPVOID*>(&fpDeviceIoControl)) != MH_OK)
    {
        return 1;
    }

    if (MH_EnableHook(&DeviceIoControl) != MH_OK)
    {
        return 1;
    }

    auto mod = LoadLibrary(L"xinput1_3.dll");
    auto enable = reinterpret_cast<XInputEnable_t>(GetProcAddress(mod, "XInputEnable"));
    auto getState = reinterpret_cast<XInputGetState_t>(GetProcAddress(mod, "XInputGetState"));

    enable(TRUE);
    XINPUT_STATE state;

    while (TRUE)
    {
        getState(1, &state);
        Sleep(1000);
    };

    // Disable the hook for MessageBoxW.
    if (MH_DisableHook(MH_ALL_HOOKS) != MH_OK)
    {
        return 1;
    }

    // Uninitialize MinHook.
    if (MH_Uninitialize() != MH_OK)
    {
        return 1;
    }

    return 0;
}

<|endoftext|>


<commit_before>/** post_auction_service.cc                                 -*- C++ -*-
    Rémi Attab, 18 Apr 2014
    Copyright (c) 2014 Datacratic.  All rights reserved.

    Implementation of the post auction service.

*/

#include "post_auction_service.h"
#include "simple_event_matcher.h"
#include "sharded_event_matcher.h"
#include "rtbkit/common/messages.h"

using namespace std;
using namespace Datacratic;
using namespace ML;

namespace RTBKIT {

/******************************************************************************/
/* POST AUCTION SERVICE                                                       */
/******************************************************************************/

Logging::Category PostAuctionService::print("PostAuctionService");
Logging::Category PostAuctionService::error("PostAuctionService Error", PostAuctionService::print);
Logging::Category PostAuctionService::trace("PostAuctionService Trace", PostAuctionService::print);

PostAuctionService::
PostAuctionService(
        std::shared_ptr<ServiceProxies> proxies, const std::string & serviceName)
    : ServiceBase(serviceName, proxies),

      auctionTimeout(EventMatcher::DefaultAuctionTimeout),
      winTimeout(EventMatcher::DefaultWinTimeout),

      loopMonitor(*this),
      configListener(getZmqContext()),
      monitorProviderClient(getZmqContext(), *this),

      auctions(65536),
      events(65536),

      logger(getZmqContext()),
      endpoint(getZmqContext()),
      bridge(getZmqContext()),
      router(!!getZmqContext())
{}

PostAuctionService::
PostAuctionService(ServiceBase & parent, const std::string & serviceName)
    : ServiceBase(serviceName, parent),

      auctionTimeout(EventMatcher::DefaultAuctionTimeout),
      winTimeout(EventMatcher::DefaultWinTimeout),

      loopMonitor(*this),
      configListener(getZmqContext()),
      monitorProviderClient(getZmqContext(), *this),

      auctions(65536),
      events(65536),

      logger(getZmqContext()),
      endpoint(getZmqContext()),
      bridge(getZmqContext()),
      router(!!getZmqContext())
{}

void
PostAuctionService::
bindTcp()
{
    logger.bindTcp(getServices()->ports->getRange("logs"));
    endpoint.bindTcp(getServices()->ports->getRange("postAuctionLoop"));
    bridge.agents.bindTcp(getServices()->ports->getRange("postAuctionLoopAgents"));
}

void
PostAuctionService::
init(size_t shards)
{
    // Loop monitor is purely for monitoring purposes. There's no message we can
    // just drop in the PAL to alleviate the load.
    loopMonitor.init();
    loopMonitor.addMessageLoop("postAuctionLoop", &loop);

    if(!bidder) {
        Json::Value json;
        json["type"] = "agents";
        initBidderInterface(json);
    }

    initMatcher(shards);
    initConnections();
    monitorProviderClient.init(getServices()->config);
}

void
PostAuctionService::
initBidderInterface(Json::Value const & json)
{
    bidder = BidderInterface::create("bidder", getServices(), json);
    bidder->init(&bridge);
}

void
PostAuctionService::
initMatcher(size_t shards)
{
    if (shards <= 1) {
        LOG(print) << "Creating SimpleEventMatcher" << endl;
        matcher.reset(new SimpleEventMatcher(serviceName(), getServices()));
    }

    else {
        LOG(print) << "Creating ShardedEventMatcher with " << shards << " shards"
            << endl;

        ShardedEventMatcher* m;
        matcher.reset(m = new ShardedEventMatcher(serviceName(), getServices()));
        m->init(shards);
        loop.addSource("PostAuctionService::matcher", *m);
    }

    using std::placeholders::_1;

    matcher->onMatchedWinLoss =
        std::bind(&PostAuctionService::doMatchedWinLoss, this, _1);

    matcher->onMatchedCampaignEvent =
        std::bind(&PostAuctionService::doMatchedCampaignEvent, this, _1);

    matcher->onUnmatchedEvent =
        std::bind(&PostAuctionService::doUnmatched, this, _1);

    matcher->onError = std::bind(&PostAuctionService::doError, this, _1);

    matcher->setWinTimeout(winTimeout);
    matcher->setAuctionTimeout(auctionTimeout);
}

void
PostAuctionService::
initConnections()
{
    using std::placeholders::_1;
    using std::placeholders::_2;

    registerServiceProvider(serviceName(), { "rtbPostAuctionService" });

    LOG(print) << "post auction logger on " << serviceName() + "/logger" << endl;
    logger.init(getServices()->config, serviceName() + "/logger");
    loop.addSource("PostAuctionService::logger", logger);

    auctions.onEvent = std::bind(&PostAuctionService::doAuction, this, _1);
    loop.addSource("PostAuctionService::auctions", auctions);

    events.onEvent = std::bind(&PostAuctionService::doEvent, this,_1);
    loop.addSource("PostAuctionService::events", events);

    // Initialize zeromq endpoints
    endpoint.init(getServices()->config, ZMQ_XREP, serviceName() + "/events");

    router.bind("AUCTION", std::bind(&PostAuctionService::doAuctionMessage, this, _1));
    router.bind("WIN", std::bind(&PostAuctionService::doWinMessage, this, _1));
    router.bind("LOSS", std::bind(&PostAuctionService::doLossMessage, this,_1));
    router.bind("EVENT", std::bind(&PostAuctionService::doCampaignEventMessage, this, _1));
    router.defaultHandler = [=](const std::vector<std::string> & message) {
        LOG(error) << "unroutable message: " << message[0] << std::endl;
    };

    endpoint.messageHandler = std::bind(
            &ZmqMessageRouter::handleMessage, &router, std::placeholders::_1);
    loop.addSource("PostAuctionService::endpoint", endpoint);

    bridge.agents.init(getServices()->config, serviceName() + "/agents");
    bridge.agents.clientMessageHandler = [&] (const std::vector<std::string> & msg)
        {
            // Clients should never send the post auction service anything,
            // but we catch it here just in case
            LOG(print) << "PostAuctionService got agent message " << msg << endl;
        };
    loop.addSource("PostAuctionService::bridge.agents", bridge.agents);

    configListener.init(getServices()->config);
    configListener.onConfigChange =
        std::bind(&PostAuctionService::doConfigChange, this, _1, _2);
    loop.addSource("PostAuctionService::configListener", configListener);

    // Every second we check for expired auctions
    loop.addPeriodic("PostAuctionService::checkExpiredAuctions", 0.1,
            std::bind(&EventMatcher::checkExpiredAuctions, matcher.get()));

}

void
PostAuctionService::
start(std::function<void ()> onStop)
{
    loop.start(onStop);
    monitorProviderClient.start();
    loopMonitor.start();
    matcher->start();
}

void
PostAuctionService::
shutdown()
{
    matcher->shutdown();
    loopMonitor.shutdown();
    loop.shutdown();
    logger.shutdown();
    bridge.shutdown();
    endpoint.shutdown();
    configListener.shutdown();
    monitorProviderClient.shutdown();
}

void
PostAuctionService::
doConfigChange(
        const std::string & agent,
        std::shared_ptr<const AgentConfig> config)
{
    if (!config) return;
    if (config->account.empty())
        throw ML::Exception("attempt to add an account with empty values");

    banker->addSpendAccount(config->account, Amount(),
            [=] (std::exception_ptr error, ShadowAccount && acount) {
                if(error) logException(error, "Banker addSpendAccount");
            });
}

void
PostAuctionService::
doAuctionMessage(const std::vector<std::string> & message)
{
    recordHit("messages.AUCTION");

    auto msg = Message<SubmittedAuctionEvent>::fromString(message.at(2));
    if (msg) {
        auto event = std::make_shared<SubmittedAuctionEvent>(std::move(msg.payload));
        doAuction(std::move(event));
    }

    else {
        LOG(error)
            << "error while parsing AUCTION message: " << message.at(2) << endl;
    }
}

void
PostAuctionService::
doWinMessage(const std::vector<std::string> & message)
{
    recordHit("messages.WIN");
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    doEvent(event);
}

void
PostAuctionService::
doLossMessage(const std::vector<std::string> & message)
{
    recordHit("messages.LOSS");
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    doEvent(event);
}

void
PostAuctionService::
doCampaignEventMessage(const std::vector<std::string> & message)
{
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    recordHit("messages.EVENT." + event->label);
    doEvent(event);
}

void
PostAuctionService::
injectSubmittedAuction(
        const Id & auctionId,
        const Id & adSpotId,
        std::shared_ptr<BidRequest> bidRequest,
        const std::string & bidRequestStr,
        const std::string & bidRequestStrFormat,
        const JsonHolder & augmentations,
        const Auction::Response & bidResponse,
        Date lossTimeout)
{
    if (bidRequestStr.size() == 0) {
        throw ML::Exception("invalid bidRequestStr");
    }
    if (bidRequestStrFormat.size() == 0) {
        throw ML::Exception("invalid bidRequestStrFormat");
    }

    auto event = std::make_shared<SubmittedAuctionEvent>();
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->bidRequest(bidRequest);
    event->bidRequestStr = bidRequestStr;
    event->bidRequestStrFormat = bidRequestStrFormat;
    event->augmentations = augmentations;
    event->bidResponse = bidResponse;
    event->lossTimeout = lossTimeout;

    auctions.push(event);
}
void
PostAuctionService::
injectWin(
        const Id & auctionId,
        const Id & adSpotId,
        Amount winPrice,
        Date timestamp,
        const JsonHolder & winMeta,
        const UserIds & uids,
        const AccountKey & account,
        Date bidTimestamp)
{
    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_WIN;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->winPrice = winPrice;
    event->metadata = winMeta;
    event->uids = uids;
    event->account = account;
    event->bidTimestamp = bidTimestamp;

    events.push(event);
}

void
PostAuctionService::
injectLoss(const Id & auctionId,
           const Id & adSpotId,
           Date timestamp,
           const JsonHolder & json,
           const AccountKey & account,
           Date bidTimestamp)
{
    if (timestamp == Date())
        timestamp = Date::now();

    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_LOSS;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->winPrice = Amount();
    event->account = account;
    event->bidTimestamp = bidTimestamp;

    events.push(event);
}

void
PostAuctionService::
injectCampaignEvent(const string & label,
                    const Id & auctionId,
                    const Id & adSpotId,
                    Date timestamp,
                    const JsonHolder & impressionMeta,
                    const UserIds & uids)
{
    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_CAMPAIGN_EVENT;
    event->label = label;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->metadata = impressionMeta;
    event->uids = uids;

    events.push(event);
}

void
PostAuctionService::
doAuction(std::shared_ptr<SubmittedAuctionEvent> event)
{
    stats.auctions++;
    matcher->doAuction(std::move(event));
}

void
PostAuctionService::
doEvent(std::shared_ptr<PostAuctionEvent> event)
{
    stats.events++;
    matcher->doEvent(std::move(event));
}

void
PostAuctionService::
checkExpiredAuctions()
{
    matcher->checkExpiredAuctions();
}

void
PostAuctionService::
doMatchedWinLoss(std::shared_ptr<MatchedWinLoss> event)
{
    if (event->type == MatchedWinLoss::Loss)
        stats.matchedLosses++;
    else stats.matchedWins++;

    lastWinLoss = Date::now();

    event->publish(logger);
    bidder->sendWinLossMessage(*event);
}

void
PostAuctionService::
doMatchedCampaignEvent(std::shared_ptr<MatchedCampaignEvent> event)
{
    stats.matchedCampaignEvents++;

    lastCampaignEvent = Date::now();

    event->publish(logger);

    // For the moment, send the message to all of the agents that are
    // bidding on this account

    bool sent = false;
    auto onMatchingAgent = [&] (const AgentConfigEntry & entry)
        {
            if (!entry.config) return;
            bidder->sendCampaignEventMessage(entry.name, *event);
            sent = true;
        };

    const AccountKey & account = event->account;
    configListener.forEachAccountAgent(account, onMatchingAgent);

    if (!sent) {
        recordHit("delivery.%s.orphaned", event->label);
        logPAError("doCampaignEvent.noListeners" + event->label,
                "nothing listening for account " + account.toString());
    }
    else recordHit("delivery.%s.delivered", event->label);
}

void
PostAuctionService::
doUnmatched(std::shared_ptr<UnmatchedEvent> event)
{
    stats.unmatchedEvents++;
    event->publish(logger);
}

void
PostAuctionService::
doError(std::shared_ptr<PostAuctionErrorEvent> error)
{
    stats.errors++;
    error->publish(logger);
}

std::string
PostAuctionService::
getProviderClass() const
{
    return "rtbPostAuctionService";
}

MonitorIndicator
PostAuctionService::
getProviderIndicators()
    const
{
    Json::Value value;

    /* PA health check:
       - last campaign event in the last 10 seconds */
    Date now = Date::now();
    bool winLossOk = now < lastWinLoss.plusSeconds(10);
    bool campaignEventOk = now < lastCampaignEvent.plusSeconds(10);

    // Kept around for posterity.
    // Earned the "Best Error Message in RTBKIT" award.
#if 0
    if (!status)  {
      cerr << "--- WRONGNESS DETECTED:"
          << " last event: " << (now - lastCampaignEvent)
          << endl;
    }
#endif

    MonitorIndicator ind;
    ind.serviceName = serviceName();
    ind.status = winLossOk || campaignEventOk;
    ind.message = string()
        + "WinLoss pipe: " + (winLossOk ? "OK" : "ERROR") + ", "
        + "CampaignEvent pipe: " + (campaignEventOk ? "OK" : "ERROR");

    return ind;
}

/******************************************************************************/
/* STATS                                                                      */
/******************************************************************************/

PostAuctionService::Stats::
Stats() :
    auctions(0), events(0),
    matchedWins(0), matchedCampaignEvents(0), unmatchedEvents(0),
    errors(0)
{}

PostAuctionService::Stats::
Stats(const Stats& other) :
    auctions(other.auctions),
    events(other.events),

    matchedWins(other.matchedWins),
    matchedLosses(other.matchedLosses),
    matchedCampaignEvents(other.matchedCampaignEvents),
    unmatchedEvents(other.unmatchedEvents),
    errors(other.errors)
{}

auto
PostAuctionService::Stats::
operator=(const Stats& other) -> Stats&
{
    auctions = other.auctions;
    events = other.events;

    matchedWins = other.matchedWins;
    matchedLosses = other.matchedLosses;
    matchedCampaignEvents = other.matchedCampaignEvents;
    unmatchedEvents = other.unmatchedEvents;
    errors = other.errors;

    return *this;
}

auto
PostAuctionService::Stats::
operator-=(const Stats& other) -> Stats&
{
    auctions -= other.auctions;
    events -= other.events;

    matchedWins -= other.matchedWins;
    matchedLosses -= other.matchedLosses;
    matchedCampaignEvents -= other.matchedCampaignEvents;
    unmatchedEvents -= other.unmatchedEvents;
    errors -= other.errors;

    return *this;
}

} // namepsace RTBKIT
<commit_msg>Post auction loop no longer considers losses for its health check.<commit_after>/** post_auction_service.cc                                 -*- C++ -*-
    Rémi Attab, 18 Apr 2014
    Copyright (c) 2014 Datacratic.  All rights reserved.

    Implementation of the post auction service.

*/

#include "post_auction_service.h"
#include "simple_event_matcher.h"
#include "sharded_event_matcher.h"
#include "rtbkit/common/messages.h"

using namespace std;
using namespace Datacratic;
using namespace ML;

namespace RTBKIT {

/******************************************************************************/
/* POST AUCTION SERVICE                                                       */
/******************************************************************************/

Logging::Category PostAuctionService::print("PostAuctionService");
Logging::Category PostAuctionService::error("PostAuctionService Error", PostAuctionService::print);
Logging::Category PostAuctionService::trace("PostAuctionService Trace", PostAuctionService::print);

PostAuctionService::
PostAuctionService(
        std::shared_ptr<ServiceProxies> proxies, const std::string & serviceName)
    : ServiceBase(serviceName, proxies),

      auctionTimeout(EventMatcher::DefaultAuctionTimeout),
      winTimeout(EventMatcher::DefaultWinTimeout),

      loopMonitor(*this),
      configListener(getZmqContext()),
      monitorProviderClient(getZmqContext(), *this),

      auctions(65536),
      events(65536),

      logger(getZmqContext()),
      endpoint(getZmqContext()),
      bridge(getZmqContext()),
      router(!!getZmqContext())
{}

PostAuctionService::
PostAuctionService(ServiceBase & parent, const std::string & serviceName)
    : ServiceBase(serviceName, parent),

      auctionTimeout(EventMatcher::DefaultAuctionTimeout),
      winTimeout(EventMatcher::DefaultWinTimeout),

      loopMonitor(*this),
      configListener(getZmqContext()),
      monitorProviderClient(getZmqContext(), *this),

      auctions(65536),
      events(65536),

      logger(getZmqContext()),
      endpoint(getZmqContext()),
      bridge(getZmqContext()),
      router(!!getZmqContext())
{}

void
PostAuctionService::
bindTcp()
{
    logger.bindTcp(getServices()->ports->getRange("logs"));
    endpoint.bindTcp(getServices()->ports->getRange("postAuctionLoop"));
    bridge.agents.bindTcp(getServices()->ports->getRange("postAuctionLoopAgents"));
}

void
PostAuctionService::
init(size_t shards)
{
    // Loop monitor is purely for monitoring purposes. There's no message we can
    // just drop in the PAL to alleviate the load.
    loopMonitor.init();
    loopMonitor.addMessageLoop("postAuctionLoop", &loop);

    if(!bidder) {
        Json::Value json;
        json["type"] = "agents";
        initBidderInterface(json);
    }

    initMatcher(shards);
    initConnections();
    monitorProviderClient.init(getServices()->config);
}

void
PostAuctionService::
initBidderInterface(Json::Value const & json)
{
    bidder = BidderInterface::create("bidder", getServices(), json);
    bidder->init(&bridge);
}

void
PostAuctionService::
initMatcher(size_t shards)
{
    if (shards <= 1) {
        LOG(print) << "Creating SimpleEventMatcher" << endl;
        matcher.reset(new SimpleEventMatcher(serviceName(), getServices()));
    }

    else {
        LOG(print) << "Creating ShardedEventMatcher with " << shards << " shards"
            << endl;

        ShardedEventMatcher* m;
        matcher.reset(m = new ShardedEventMatcher(serviceName(), getServices()));
        m->init(shards);
        loop.addSource("PostAuctionService::matcher", *m);
    }

    using std::placeholders::_1;

    matcher->onMatchedWinLoss =
        std::bind(&PostAuctionService::doMatchedWinLoss, this, _1);

    matcher->onMatchedCampaignEvent =
        std::bind(&PostAuctionService::doMatchedCampaignEvent, this, _1);

    matcher->onUnmatchedEvent =
        std::bind(&PostAuctionService::doUnmatched, this, _1);

    matcher->onError = std::bind(&PostAuctionService::doError, this, _1);

    matcher->setWinTimeout(winTimeout);
    matcher->setAuctionTimeout(auctionTimeout);
}

void
PostAuctionService::
initConnections()
{
    using std::placeholders::_1;
    using std::placeholders::_2;

    registerServiceProvider(serviceName(), { "rtbPostAuctionService" });

    LOG(print) << "post auction logger on " << serviceName() + "/logger" << endl;
    logger.init(getServices()->config, serviceName() + "/logger");
    loop.addSource("PostAuctionService::logger", logger);

    auctions.onEvent = std::bind(&PostAuctionService::doAuction, this, _1);
    loop.addSource("PostAuctionService::auctions", auctions);

    events.onEvent = std::bind(&PostAuctionService::doEvent, this,_1);
    loop.addSource("PostAuctionService::events", events);

    // Initialize zeromq endpoints
    endpoint.init(getServices()->config, ZMQ_XREP, serviceName() + "/events");

    router.bind("AUCTION", std::bind(&PostAuctionService::doAuctionMessage, this, _1));
    router.bind("WIN", std::bind(&PostAuctionService::doWinMessage, this, _1));
    router.bind("LOSS", std::bind(&PostAuctionService::doLossMessage, this,_1));
    router.bind("EVENT", std::bind(&PostAuctionService::doCampaignEventMessage, this, _1));
    router.defaultHandler = [=](const std::vector<std::string> & message) {
        LOG(error) << "unroutable message: " << message[0] << std::endl;
    };

    endpoint.messageHandler = std::bind(
            &ZmqMessageRouter::handleMessage, &router, std::placeholders::_1);
    loop.addSource("PostAuctionService::endpoint", endpoint);

    bridge.agents.init(getServices()->config, serviceName() + "/agents");
    bridge.agents.clientMessageHandler = [&] (const std::vector<std::string> & msg)
        {
            // Clients should never send the post auction service anything,
            // but we catch it here just in case
            LOG(print) << "PostAuctionService got agent message " << msg << endl;
        };
    loop.addSource("PostAuctionService::bridge.agents", bridge.agents);

    configListener.init(getServices()->config);
    configListener.onConfigChange =
        std::bind(&PostAuctionService::doConfigChange, this, _1, _2);
    loop.addSource("PostAuctionService::configListener", configListener);

    // Every second we check for expired auctions
    loop.addPeriodic("PostAuctionService::checkExpiredAuctions", 0.1,
            std::bind(&EventMatcher::checkExpiredAuctions, matcher.get()));

}

void
PostAuctionService::
start(std::function<void ()> onStop)
{
    loop.start(onStop);
    monitorProviderClient.start();
    loopMonitor.start();
    matcher->start();
}

void
PostAuctionService::
shutdown()
{
    matcher->shutdown();
    loopMonitor.shutdown();
    loop.shutdown();
    logger.shutdown();
    bridge.shutdown();
    endpoint.shutdown();
    configListener.shutdown();
    monitorProviderClient.shutdown();
}

void
PostAuctionService::
doConfigChange(
        const std::string & agent,
        std::shared_ptr<const AgentConfig> config)
{
    if (!config) return;
    if (config->account.empty())
        throw ML::Exception("attempt to add an account with empty values");

    banker->addSpendAccount(config->account, Amount(),
            [=] (std::exception_ptr error, ShadowAccount && acount) {
                if(error) logException(error, "Banker addSpendAccount");
            });
}

void
PostAuctionService::
doAuctionMessage(const std::vector<std::string> & message)
{
    recordHit("messages.AUCTION");

    auto msg = Message<SubmittedAuctionEvent>::fromString(message.at(2));
    if (msg) {
        auto event = std::make_shared<SubmittedAuctionEvent>(std::move(msg.payload));
        doAuction(std::move(event));
    }

    else {
        LOG(error)
            << "error while parsing AUCTION message: " << message.at(2) << endl;
    }
}

void
PostAuctionService::
doWinMessage(const std::vector<std::string> & message)
{
    recordHit("messages.WIN");
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    doEvent(event);
}

void
PostAuctionService::
doLossMessage(const std::vector<std::string> & message)
{
    recordHit("messages.LOSS");
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    doEvent(event);
}

void
PostAuctionService::
doCampaignEventMessage(const std::vector<std::string> & message)
{
    auto event = std::make_shared<PostAuctionEvent>(
            ML::DB::reconstituteFromString<PostAuctionEvent>(message.at(2)));
    recordHit("messages.EVENT." + event->label);
    doEvent(event);
}

void
PostAuctionService::
injectSubmittedAuction(
        const Id & auctionId,
        const Id & adSpotId,
        std::shared_ptr<BidRequest> bidRequest,
        const std::string & bidRequestStr,
        const std::string & bidRequestStrFormat,
        const JsonHolder & augmentations,
        const Auction::Response & bidResponse,
        Date lossTimeout)
{
    if (bidRequestStr.size() == 0) {
        throw ML::Exception("invalid bidRequestStr");
    }
    if (bidRequestStrFormat.size() == 0) {
        throw ML::Exception("invalid bidRequestStrFormat");
    }

    auto event = std::make_shared<SubmittedAuctionEvent>();
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->bidRequest(bidRequest);
    event->bidRequestStr = bidRequestStr;
    event->bidRequestStrFormat = bidRequestStrFormat;
    event->augmentations = augmentations;
    event->bidResponse = bidResponse;
    event->lossTimeout = lossTimeout;

    auctions.push(event);
}
void
PostAuctionService::
injectWin(
        const Id & auctionId,
        const Id & adSpotId,
        Amount winPrice,
        Date timestamp,
        const JsonHolder & winMeta,
        const UserIds & uids,
        const AccountKey & account,
        Date bidTimestamp)
{
    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_WIN;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->winPrice = winPrice;
    event->metadata = winMeta;
    event->uids = uids;
    event->account = account;
    event->bidTimestamp = bidTimestamp;

    events.push(event);
}

void
PostAuctionService::
injectLoss(const Id & auctionId,
           const Id & adSpotId,
           Date timestamp,
           const JsonHolder & json,
           const AccountKey & account,
           Date bidTimestamp)
{
    if (timestamp == Date())
        timestamp = Date::now();

    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_LOSS;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->winPrice = Amount();
    event->account = account;
    event->bidTimestamp = bidTimestamp;

    events.push(event);
}

void
PostAuctionService::
injectCampaignEvent(const string & label,
                    const Id & auctionId,
                    const Id & adSpotId,
                    Date timestamp,
                    const JsonHolder & impressionMeta,
                    const UserIds & uids)
{
    auto event = std::make_shared<PostAuctionEvent>();
    event->type = PAE_CAMPAIGN_EVENT;
    event->label = label;
    event->auctionId = auctionId;
    event->adSpotId = adSpotId;
    event->timestamp = timestamp;
    event->metadata = impressionMeta;
    event->uids = uids;

    events.push(event);
}

void
PostAuctionService::
doAuction(std::shared_ptr<SubmittedAuctionEvent> event)
{
    stats.auctions++;
    matcher->doAuction(std::move(event));
}

void
PostAuctionService::
doEvent(std::shared_ptr<PostAuctionEvent> event)
{
    stats.events++;
    matcher->doEvent(std::move(event));
}

void
PostAuctionService::
checkExpiredAuctions()
{
    matcher->checkExpiredAuctions();
}

void
PostAuctionService::
doMatchedWinLoss(std::shared_ptr<MatchedWinLoss> event)
{
    if (event->type == MatchedWinLoss::Win) {
        lastWinLoss = Date::now();
        stats.matchedWins++;
    }
    else stats.matchedLosses++;

    event->publish(logger);
    bidder->sendWinLossMessage(*event);
}

void
PostAuctionService::
doMatchedCampaignEvent(std::shared_ptr<MatchedCampaignEvent> event)
{
    stats.matchedCampaignEvents++;

    lastCampaignEvent = Date::now();

    event->publish(logger);

    // For the moment, send the message to all of the agents that are
    // bidding on this account

    bool sent = false;
    auto onMatchingAgent = [&] (const AgentConfigEntry & entry)
        {
            if (!entry.config) return;
            bidder->sendCampaignEventMessage(entry.name, *event);
            sent = true;
        };

    const AccountKey & account = event->account;
    configListener.forEachAccountAgent(account, onMatchingAgent);

    if (!sent) {
        recordHit("delivery.%s.orphaned", event->label);
        logPAError("doCampaignEvent.noListeners" + event->label,
                "nothing listening for account " + account.toString());
    }
    else recordHit("delivery.%s.delivered", event->label);
}

void
PostAuctionService::
doUnmatched(std::shared_ptr<UnmatchedEvent> event)
{
    stats.unmatchedEvents++;
    event->publish(logger);
}

void
PostAuctionService::
doError(std::shared_ptr<PostAuctionErrorEvent> error)
{
    stats.errors++;
    error->publish(logger);
}

std::string
PostAuctionService::
getProviderClass() const
{
    return "rtbPostAuctionService";
}

MonitorIndicator
PostAuctionService::
getProviderIndicators()
    const
{
    Json::Value value;

    /* PA health check:
       - last campaign event in the last 10 seconds */
    Date now = Date::now();
    bool winLossOk = now < lastWinLoss.plusSeconds(10);
    bool campaignEventOk = now < lastCampaignEvent.plusSeconds(10);

    // Kept around for posterity.
    // Earned the "Best Error Message in RTBKIT" award.
#if 0
    if (!status)  {
      cerr << "--- WRONGNESS DETECTED:"
          << " last event: " << (now - lastCampaignEvent)
          << endl;
    }
#endif

    MonitorIndicator ind;
    ind.serviceName = serviceName();
    ind.status = winLossOk || campaignEventOk;
    ind.message = string()
        + "WinLoss pipe: " + (winLossOk ? "OK" : "ERROR") + ", "
        + "CampaignEvent pipe: " + (campaignEventOk ? "OK" : "ERROR");

    return ind;
}

/******************************************************************************/
/* STATS                                                                      */
/******************************************************************************/

PostAuctionService::Stats::
Stats() :
    auctions(0), events(0),
    matchedWins(0), matchedCampaignEvents(0), unmatchedEvents(0),
    errors(0)
{}

PostAuctionService::Stats::
Stats(const Stats& other) :
    auctions(other.auctions),
    events(other.events),

    matchedWins(other.matchedWins),
    matchedLosses(other.matchedLosses),
    matchedCampaignEvents(other.matchedCampaignEvents),
    unmatchedEvents(other.unmatchedEvents),
    errors(other.errors)
{}

auto
PostAuctionService::Stats::
operator=(const Stats& other) -> Stats&
{
    auctions = other.auctions;
    events = other.events;

    matchedWins = other.matchedWins;
    matchedLosses = other.matchedLosses;
    matchedCampaignEvents = other.matchedCampaignEvents;
    unmatchedEvents = other.unmatchedEvents;
    errors = other.errors;

    return *this;
}

auto
PostAuctionService::Stats::
operator-=(const Stats& other) -> Stats&
{
    auctions -= other.auctions;
    events -= other.events;

    matchedWins -= other.matchedWins;
    matchedLosses -= other.matchedLosses;
    matchedCampaignEvents -= other.matchedCampaignEvents;
    unmatchedEvents -= other.unmatchedEvents;
    errors -= other.errors;

    return *this;
}

} // namepsace RTBKIT
<|endoftext|>


<commit_before>/**
 * \file
 * \brief Mock of Semaphore class
 *
 * \author Copyright (C) 2017-2019 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
#define UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_

#include "unit-test-common.hpp"

#include "distortos/TickClock.hpp"

namespace distortos
{

#ifdef DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

namespace mock
{

#endif	// def DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

class Semaphore
{
public:

	using Value = unsigned int;

	Semaphore()
	{
		auto& instance = getInstanceInternal();
		REQUIRE(instance == nullptr);
		instance = this;
	}

	explicit Semaphore(const Value value, const Value maxValue = std::numeric_limits<Value>::max())
	{
		getInstance().construct(value, maxValue);
	}

	virtual ~Semaphore()
	{
		auto& instance = getInstanceInternal();
		if (instance == this)
			instance = {};
	}

	MAKE_MOCK2(construct, void(Value, Value));
	MAKE_CONST_MOCK0(getValue, Value());
	MAKE_MOCK0(post, int());
	MAKE_MOCK0(tryWait, int());
	MAKE_MOCK1(tryWaitFor, int(TickClock::duration));
	MAKE_MOCK1(tryWaitUntil, int(TickClock::time_point));
	MAKE_MOCK0(wait, int());

	static Semaphore& getInstance()
	{
		const auto instance = getInstanceInternal();
		REQUIRE(instance != nullptr);
		return *instance;
	}

private:

	static Semaphore*& getInstanceInternal()
	{
		static Semaphore* instance;
		return instance;
	}
};

#ifdef DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

}	// namespace mock

class Semaphore
{
public:

	using Value = mock::Semaphore::Value;

	constexpr explicit Semaphore(Value, Value = std::numeric_limits<Value>::max())
	{

	}

	int post()
	{
		return mock::Semaphore::getInstance().post();
	}

	int wait()
	{
		return mock::Semaphore::getInstance().wait();
	}
};

#endif	// def DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

}	// namespace distortos

#endif	// UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
<commit_msg>Add getMaxValue() to Semaphore's mock<commit_after>/**
 * \file
 * \brief Mock of Semaphore class
 *
 * \author Copyright (C) 2017-2019 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
#define UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_

#include "unit-test-common.hpp"

#include "distortos/TickClock.hpp"

namespace distortos
{

#ifdef DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

namespace mock
{

#endif	// def DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

class Semaphore
{
public:

	using Value = unsigned int;

	Semaphore()
	{
		auto& instance = getInstanceInternal();
		REQUIRE(instance == nullptr);
		instance = this;
	}

	explicit Semaphore(const Value value, const Value maxValue = std::numeric_limits<Value>::max())
	{
		getInstance().construct(value, maxValue);
	}

	virtual ~Semaphore()
	{
		auto& instance = getInstanceInternal();
		if (instance == this)
			instance = {};
	}

	MAKE_MOCK2(construct, void(Value, Value));
	MAKE_CONST_MOCK0(getMaxValue, Value());
	MAKE_CONST_MOCK0(getValue, Value());
	MAKE_MOCK0(post, int());
	MAKE_MOCK0(tryWait, int());
	MAKE_MOCK1(tryWaitFor, int(TickClock::duration));
	MAKE_MOCK1(tryWaitUntil, int(TickClock::time_point));
	MAKE_MOCK0(wait, int());

	static Semaphore& getInstance()
	{
		const auto instance = getInstanceInternal();
		REQUIRE(instance != nullptr);
		return *instance;
	}

private:

	static Semaphore*& getInstanceInternal()
	{
		static Semaphore* instance;
		return instance;
	}
};

#ifdef DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

}	// namespace mock

class Semaphore
{
public:

	using Value = mock::Semaphore::Value;

	constexpr explicit Semaphore(Value, Value = std::numeric_limits<Value>::max())
	{

	}

	int post()
	{
		return mock::Semaphore::getInstance().post();
	}

	int wait()
	{
		return mock::Semaphore::getInstance().wait();
	}
};

#endif	// def DISTORTOS_UNIT_TEST_SEMAPHOREMOCK_USE_WRAPPER

}	// namespace distortos

#endif	// UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
<|endoftext|>


<commit_before>/*
  BelaCsound.cpp:

  Copyright (C) 2017 V Lazzarini

  This file is part of Csound.

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

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

  You should have received a copy of the GNU Lesser General Public
  License along with Csound; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
  02111-1307 USA
*/
#include <Bela.h>
#include <Midi.h>
#include <csound/csound.hpp>
#include <csound/plugin.h>
#include <vector>
#include <sstream>

#define ANCHNS 8

static int OpenMidiInDevice(CSOUND *csound, void **userData, const char *dev);
static int CloseMidiInDevice(CSOUND *csound, void *userData);
static int ReadMidiData(CSOUND *csound, void *userData, unsigned char *mbuf,
			int nbytes);
static int OpenMidiOutDevice(CSOUND *csound, void **userData, const char *dev);
static int CloseMidiOutDevice(CSOUND *csound, void *userData);
static int WriteMidiData(CSOUND *csound, void *userData, const unsigned char *mbuf,
			 int nbytes);

/** DigiIn opcode
    ksig digiInBela ipin
    asig digiInBela ipin
*/
struct DigiIn : csnd::Plugin<1, 1> {
  int pin;
  int fcount;
  int frms;
  int init_done;
  BelaContext *context;

  int init() {
    pin = (int) inargs[0];
    if(pin < 0 ) pin = 0;
    if(pin > 15) pin = 15;
    context = (BelaContext *) csound->host_data();
    fcount = 0;
    init_done = 0;
    frms = context->digitalFrames;
    return OK;
  }

  int kperf() {
   if(!init_done) {
      pinMode(context,0,pin,0);
      init_done = 1;
    }
    outargs[0] = (MYFLT) digitalRead(context,fcount,pin);
    fcount += nsmps;
    fcount %= frms;
    return OK;
  }

  int aperf() {
    csnd::AudioSig out(this, outargs(0));
    int cnt = fcount;
    if(!init_done) {
      pinMode(context,0,pin,0);
      init_done = 1;
    }
    for (auto &s : out) {
      s = (MYFLT) digitalRead(context,cnt,pin);
      if(cnt == frms - 1) cnt = 0;
      else cnt++;
    }
    fcount = cnt;
    return OK;
  }
};

/** DigiOut opcode
    digiOutBela ksig,ipin
    digiOutBela asig,ipin
*/
struct DigiOut : csnd::Plugin<0, 2> {
  int pin;
  int fcount;
  int frms;
  int init_done;
  BelaContext *context;

  int init() {
    pin = (int) inargs[1];
    if(pin < 0 ) pin = 0;
    if(pin > 15) pin = 15;
    context = (BelaContext *) csound->host_data();
    init_done = 0;
    fcount = 0;
    frms = context->digitalFrames;
    return OK;
  }

  int kperf() {
    if(!init_done) {
      pinMode(context,0,pin,1);
      init_done = 1;
    }
    digitalWrite(context,fcount,pin,(inargs[0] > 0.0 ? 1 : 0));
    fcount += nsmps;
    fcount %= frms;
    return OK;
  }

  int aperf() {
    csnd::AudioSig in(this, inargs(0));
    int cnt = fcount;
    if(!init_done) {
      pinMode(context,0,pin,1);
      init_done = 1;
    }
    for (auto s : in) {
      digitalWriteOnce(context,cnt,pin, (s > 0.0 ? 1 : 0));
      if(cnt == frms - 1) cnt = 0;
      else cnt++;
    }
    fcount = cnt;
    return OK;
  }
};

struct CsChan {
  std::vector<MYFLT> samples;
  std::stringstream name;
};

struct CsData {
  Csound *csound;
  int blocksize;
  int res;
  int count;
  CsChan channel[ANCHNS];
  CsChan ochannel[ANCHNS];
};

static CsData gCsData;
static Midi gMidi;

bool setup(BelaContext *context, void *Data)
{
  Csound *csound;
  const char *csdfile = "my.csd"; /* CSD name */
  const char *args[] = { "csound", csdfile, "-iadc", "-odac", "-+rtaudio=null",
			 "--realtime", "--daemon"};
  int numArgs = (int) (sizeof(args)/sizeof(char *));

  if(context->audioInChannels != context->audioOutChannels) {
    printf("Error: number of audio inputs != number of audio outputs.\n");
    return false;
  }

  if(context->analogInChannels != context->analogOutChannels) {
    printf("Error: number of analog inputs != number of analog outputs.\n");
    return false;
  }

  /* set up Csound */
  csound = new Csound();
  gCsData.csound = csound;
  csound->SetHostData((void *) context);
  csound->SetHostImplementedAudioIO(1,0);
  csound->SetHostImplementedMIDIIO(1);
  csound->SetExternalMidiInOpenCallback(OpenMidiInDevice);
  csound->SetExternalMidiReadCallback(ReadMidiData);
  csound->SetExternalMidiInCloseCallback(CloseMidiInDevice);
  csound->SetExternalMidiOutOpenCallback(OpenMidiOutDevice);
  csound->SetExternalMidiWriteCallback(WriteMidiData);
  csound->SetExternalMidiOutCloseCallback(CloseMidiOutDevice);
  /* set up digi opcodes */
  if(csnd::plugin<DigiIn>((csnd::Csound *) csound->GetCsound(), "digiInBela",
			  "k","i", csnd::thread::ik) != 0)
    printf("Warning: could not add digiInBela k-rate opcode\n");
  if(csnd::plugin<DigiIn>((csnd::Csound *) csound->GetCsound(), "digiInBela",
			  "a", "i", csnd::thread::ia) != 0)
    printf("Warning: could not add digiInBela a-rate opcode\n");
  if(csnd::plugin<DigiOut>((csnd::Csound *) csound->GetCsound(), "digiOutBela" ,
			   "", "ki", csnd::thread::ik) != 0)
    printf("Warning: could not add digiOutBela k-rate opcode\n");
  if(csnd::plugin<DigiOut>((csnd::Csound *) csound->GetCsound(), "digiOutBela" ,
			   "", "ai", csnd::thread::ia) != 0)
    printf("Warning: could not add digiOutBela a-rate opcode\n");

  /* compile CSD */
  if((gCsData.res = csound->Compile(numArgs, args)) != 0) {
    printf("Error: Csound could not compile CSD file.\n");
    return false;
  }
  gCsData.blocksize = csound->GetKsmps()*csound->GetNchnls();
  gCsData.count = 0;

  /* set up the channels */
  for(int i=0; i < ANCHNS; i++) {
    gCsData.channel[i].samples.resize(csound->GetKsmps());
    gCsData.channel[i].name << "analogIn" << i;
    gCsData.ochannel[i].samples.resize(csound->GetKsmps());
    gCsData.ochannel[i].name << "analogOut" << i;
  }

  return true;
}

void render(BelaContext *context, void *Data)
{
  if(gCsData.res == 0) {
    int n,i,k,count, frmcount,blocksize,res = gCsData.res;
    Csound *csound = gCsData.csound;
    MYFLT scal = csound->Get0dBFS();
    MYFLT* audioIn = csound->GetSpin();
    MYFLT* audioOut = csound->GetSpout();
    int nchnls = csound->GetNchnls();
    int chns = nchnls < context->audioOutChannels ?
      nchnls : context->audioOutChannels;
    int an_chns = context->analogInChannels > ANCHNS ?
      ANCHNS : context->analogInChannels;
    CsChan *channel = gCsData.channel;
    CsChan *ochannel = gCsData.ochannel;
    float frm = 0.f, incr =
      ((float) context->analogFrames)/context->audioFrames;
    count = gCsData.count;
    blocksize = gCsData.blocksize;

    /* processing loop */
    for(n = 0; n < context->audioFrames; n++, frm+=incr, count+=nchnls){
      if(count == blocksize) {
	/* set the channels */
	for(i = 0; i < an_chns; i++) {
          csound->SetChannel(channel[i].name.str().c_str(),
			     channel[i].samples.data());
	  csound->GetAudioChannel(ochannel[i].name.str().c_str(),
				  ochannel[i].samples.data());
	}
	/* run csound */
	if((res = csound->PerformKsmps()) == 0) count = 0;
	else break;
      }
      /* read/write audio data */
      for(i = 0; i < chns; i++){
	audioIn[count+i] = audioRead(context,n,i)*scal;
	audioWrite(context,n,i,audioOut[count+i]/scal);
      }
      /* read analogue data
         analogue frame pos frm gets incremented according to the
         ratio analogFrames/audioFrames.
      */
      frmcount = count/nchnls;
      for(i = 0; i < an_chns; i++) {
	k = (int) frm;
        channel[i].samples[frmcount] = analogRead(context,k,i);
	analogWriteOnce(context,k,i,ochannel[i].samples[frmcount]);
      }
    }
    gCsData.res = res;
    gCsData.count = count;
  }
}

void cleanup(BelaContext *context, void *Data)
{
  delete gCsData.csound;
}

/** MIDI functions
 */
int OpenMidiInDevice(CSOUND *csound, void **userData, const char *dev) {
  if(gMidi.readFrom(dev) == 1) {
    gMidi.enableParser(false);
    *userData = (void *) &gMidi;
    return 0;
  }
  csoundMessage(csound, "Could not open Midi in device %s", dev);
  return -1;
}

int CloseMidiInDevice(CSOUND *csound, void *userData) {
  return 0;
}

int ReadMidiData(CSOUND *csound, void *userData,
		 unsigned char *mbuf, int nbytes) {
  int n = 0, byte;
  if(userData) {
    Midi *midi = (Midi *) userData;
    while((byte = midi->getInput()) >= 0) {
      *mbuf++ = (unsigned char) byte;
      if(++n == nbytes) break;
    }
    return n;
  }
  return 0;
}

int OpenMidiOutDevice(CSOUND *csound, void **userData, const char *dev) {
  if(gMidi.writeTo(dev) == 1) {
    gMidi.enableParser(false);
    *userData = (void *) &gMidi;
    return 0;
  }
  csoundMessage(csound, "Could not open Midi out device %s", dev);
  return -1;
}

int CloseMidiOutDevice(CSOUND *csound, void *userData) {
  return 0;
}

int WriteMidiData(CSOUND *csound, void *userData,
		  const unsigned char *mbuf, int nbytes) {
  if(userData) {
    Midi *midi = (Midi *) userData;
    if(midi->writeOutput((midi_byte_t *)mbuf, nbytes) > 0) return nbytes;
    return 0;
  }
  return 0;
}
<commit_msg>bela scope<commit_after>/*
  BelaCsound.cpp:

  Copyright (C) 2017 V Lazzarini

  This file is part of Csound.

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

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

  You should have received a copy of the GNU Lesser General Public
  License along with Csound; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
  02111-1307 USA
*/
#include <Bela.h>
#include <Midi.h>
#include <Scope.h>
#include <csound/csound.hpp>
#include <csound/plugin.h>
#include <vector>
#include <sstream>

#define ANCHNS 8

static int OpenMidiInDevice(CSOUND *csound, void **userData, const char *dev);
static int CloseMidiInDevice(CSOUND *csound, void *userData);
static int ReadMidiData(CSOUND *csound, void *userData, unsigned char *mbuf,
			int nbytes);
static int OpenMidiOutDevice(CSOUND *csound, void **userData, const char *dev);
static int CloseMidiOutDevice(CSOUND *csound, void *userData);
static int WriteMidiData(CSOUND *csound, void *userData, const unsigned char *mbuf,
			 int nbytes);

/** DigiIn opcode
    ksig digiInBela ipin
    asig digiInBela ipin
*/
struct DigiIn : csnd::Plugin<1, 1> {
  int pin;
  int fcount;
  int frms;
  int init_done;
  BelaContext *context;

  int init() {
    pin = (int) inargs[0];
    if(pin < 0 ) pin = 0;
    if(pin > 15) pin = 15;
    context = (BelaContext *) csound->host_data();
    fcount = 0;
    init_done = 0;
    frms = context->digitalFrames;
    return OK;
  }

  int kperf() {
   if(!init_done) {
      pinMode(context,0,pin,0);
      init_done = 1;
    }
    outargs[0] = (MYFLT) digitalRead(context,fcount,pin);
    fcount += nsmps;
    fcount %= frms;
    return OK;
  }

  int aperf() {
    csnd::AudioSig out(this, outargs(0));
    int cnt = fcount;
    if(!init_done) {
      pinMode(context,0,pin,0);
      init_done = 1;
    }
    for (auto &s : out) {
      s = (MYFLT) digitalRead(context,cnt,pin);
      if(cnt == frms - 1) cnt = 0;
      else cnt++;
    }
    fcount = cnt;
    return OK;
  }
};

/** DigiOut opcode
    digiOutBela ksig,ipin
    digiOutBela asig,ipin
*/
struct DigiOut : csnd::Plugin<0, 2> {
  int pin;
  int fcount;
  int frms;
  int init_done;
  BelaContext *context;

  int init() {
    pin = (int) inargs[1];
    if(pin < 0 ) pin = 0;
    if(pin > 15) pin = 15;
    context = (BelaContext *) csound->host_data();
    init_done = 0;
    fcount = 0;
    frms = context->digitalFrames;
    return OK;
  }

  int kperf() {
    if(!init_done) {
      pinMode(context,0,pin,1);
      init_done = 1;
    }
    digitalWrite(context,fcount,pin,(inargs[0] > 0.0 ? 1 : 0));
    fcount += nsmps;
    fcount %= frms;
    return OK;
  }

  int aperf() {
    csnd::AudioSig in(this, inargs(0));
    int cnt = fcount;
    if(!init_done) {
      pinMode(context,0,pin,1);
      init_done = 1;
    }
    for (auto s : in) {
      digitalWriteOnce(context,cnt,pin, (s > 0.0 ? 1 : 0));
      if(cnt == frms - 1) cnt = 0;
      else cnt++;
    }
    fcount = cnt;
    return OK;
  }
};

struct CsChan {
  std::vector<MYFLT> samples;
  std::stringstream name;
};

struct CsData {
  Csound *csound;
  int blocksize;
  int res;
  int count;
  CsChan channel[ANCHNS];
  CsChan ochannel[ANCHNS];
  CsChan schannel;
};

static CsData gCsData;
static Midi gMidi;

bool setup(BelaContext *context, void *Data)
{
  Csound *csound;
  const char *csdfile = "my.csd"; /* CSD name */
  const char *args[] = { "csound", csdfile, "-iadc", "-odac", "-+rtaudio=null",
			 "--realtime", "--daemon"};
  int numArgs = (int) (sizeof(args)/sizeof(char *));

  if(context->audioInChannels != context->audioOutChannels) {
    printf("Error: number of audio inputs != number of audio outputs.\n");
    return false;
  }

  if(context->analogInChannels != context->analogOutChannels) {
    printf("Error: number of analog inputs != number of analog outputs.\n");
    return false;
  }

  /* set up Csound */
  csound = new Csound();
  gCsData.csound = csound;
  csound->SetHostData((void *) context);
  csound->SetHostImplementedAudioIO(1,0);
  csound->SetHostImplementedMIDIIO(1);
  csound->SetExternalMidiInOpenCallback(OpenMidiInDevice);
  csound->SetExternalMidiReadCallback(ReadMidiData);
  csound->SetExternalMidiInCloseCallback(CloseMidiInDevice);
  csound->SetExternalMidiOutOpenCallback(OpenMidiOutDevice);
  csound->SetExternalMidiWriteCallback(WriteMidiData);
  csound->SetExternalMidiOutCloseCallback(CloseMidiOutDevice);
  /* set up digi opcodes */
  if(csnd::plugin<DigiIn>((csnd::Csound *) csound->GetCsound(), "digiInBela",
			  "k","i", csnd::thread::ik) != 0)
    printf("Warning: could not add digiInBela k-rate opcode\n");
  if(csnd::plugin<DigiIn>((csnd::Csound *) csound->GetCsound(), "digiInBela",
			  "a", "i", csnd::thread::ia) != 0)
    printf("Warning: could not add digiInBela a-rate opcode\n");
  if(csnd::plugin<DigiOut>((csnd::Csound *) csound->GetCsound(), "digiOutBela" ,
			   "", "ki", csnd::thread::ik) != 0)
    printf("Warning: could not add digiOutBela k-rate opcode\n");
  if(csnd::plugin<DigiOut>((csnd::Csound *) csound->GetCsound(), "digiOutBela" ,
			   "", "ai", csnd::thread::ia) != 0)
    printf("Warning: could not add digiOutBela a-rate opcode\n");

  /* compile CSD */
  if((gCsData.res = csound->Compile(numArgs, args)) != 0) {
    printf("Error: Csound could not compile CSD file.\n");
    return false;
  }
  gCsData.blocksize = csound->GetKsmps()*csound->GetNchnls();
  gCsData.count = 0;

  /* set up the channels */
  for(int i=0; i < ANCHNS; i++) {
    gCsData.channel[i].samples.resize(csound->GetKsmps());
    gCsData.channel[i].name << "analogIn" << i;
    gCsData.ochannel[i].samples.resize(csound->GetKsmps());
    gCsData.ochannel[i].name << "analogOut" << i;
  }

  gCsData.schannel.samples.resize(csound->GetKsmps());
  gCsData.schannel.name << "scope";
  scope.setup(1, context->audioSampleRate);

  return true;
}

void render(BelaContext *context, void *Data)
{
  if(gCsData.res == 0) {
    int n,i,k,count, frmcount,blocksize,res = gCsData.res;
    Csound *csound = gCsData.csound;
    MYFLT scal = csound->Get0dBFS();
    MYFLT* audioIn = csound->GetSpin();
    MYFLT* audioOut = csound->GetSpout();
    int nchnls = csound->GetNchnls();
    int chns = nchnls < context->audioOutChannels ?
      nchnls : context->audioOutChannels;
    int an_chns = context->analogInChannels > ANCHNS ?
      ANCHNS : context->analogInChannels;
    CsChan *channel = gCsData.channel;
    CsChan *ochannel = gCsData.ochannel;
    CsChan &schannel = gCsData.scope;
    float frm = 0.f, incr =
      ((float) context->analogFrames)/context->audioFrames;
    count = gCsData.count;
    blocksize = gCsData.blocksize;

    /* processing loop */
    for(n = 0; n < context->audioFrames; n++, frm+=incr, count+=nchnls){
      if(count == blocksize) {
	/* set the channels */
	for(i = 0; i < an_chns; i++) {
          csound->SetChannel(channel[i].name.str().c_str(),
			     channel[i].samples.data());
	  csound->GetAudioChannel(ochannel[i].name.str().c_str(),
				  ochannel[i].samples.data());
	}
        /* get the scope data */
        csound->GetAudioChannel(schannel.name.str().c_str(),
				  schannel.samples.data());
	/* run csound */
	if((res = csound->PerformKsmps()) == 0) count = 0;
	else break;
      }
      /* read/write audio data */
      for(i = 0; i < chns; i++){
	audioIn[count+i] = audioRead(context,n,i)*scal;
	audioWrite(context,n,i,audioOut[count+i]/scal);
      }
      /* read analogue data
         analogue frame pos frm gets incremented according to the
         ratio analogFrames/audioFrames.
      */
      frmcount = count/nchnls;
      for(i = 0; i < an_chns; i++) {
	k = (int) frm;
        channel[i].samples[frmcount] = analogRead(context,k,i);
	analogWriteOnce(context,k,i,ochannel[i].samples[frmcount]);
      }
      scope.log(schannel.samples[frmcount]);
    }
    gCsData.res = res;
    gCsData.count = count;
  }
}

void cleanup(BelaContext *context, void *Data)
{
  delete gCsData.csound;
}

/** MIDI functions
 */
int OpenMidiInDevice(CSOUND *csound, void **userData, const char *dev) {
  if(gMidi.readFrom(dev) == 1) {
    gMidi.enableParser(false);
    *userData = (void *) &gMidi;
    return 0;
  }
  csoundMessage(csound, "Could not open Midi in device %s", dev);
  return -1;
}

int CloseMidiInDevice(CSOUND *csound, void *userData) {
  return 0;
}

int ReadMidiData(CSOUND *csound, void *userData,
		 unsigned char *mbuf, int nbytes) {
  int n = 0, byte;
  if(userData) {
    Midi *midi = (Midi *) userData;
    while((byte = midi->getInput()) >= 0) {
      *mbuf++ = (unsigned char) byte;
      if(++n == nbytes) break;
    }
    return n;
  }
  return 0;
}

int OpenMidiOutDevice(CSOUND *csound, void **userData, const char *dev) {
  if(gMidi.writeTo(dev) == 1) {
    gMidi.enableParser(false);
    *userData = (void *) &gMidi;
    return 0;
  }
  csoundMessage(csound, "Could not open Midi out device %s", dev);
  return -1;
}

int CloseMidiOutDevice(CSOUND *csound, void *userData) {
  return 0;
}

int WriteMidiData(CSOUND *csound, void *userData,
		  const unsigned char *mbuf, int nbytes) {
  if(userData) {
    Midi *midi = (Midi *) userData;
    if(midi->writeOutput((midi_byte_t *)mbuf, nbytes) > 0) return nbytes;
    return 0;
  }
  return 0;
}
<|endoftext|>