text:
#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <crest/util/eigen_extensions.hpp>
#include <util/eigen_matchers.hpp>

#include <Eigen/Sparse>

using ::testing::Eq;

class sparse_submatrix_test : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        zero_10x10 = Eigen::SparseMatrix<int>(10, 10);

        diagonal_10x10 = Eigen::SparseMatrix<int>(10, 10);
        for (int i = 0; i < 10; ++i)
        {
            diagonal_10x10.insert(i, i) = i;
        }

        dense_5x5 = Eigen::SparseMatrix<int>(5, 5);
        for (int i = 0; i < 5; ++i)
        {
            for (int j = 0; j < 5; ++j)
            {
                dense_5x5.insert(i, j) = j + 5 * i;
            }
        }
    }

    Eigen::SparseMatrix<int> zero_10x10;
    Eigen::SparseMatrix<int> diagonal_10x10;
    Eigen::SparseMatrix<int> dense_5x5;
};

class submatrix_test : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        dense_5x5 << 0, 1, 2, 3, 4,
                5, 6, 7, 8, 9,
                10, 11, 12, 13, 14,
                15, 16, 17, 18, 19,
                20, 21, 22, 23, 24;
    }

    Eigen::Matrix<int, 5, 5> dense_5x5;
};

TEST_F(sparse_submatrix_test, zero_10x10)
{
    const auto rows = std::vector<int> { 3, 5, 7 };
    const auto cols = std::vector<int> { 0, 9 };
    const auto mat = sparse_submatrix(zero_10x10, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(2));
    EXPECT_THAT(mat.nonZeros(), Eq(0));
}

TEST_F(sparse_submatrix_test, diagonal_10x10)
{
    const auto rows = std::vector<int> { 3, 5, 7 };
    const auto cols = std::vector<int> { 3, 4, 5 };
    const auto mat = sparse_submatrix(diagonal_10x10, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(3));
    EXPECT_THAT(mat.nonZeros(), Eq(2));

    EXPECT_THAT(mat.coeff(0, 0), Eq(3));
    EXPECT_THAT(mat.coeff(1, 2), Eq(5));
}

TEST_F(sparse_submatrix_test, dense_5x5_arbitrary_indices)
{
    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = sparse_submatrix(dense_5x5, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(3));
    EXPECT_THAT(mat.cols(), Eq(2));
    EXPECT_THAT(mat.nonZeros(), Eq(6));

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(Eigen::MatrixXi(mat).isApprox(expected));
}

TEST_F(sparse_submatrix_test, dense_5x5_all_indices)
{
    const auto rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto mat = sparse_submatrix(dense_5x5, rows, cols);

    EXPECT_THAT(mat.rows(), Eq(5));
    EXPECT_THAT(mat.cols(), Eq(5));
    EXPECT_THAT(mat.nonZeros(), Eq(25));

    const Eigen::MatrixXi expected = Eigen::MatrixXi(dense_5x5);

    EXPECT_TRUE(Eigen::MatrixXi(mat).isApprox(expected));
}

TEST(sparse_submatrix_as_dense_test, dense_5x5_arbitrary_indices)
{
    auto dense_5x5 = Eigen::SparseMatrix<int>(5, 5);
    for (int i = 0; i < 5; ++i) {
        for (int j = 0; j < 5; ++j) {
            dense_5x5.insert(i, j) = j + 5 * i;
        }
    }

    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = sparse_submatrix_as_dense(dense_5x5, rows, cols);

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(mat == expected);
}

TEST_F(submatrix_test, dense_5x5)
{
    const auto rows = std::vector<int> { 1, 2, 4 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto mat = submatrix(dense_5x5, rows, cols);

    Eigen::Matrix<int, 3, 2> expected;
    expected <<
             7, 9,
            12, 14,
            22, 24;

    EXPECT_TRUE(mat.isApprox(expected));
}

TEST_F(submatrix_test, dense_5x5_all_indices)
{
    const auto rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto mat = submatrix(dense_5x5, rows, cols);

    const Eigen::MatrixXi expected = Eigen::MatrixXi(dense_5x5);

    EXPECT_TRUE(mat.isApprox(expected));
}

TEST_F(sparse_submatrix_test, sparsity_pattern_zero_10x10) {
    const auto reverse_rows = std::vector<int> { -1, -1, -1, 0, -1, 1, -1, 2, -1, -1, -1};
    const auto cols = std::vector<int> { 0, 9 };
    const auto pattern = detail::submatrix_sparsity_pattern(zero_10x10, reverse_rows, cols);

    EXPECT_TRUE(pattern.isZero());
    EXPECT_THAT(pattern.rows(), Eq(2));
}

TEST_F(sparse_submatrix_test, sparsity_pattern_diagonal_10x10) {
    const auto reverse_rows = std::vector<int> { -1, -1, -1, 0, -1, 1, -1, 2, -1, -1, -1};
    const auto cols = std::vector<int> { 3, 4, 5 };
    const auto pattern = detail::submatrix_sparsity_pattern(diagonal_10x10, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(3));
    EXPECT_THAT(pattern(0), 1);
    EXPECT_THAT(pattern(1), 0);
    EXPECT_THAT(pattern(2), 1);
}

TEST_F(sparse_submatrix_test, sparsity_pattern_dense_5x5_arbitrary_indices)
{
    const auto reverse_rows = std::vector<int> { -1, 0, 0, -1, 2 };
    const auto cols = std::vector<int> { 2, 4 };
    const auto pattern = detail::submatrix_sparsity_pattern(dense_5x5, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(2));
    EXPECT_THAT(pattern(0), 3);
    EXPECT_THAT(pattern(1), 3);
}

TEST_F(sparse_submatrix_test, sparsity_pattern_dense_5x5_all_indices)
{
    const auto reverse_rows = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto cols = std::vector<int> { 0, 1, 2, 3, 4 };
    const auto pattern = detail::submatrix_sparsity_pattern(dense_5x5, reverse_rows, cols);

    EXPECT_THAT(pattern.rows(), Eq(5));
    EXPECT_THAT(pattern(0), Eq(5));
    EXPECT_THAT(pattern(1), Eq(5));
    EXPECT_THAT(pattern(2), Eq(5));
    EXPECT_THAT(pattern(3), Eq(5));
    EXPECT_THAT(pattern(4), Eq(5));
}

max_stars_repo_name: Andlon/crest

max_issues_repo_name: Andlon/crest

max_forks_repo_name: Andlon/crest


text:
/*
 * File: classad-plugin-loader.cpp
 * Author: Monforte Salvatore
 * For license conditions see http://www.eu-datagrid.org/license.html
 */

// $Id: classad_plugin_loader.cpp,v 1.1.2.1 2012/09/11 10:19:40 mcecchi Exp $

#include <iostream>
#include <boost/thread/once.hpp>

#include "classad_plugin_loader.h"
#include <classad_distribution.h>
#include <fnCall.h>

using namespace classad;
using namespace std;

namespace glite {
namespace wms {
namespace classad_plugin {

extern bool doMatch(const char *, const ArgumentList&, EvalState&, Value&);
extern bool listAttrRegEx(const char *, const ArgumentList&, EvalState&, Value&);
extern bool retrieveCloseSEsInfo(const char *, const ArgumentList&, EvalState&, Value&);
extern bool testMemberEx(const char *, const ArgumentList&, EvalState&, Value&);
extern bool successFraction(const char *, const ArgumentList&, EvalState&, Value&);

static ClassAdFunctionMapping functions[] =
{
  { "fqanMember", (void *)testMemberEx, 0 },
  { "allMatch",      (void *)doMatch,   0 },
  { "anyMatch",      (void *)doMatch,   0 },
  { "",            NULL,                 0 }
};

namespace {

boost::once_flag f_once = BOOST_ONCE_INIT;

void load()
{
  //classad::FunctionCall::RegisterFunctions(functions);
  std::string name("fqanMember");
  classad::FunctionCall::RegisterFunction(name, testMemberEx);
  name = "allMatch";
  classad::FunctionCall::RegisterFunction(name, doMatch);
  name = "anyMatch";
  classad::FunctionCall::RegisterFunction(name, doMatch);
  name = "whichMatch";
  classad::FunctionCall::RegisterFunction(name, doMatch);
  name = "listAttrRegEx";
  classad::FunctionCall::RegisterFunction(name, listAttrRegEx);
  name = "retrieveCloseSEsInfo";
  classad::FunctionCall::RegisterFunction(name, retrieveCloseSEsInfo);
  name = "successFraction";
  classad::FunctionCall::RegisterFunction(name, successFraction);
}

}

classad_plugin_loader::classad_plugin_loader()
{
  boost::call_once(load, f_once);
}

} // namespace classad_plugin
} // namespace wms
} // namespace glite

ext: cpp

max_stars_repo_name: italiangrid/wms

max_issues_repo_name: italiangrid/wms

max_forks_repo_name: italiangrid/wms


text:
#include <mlpack/core.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/amf/update_rules/svd_incomplete_incremental_learning.hpp>
#include <mlpack/methods/amf/update_rules/svd_complete_incremental_learning.hpp>
#include <mlpack/methods/amf/init_rules/random_init.hpp>
#include <mlpack/methods/amf/termination_policies/incomplete_incremental_termination.hpp>
#include <mlpack/methods/amf/termination_policies/complete_incremental_termination.hpp>
#include <mlpack/methods/amf/termination_policies/simple_tolerance_termination.hpp>
#include <mlpack/methods/amf/termination_policies/validation_RMSE_termination.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(SVDIncrementalTest);

using namespace std;
using namespace mlpack;
using namespace mlpack::amf;
using namespace arma;

/**
 * Test for convergence of incomplete incremenal learning
 */
BOOST_AUTO_TEST_CASE(SVDIncompleteIncrementalConvergenceTest)
{
  sp_mat data;
  data.sprandn(1000, 1000, 0.2);

  SVDIncompleteIncrementalLearning svd(0.01);
  IncompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> > iit;

  AMF<IncompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> >,
      RandomInitialization,
      SVDIncompleteIncrementalLearning> amf(iit, RandomInitialization(), svd);

  mat m1,m2;
  amf.Apply(data, 2, m1, m2);

  BOOST_REQUIRE_NE(amf.TerminationPolicy().Iteration(),
                   amf.TerminationPolicy().MaxIterations());
}

/**
 * Test for convergence of complete incremenal learning
 */
BOOST_AUTO_TEST_CASE(SVDCompleteIncrementalConvergenceTest)
{
  sp_mat data;
  data.sprandn(1000, 1000, 0.2);

  SVDCompleteIncrementalLearning<sp_mat> svd(0.01);
  CompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> > iit;

  AMF<CompleteIncrementalTermination<SimpleToleranceTermination<sp_mat> >,
      RandomInitialization,
      SVDCompleteIncrementalLearning<sp_mat> > amf(iit,
                                                   RandomInitialization(),
                                                   svd);
  mat m1,m2;
  amf.Apply(data, 2, m1, m2);

  BOOST_REQUIRE_NE(amf.TerminationPolicy().Iteration(),
                   amf.TerminationPolicy().MaxIterations());
}

//! This is used to ensure we start from the same initial point.
class SpecificRandomInitialization
{
 public:
  SpecificRandomInitialization(const size_t n, const size_t r, const size_t m) :
      W(arma::randu<arma::mat>(n, r)),
      H(arma::randu<arma::mat>(r, m)) { }

  template<typename MatType>
  inline void Initialize(const MatType& /* V */,
                         const size_t /* r */,
                         arma::mat& W,
                         arma::mat& H)
  {
    W = this->W;
    H = this->H;
  }

 private:
  arma::mat W;
  arma::mat H;
};

BOOST_AUTO_TEST_CASE(SVDIncompleteIncrementalRegularizationTest)
{
  mat dataset;
  data::Load("GroupLens100k.csv", dataset);

  // Generate list of locations for batch insert constructor for sparse
  // matrices.
  arma::umat locations(2, dataset.n_cols);
  arma::vec values(dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; ++i)
  {
    // We have to transpose it because items are rows, and users are columns.
    locations(0, i) = ((arma::uword) dataset(0, i));
    locations(1, i) = ((arma::uword) dataset(1, i));
    values(i) = dataset(2, i);
  }

  // Find maximum user and item IDs.
  const size_t maxUserID = (size_t) max(locations.row(0)) + 1;
  const size_t maxItemID = (size_t) max(locations.row(1)) + 1;

  // Fill sparse matrix.
  sp_mat cleanedData = arma::sp_mat(locations, values, maxUserID, maxItemID);
  sp_mat cleanedData2 = cleanedData;

  SpecificRandomInitialization sri(cleanedData.n_rows, 2, cleanedData.n_cols);

  ValidationRMSETermination<sp_mat> vrt(cleanedData, 2000);
  AMF<IncompleteIncrementalTermination<ValidationRMSETermination<sp_mat> >,
      SpecificRandomInitialization,
      SVDIncompleteIncrementalLearning> amf1(vrt, sri,
      SVDIncompleteIncrementalLearning(0.001, 0, 0));

  mat m1, m2;
  double regularRMSE = amf1.Apply(cleanedData, 2, m1, m2);

  ValidationRMSETermination<sp_mat> vrt2(cleanedData2, 2000);
  AMF<IncompleteIncrementalTermination<ValidationRMSETermination<sp_mat> >,
      SpecificRandomInitialization,
      SVDIncompleteIncrementalLearning> amf2(vrt2, sri,
      SVDIncompleteIncrementalLearning(0.001, 0.01, 0.01));

  mat m3, m4;
  double regularizedRMSE = amf2.Apply(cleanedData2, 2, m3, m4);

  BOOST_REQUIRE_LT(regularizedRMSE, regularRMSE + 0.075);
}

BOOST_AUTO_TEST_SUITE_END();


text:
#include <librealsense2/rs.hpp>
#include <algorithm>            // std::min, std::max
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <iostream>
#include <chrono>

#include <pcl/registration/icp.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <string>

typedef pcl::PointXYZRGB P_pcl;
typedef pcl::PointCloud<P_pcl> point_cloud;
typedef point_cloud::Ptr ptr_cloud;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

void print4x4Matrix (const Eigen::Matrix4d & matrix)
{
    printf ("Rotation matrix :\n");
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
    printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
    printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
    printf ("Translation vector :\n");
    printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}

inline void pass_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0,9.0);
    pass.filter(*cloud);
}

inline void planeDetect(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, double threshold) {
    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    seg.setOptimizeCoefficients(true);
    seg.setInputCloud(cloud);
    seg.setModelType(pcl::SACMODEL_PLANE);//モデル
    seg.setMethodType(pcl::SAC_RANSAC);//検出手法
    seg.setDistanceThreshold(threshold); //閾値 0.5とか
    seg.segment(*inliers, *coefficients);
}

inline void planeRemoval(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointIndices::Ptr inliers, bool negative) {
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(negative);
    extract.filter(*cloud);
}

// Get RGB values based on normals - texcoords, normals value [u v]
std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
{
    const int w = texture.get_width(), h = texture.get_height();

    // convert normals [u v] to basic coords [x y]
    int x = std::min(std::max(int(texcoords.u*w + .5f), 0), w - 1);
    int y = std::min(std::max(int(texcoords.v*h + .5f), 0), h - 1);

    int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
    const auto texture_data = reinterpret_cast<const uint8_t*>(texture.get_data());
    return std::tuple<uint8_t, uint8_t, uint8_t>(texture_data[idx], texture_data[idx+1], texture_data[idx+2]);
}

auto points_to_pcl(const rs2::points& points, const rs2::video_frame& color){

    // OpenCV Mat for showing the rgb color image, just as part of processing
    cv::Mat colorr(cv::Size(640, 480), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP);
    //namedWindow("Display Image", WINDOW_AUTOSIZE );
    //imshow("Display Image", colorr);

    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    ptr_cloud cloud(new point_cloud);

    // Config of PCL Cloud object
    cloud->width = static_cast<uint32_t>(sp.width());
    cloud->height = static_cast<uint32_t>(sp.height());
    cloud->is_dense = false;
    cloud->points.resize(points.size());

    auto tex_coords = points.get_texture_coordinates();
    auto vertices = points.get_vertices();

    // Iterating through all points and setting XYZRGB coordinates
    // and RGB values
    for (int i = 0; i < points.size(); ++i)
    {
        cloud->points[i].x = vertices[i].x;
        cloud->points[i].y = vertices[i].y;
        cloud->points[i].z = vertices[i].z;

        std::tuple<uint8_t, uint8_t, uint8_t> current_color;
        current_color = get_texcolor(color, tex_coords[i]);

        // Reversed order- 2-1-0 because of BGR model used in camera
        cloud->points[i].r = std::get<2>(current_color);
        cloud->points[i].g = std::get<1>(current_color);
        cloud->points[i].b = std::get<0>(current_color);

    }

   return cloud;
}

int main() {
    FILE *fp;
    int count= 0;
    const int markers_x         = 3;   // x方向に配置するマーカー数
    const int markers_y         = 2;   // y方向に配置するマーカー数
    const float marker_length     = 0.2; // マーカーのサイズ
    const float marker_separation = 0.07;  // マーカー間の間隔

    float degree = 180/3.141592;
    int wide = 1280;
    int height = 720;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB> Final;
    pcl::io::loadPLYFile("model.ply", *model);
    //aruco
    cv::Mat camera_matrix, dist_coeffs;
    std::ostringstream vector_to_marker;
    float actual_marker_length = 0.15;
    //cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50);
    cv::FileStorage fs("calibration_params.yml", cv::FileStorage::READ);
    std::vector<int> ids;
    std::vector<std::vector<cv::Point2f> > corners;
    fs["camera_matrix"] >> camera_matrix;
    fs["distortion_coefficients"] >> dist_coeffs;

    cv::Ptr<cv::aruco::GridBoard> grid_board = cv::aruco::GridBoard::create(markers_x, markers_y, marker_length, marker_separation,dictionary);
    cv::Ptr<cv::aruco::Board> board = grid_board.staticCast<cv::aruco::Board>();
    std::cout << "camera_matrix\n" << camera_matrix << std::endl;
    std::cout << "\ndist coeffs\n" << dist_coeffs << std::endl;

    rs2::pointcloud pc;
    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Create a configuration for configuring the pipeline with a non default profile
    rs2::config cfg;

    // Add desired streams to configuration
    cfg.enable_stream(RS2_STREAM_COLOR, wide, height, RS2_FORMAT_BGR8, 30);
    //cfg.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
    cfg.enable_stream(RS2_STREAM_DEPTH, wide, height, RS2_FORMAT_Z16, 30);

    // Start streaming with default recommended configuration
    pipe.start(cfg);

    std::chrono::system_clock::time_point start, end;
    while(1){

        start = std::chrono::system_clock::now();
    auto frames = pipe.wait_for_frames();
    auto depth = frames.get_depth_frame();
    auto colored_frame = frames.get_color_frame();

    // Order here is crucial!
    // map_to() color frame has to be done befor point calculation
    // otherwise texture won't be mapped

    //pc.map_to(colored_frame);
    auto points = pc.calculate(depth);
    // Actual calling of conversion and saving XYZRGBRGB cloud to file

    ptr_cloud cloud = points_to_pcl(points, colored_frame);
    std::cout << cloud->empty() << std::endl;

    if(cloud->size() != false) {

        cv::Mat image(cv::Size(wide, height), CV_8UC3, (void*)colored_frame.get_data(), cv::Mat::AUTO_STEP);
        cv::aruco::detectMarkers(image, dictionary, corners, ids);
        if (ids.size() > 0)
        {
            cv::aruco::drawDetectedMarkers(image, corners, ids);
            //std::vector<cv::Vec3d> rvecs, tvecs;
            cv::Vec3d rvecs, tvecs;
            //cv::aruco::estimatePoseSingleMarkers(corners, actual_marker_length, camera_matrix, dist_coeffs, rvecs, tvecs);
            // draw axis for each marker
            for(int i=0; i < ids.size(); i++)
            {
                cv::aruco::estimatePoseBoard(corners,ids,board,camera_matrix,dist_coeffs,rvecs,tvecs);
                cv::aruco::drawAxis(image, camera_matrix, dist_coeffs, rvecs, tvecs, 0.1);
                //cv::aruco::drawAxis(image, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.1);
               // std::cout <<"x: " << rvecs[i][0]*degree << " y: " << rvecs[i][1]*degree << " z: "<< rvecs[i][2]*degree <<std::endl;
                 std::cout <<"x: " << rvecs[0]*degree << " y: " << rvecs[1]*degree << " z: "<< rvecs[2]*degree <<std::endl;
                if((fp=fopen("save.csv","a"))!=NULL){
                    fprintf(fp,"step %d,\n",count);
                    fprintf(fp,"%f,%f,%f\n",rvecs[0],rvecs[1],rvecs[2]);
                    fclose(fp);
                }
    }

    end = std::chrono::system_clock::now();
    double elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();

    cv::waitKey(0);
    count++;
    }

    return 1;
}

max_stars_repo_name: KentaKawamata/estimatePosebyAR

max_issues_repo_name: KentaKawamata/estimatePosebyAR

max_forks_repo_name: KentaKawamata/estimatePosebyAR


text:
#include "ILP_input.h"
#include <Eigen/Eigen>
#include "cuthill-mckee.h"
#include "bfs_ordering.hxx"
#include "minimum_degree_ordering.hxx"
#include "time_measure_util.h"
#include <iostream>

namespace LPMP {

    bool ILP_input::var_exists(const std::string& var) const
    {
        return var_name_to_index_.count(var) > 0;
    }

    size_t ILP_input::get_var_index(const std::string& var) const
    {
        assert(var_exists(var));
        return var_name_to_index_.find(var)->second;
    }

    std::string ILP_input::get_var_name(const size_t index) const
    {
        assert(index < var_index_to_name_.size());
        return var_index_to_name_[index];
    }

    size_t ILP_input::add_new_variable(const std::string& var)
    {
        assert(!var_exists(var));
        const size_t var_index = var_name_to_index_.size();
        var_name_to_index_.insert({var, var_index});
        assert(var_index_to_name_.size() == var_index);
        var_index_to_name_.push_back(var);
        if(objective_.size() <= var_index) // variables with 0 objective coefficient need not appear in objective line!
            objective_.resize(var_index+1,0.0);
        var_permutation_.push_back(var_permutation_.size()-1);
        return var_index;
    }

    size_t ILP_input::get_or_create_variable_index(const std::string& var)
    {
        if(var_exists(var))
            return get_var_index(var);
        else
            return add_new_variable(var);
    }

    size_t ILP_input::nr_variables() const
    {
        return var_name_to_index_.size();
    }

    void ILP_input::add_to_objective(const double coefficient, const std::string& var)
    {
        add_to_objective(coefficient, get_or_create_variable_index(var));
    }

    void ILP_input::add_to_objective(const double coefficient, const size_t var)
    {
        assert(var < objective_.size());
        objective_[var] += coefficient;
    }

    double ILP_input::objective(const size_t var) const
    {
        if(var >= nr_variables())
        {
            std::cout << var << ", " << nr_variables() << "\n";
            throw std::runtime_error("variable not present");
        }
        if(var >= objective_.size())
            return 0.0;
        return objective_[var];
    }

    double ILP_input::objective(const std::string& var) const
    {
        return objective(get_var_index(var));
    }

    size_t ILP_input::begin_new_inequality()
    {
        linear_constraints_.push_back({});
        // set defaul inequality identifier
        std::string ineq_name = "INEQ_NR_" + std::to_string(linear_constraints_.size()-1);
        set_inequality_identifier(ineq_name);
        return linear_constraints_.size()-1;
    }

    void ILP_input::set_inequality_identifier(const std::string& identifier)
    {
        assert(linear_constraints_.size() > 0);
        if(identifier == "")
            return;
        const std::string& old_identifier = linear_constraints_.back().identifier;
        if(old_identifier != "")
        {
            assert(inequality_identifier_to_index_.count(old_identifier) > 0);
            inequality_identifier_to_index_.erase(old_identifier);
        }
        linear_constraints_.back().identifier = identifier;
        if(inequality_identifier_to_index_.count(identifier) > 0)
            throw std::runtime_error("Duplicate inequality identifier " + identifier);
        inequality_identifier_to_index_.insert({identifier, linear_constraints_.size()-1});
    }

    void ILP_input::set_inequality_type(const inequality_type ineq)
    {
        assert(linear_constraints_.size() > 0);
        linear_constraints_.back().ineq = ineq;
    }

    void ILP_input::add_to_constraint(const int coefficient, const size_t var)
    {
        assert(linear_constraints_.size() > 0);
        auto& constr = linear_constraints_.back();
        constr.variables.push_back({coefficient, var});

        if(constr.variables.size() > 1)
            if(constr.variables.back() < constr.variables[constr.variables.size()-2])
                constr.normalize();
    }
    void ILP_input::add_to_constraint(const int coefficient, const std::string& var)
    {
        add_to_constraint(coefficient, get_or_create_variable_index(var));
    }

    void ILP_input::set_right_hand_side(const int x)
    {
        assert(linear_constraints_.size() > 0);
        linear_constraints_.back().right_hand_side = x;
    }

    size_t ILP_input::nr_constraints() const
    {
        return linear_constraints_.size();
    }

    bool ILP_input::preprocess()
    {
        for (auto it = linear_constraints_.begin(); it != linear_constraints_.end(); it++)
        {
            bool remove = false;

            // empty constraints
            if (it->variables.empty())
            {
                // feasibility check
                switch (it->ineq)
                {
                    case inequality_type::smaller_equal:
                        if(it->right_hand_side < 0)
                            return false;
                        break;
                    case inequality_type::greater_equal:
                        if(it->right_hand_side > 0)
                            return false;
                        break;
                    case inequality_type::equal:
                        if(it->right_hand_side != 0)
                            return false;
                        break;
                    default:
                        throw std::runtime_error("inequality type not supported");
                }
                remove = true;
            }

            // variable bounds and fixations
            if (it->variables.size() == 1)
            {
                auto variable = it->variables.back();
                switch (it->ineq)
                {
                    case inequality_type::smaller_equal:
                        if(std::min(variable.coefficient, 0) > it->right_hand_side)
                            return false;
                        if(std::max(variable.coefficient, 0) <= it->right_hand_side)
                            remove = true;
                        break;
                    case inequality_type::greater_equal:
                        if(std::max(variable.coefficient, 0) < it->right_hand_side)
                            return false;
                        if(std::min(variable.coefficient, 0) >= it->right_hand_side)
                            remove = true;
                        break;
                    case inequality_type::equal:
                        if(it->right_hand_side != 0 && it->right_hand_side != variable.coefficient)
                            return false;
                        break;
                    default:
                        throw std::runtime_error("inequality type not supported");
                }
            }

            // TODO implement preprocessing of variable/group fixations (e.g. x = 1, x + y = 0)

            // remove redundant constraint
            if (remove)
            {
                *it = linear_constraints_.back();
                linear_constraints_.pop_back();
                it--;
            }
        }

        return true;
    }

    inline two_dim_variable_array<size_t> ILP_input::variable_adjacency_matrix() const
    {
        MEASURE_FUNCTION_EXECUTION_TIME
        std::vector<Eigen::Triplet<int>> var_constraint_adjacency_list;

        for(size_t i=0; i<this->linear_constraints_.size(); ++i) {
            const auto& l = this->linear_constraints_[i];
            for(const auto& v : l.variables) {
                var_constraint_adjacency_list.push_back({v.var, i, 1});
            }
        }

        Eigen::SparseMatrix<int> A(this->nr_variables(), this->linear_constraints_.size());
        A.setFromTriplets(var_constraint_adjacency_list.begin(), var_constraint_adjacency_list.end());
        const Eigen::SparseMatrix<int> adj_matrix = A*A.transpose();
        assert(adj_matrix.cols() == nr_variables() && adj_matrix.rows() == nr_variables());

        std::vector<size_t> adjacency_size(nr_variables(), 0);

        for(size_t i=0; i<adj_matrix.outerSize(); ++i) {
            for(typename Eigen::SparseMatrix<int>::InnerIterator it(adj_matrix,i); it; ++it) {
                if(it.value() > 0 && i != it.index()) {
                    adjacency_size[i]++;
                }
            }
        }

        two_dim_variable_array<size_t> adjacency(adjacency_size.begin(), adjacency_size.end());
        std::fill(adjacency_size.begin(), adjacency_size.end(), 0);

        for(size_t i=0; i<adj_matrix.outerSize(); ++i) {
            for(typename Eigen::SparseMatrix<int>::InnerIterator it(adj_matrix,i); it; ++it) {
                if(it.value() > 0 && i != it.index()) {
                    adjacency(i,adjacency_size[i]++) = it.index();
                }
            }
        }

        for(size_t i=0; i<adjacency.size(); ++i) {
            assert(std::is_sorted(adjacency[i].begin(), adjacency[i].end()));
        }

        return adjacency;
    }

    /*
       inline two_dim_variable_array<size_t> ILP_input::variable_adjacency_matrix() const
       {
    //std::unordered_set<std::array<size_t,2>> adjacent_vars;
    tsl::robin_set<std::array<size_t,2>> adjacent_vars;
    for(const auto& l : this->linear_constraints_) {
    for(size_t i=0; i<l.variables.size(); ++i) {
    for(size_t j=i+1; j<l.variables.size(); ++j) {
    const size_t var1 = l.variables[i].var;
    const size_t var2 = l.variables[j].var;
    adjacent_vars.insert({std::min(var1,var2), std::max(var1, var2)});
    }
    }
    }

    std::vector<size_t> adjacency_size(this->nr_variables(),0);
    for(const auto [i,j] : adjacent_vars) {
    ++adjacency_size[i];
    ++adjacency_size[j];
    }

    two_dim_variable_array<size_t> adjacency(adjacency_size.begin(), adjacency_size.end());
    std::fill(adjacency_size.begin(), adjacency_size.end(), 0);
    for(const auto e : adjacent_vars) {
    const auto [i,j] = e;
    assert(i<j);
    assert(adjacency_size[i] < adjacency[i].size());
    assert(adjacency_size[j] < adjacency[j].size());
    adjacency(i, adjacency_size[i]++) = j;
    adjacency(j, adjacency_size[j]++) = i;
    }

    for(size_t i=0; i<adjacency.size(); ++i)
    std::sort(adjacency[i].begin(), adjacency[i].end());

    return adjacency;
    }
    */

    inline two_dim_variable_array<size_t> ILP_input::bipartite_variable_bdd_adjacency_matrix() const
    {
        MEASURE_FUNCTION_EXECUTION_TIME
        std::vector<std::pair<size_t,size_t>> var_bdd_adj_list;
        std::vector<size_t> degrees(this->nr_variables() + this->nr_constraints(), 0);

        // determine list of adjacencies and vertex degrees
        for(size_t i=0; i<this->linear_constraints_.size(); ++i) {
            const auto& l = this->linear_constraints_[i];
            for(const auto& v : l.variables) {
                var_bdd_adj_list.emplace_back(v.var, i);
                degrees[v.var]++;
                degrees[this->nr_variables() + i]++;
            }
        }

        // create adjacency matrix
        two_dim_variable_array<size_t> adjacency(degrees.begin(), degrees.end());
        std::fill(degrees.begin(), degrees.end(), 0);
        for (auto it = var_bdd_adj_list.begin(); it != var_bdd_adj_list.end(); it++)
        {
            size_t var = it->first;
            size_t bdd = this->nr_variables() + it->second;
            adjacency(var, degrees[var]++) = bdd;
            adjacency(bdd, degrees[bdd]++) = var;
        }

        return adjacency;
    }

    permutation ILP_input::reorder(ILP_input::variable_order var_ord)
    {
        if(var_ord == variable_order::input)
            var_permutation_ = permutation(nr_variables());
        else if(var_ord == variable_order::bfs)
            var_permutation_ = this->reorder_bfs();
        else if(var_ord == variable_order::cuthill)
            var_permutation_ = this->reorder_Cuthill_McKee();
        else
        {
            assert(var_ord == variable_order::mindegree);
            var_permutation_ = this->reorder_minimum_degree_ordering();
        }

        return var_permutation_;
    }

    void ILP_input::reorder(const permutation& order)
    {
        MEASURE_FUNCTION_EXECUTION_TIME;
        assert(order.size() == this->nr_variables());
        assert(order.is_permutation());
        std::vector<double> new_objective(this->nr_variables(), 0.0);
        std::vector<size_t> inverse_order(this->nr_variables());
        std::vector<std::string> new_var_index_to_name(this->nr_variables());

        for(size_t i=0; i<this->nr_variables(); ++i)
        {
            assert(var_name_to_index_.count(var_index_to_name_[order[i]]) > 0);
            var_name_to_index_.find(var_index_to_name_[order[i]]).value() = i;
        }

//#pragma omp parallel for schedule(guided)
        for(size_t i=0; i<this->nr_variables(); ++i)
        {
            if(order[i] < this->objective_.size())
                new_objective[i] = this->objective_[order[i]];

            if(order[i] < this->var_index_to_name_.size())
                new_var_index_to_name[i] = std::move(this->var_index_to_name_[order[i]]);

            inverse_order[order[i]] = i;

        }
        std::swap(this->objective_, new_objective);
        std::swap(new_var_index_to_name, this->var_index_to_name_);

//#pragma omp parallel for schedule(guided)
        for(size_t lc_index=0; lc_index<this->linear_constraints_.size(); ++lc_index)
        {
            auto& l = this->linear_constraints_[lc_index];
            for(auto& x : l.variables) {
                x.var = inverse_order[x.var];
            }
            l.normalize();
        }
    }

    inline permutation ILP_input::reorder_bfs()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = bfs_ordering(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    inline permutation ILP_input::reorder_Cuthill_McKee()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = Cuthill_McKee(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    inline permutation ILP_input::reorder_minimum_degree_ordering()
    {
        const auto adj = bipartite_variable_bdd_adjacency_matrix();
        const auto order = minimum_degree_ordering(adj, this->nr_variables());
        reorder(order);
        return order;
    }

    size_t ILP_input::nr_constraint_groups() const
    {
        return coalesce_sets_.size();
    }

    std::tuple<const size_t*, const size_t*> ILP_input::constraint_group(const size_t i) const
    {
        assert(i < nr_constraint_groups());
        return std::make_tuple(coalesce_sets_[i].begin(), coalesce_sets_[i].end());
    }

    std::tuple<Eigen::SparseMatrix<int>, Eigen::MatrixXi> ILP_input::export_constraints() const
    {
        using T = Eigen::Triplet<int>;
        std::vector<T> coefficients;

        for(size_t c=0; c<nr_constraints(); ++c)
        {
            for(const auto& l : constraints()[c].variables)
                coefficients.push_back(T(c, l.var, l.coefficient));
        }

        Eigen::SparseMatrix<int> A(nr_constraints(), nr_variables());
        A.setFromTriplets(coefficients.begin(), coefficients.end());

        Eigen::MatrixXi b(nr_constraints(), 2);
        for(size_t c=0; c<nr_constraints(); ++c)
        {
            if(constraints()[c].ineq == inequality_type::equal)
            {
                b(c,0) = constraints()[c].right_hand_side;
                b(c,1) = constraints()[c].right_hand_side;
            }
            else if(constraints()[c].ineq == inequality_type::smaller_equal)
            {
                b(c,0) = std::numeric_limits<int>::min();
                b(c,1) = constraints()[c].right_hand_side;
            }
            else if(constraints()[c].ineq == inequality_type::greater_equal)
            {
                b(c,0) = constraints()[c].right_hand_side;
                b(c,1) = std::numeric_limits<int>::max();
            }
            else
            {
                assert(false);
            }
        }

        return {A,b};
    }

}


text:
//  Unit test for boost::lexical_cast.
//
//  See http://www.boost.org for most recent version, including documentation.
//
//  Copyright Antony Polukhin, 2011.
//
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt).

#include <boost/config.hpp>

#include <boost/static_assert.hpp>
#include <boost/lexical_cast.hpp>

int main()
{
#ifdef BOOST_MSVC
    BOOST_STATIC_ASSERT((boost::is_same<wchar_t, unsigned short>::value));
#endif

    return ::boost::lexical_cast<int>(L"1000") == 1000;
}


text:
//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_quaternion_rotate);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;

  typedef math_types::value_traits     value_traits;
  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;

  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 1.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 0.0, 1.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, -1.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 0.0, -1.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 0.0, 1.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 1.0, 0.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 0.0, 0.0, 1.0 );
    q.Ru( phi, m);
    V const n = V( 1.0, 0.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
}

BOOST_AUTO_TEST_SUITE_END();

max_stars_repo_name: ricortiz/OpenTissue

max_issues_repo_name: ricortiz/OpenTissue

max_forks_repo_name: ricortiz/OpenTissue


text:
/**
   @author Shin'ichiro Nakaoka
*/

#include "PythonConsoleView.h"
#include "PythonPlugin.h"
#include <cnoid/PyUtil>
#include <cnoid/MessageView>
#include <cnoid/ViewManager>
#include <QPlainTextEdit>
#include <QBoxLayout>
#include <QTextBlock>
#include <QEventLoop>
#include <QMimeData>
#include <boost/algorithm/string.hpp>
#include <QMimeData>
#include <list>
#include "gettext.h"

using namespace std;
using namespace cnoid;

namespace {

const unsigned int HISTORY_SIZE = 100;

class PythonConsoleOut
{
public:
    PythonConsoleView::Impl* console;
    void setConsole(PythonConsoleView::Impl* console);
    void write(std::string const& text);
};

class PythonConsoleIn
{
public:
    PythonConsoleView::Impl* console;
    void setConsole(PythonConsoleView::Impl* console);
    python::object readline();
};

}

namespace cnoid {

class PythonConsoleView::Impl : public QPlainTextEdit
{
public:
    Impl(PythonConsoleView* self);
    ~Impl();

    PythonConsoleView* self;
    PythonPlugin* pythonPlugin;
    bool isConsoleInMode;
    QEventLoop eventLoop;
    string stringFromConsoleIn;
    int inputColumnOffset;
    QString prompt;
    std::list<QString>::iterator histIter;
    std::list<QString> history;
    std::vector<string> splitStringVec;
    std::vector<string> keywords;
    Signal<void(const std::string& output)> sigOutput;

    python::module mainModule;
    python::object globalNamespace;
    python::object consoleOut;
    python::object consoleIn;
    python::object sys;
    python::object orgStdout;
    python::object orgStderr;
    python::object orgStdin;
    python::object interpreter;

    void setPrompt(const char* newPrompt);
    void put(const QString& message);
    void putln(const QString& message);
    void putPrompt();
    void execCommand();
    python::object getMemberObject(std::vector<string>& moduleNames, python::object& parentObject);
    std::vector<string> getMemberNames(python::object& moduleObject);
    void tabComplete();
    QString getInputString();
    void setInputString(const QString& command);
    void addToHistory(const QString& command);
    QString getPrevHistoryEntry();
    QString getNextHistoryEntry();

    string getInputFromConsoleIn();
    void fixInput();

    virtual void keyPressEvent(QKeyEvent* event);
    virtual void insertFromMimeData(const QMimeData* source);
};

}

namespace {

void PythonConsoleOut::setConsole(PythonConsoleView::Impl* console)
{
    this->console = console;
}

void PythonConsoleOut::write(std::string const& text)
{
    console->put(QString(text.c_str()));
    console->sigOutput(text);
}

void PythonConsoleIn::setConsole(PythonConsoleView::Impl* console)
{
    this->console = console;
}

python::object PythonConsoleIn::readline()
{
    //! \todo release the GIL inside this function
    return python::str(console->getInputFromConsoleIn());
}

}

void PythonConsoleView::initializeClass(ExtensionManager* ext)
{
    ext->viewManager().registerClass<PythonConsoleView>(
        N_("PythonConsoleView"), N_("Python Console"), ViewManager::Permanent);
}

PythonConsoleView::PythonConsoleView()
{
    impl = new Impl(this);
    setFocusProxy(impl);
}

PythonConsoleView::Impl::Impl(PythonConsoleView* self)
    : self(self),
      pythonPlugin(PythonPlugin::instance())
{
    isConsoleInMode = false;
    inputColumnOffset = 0;

    splitStringVec = {
        " ", "{",  "}", "(",  ")", "[", "]", "<", ">", ":", ";", "^",
        "@", "\"", ",", "\\", "!", "#", "'", "=", "|", "*", "?", "\t" };

    self->setDefaultLayoutArea(BottomCenterArea);

    setFrameShape(QFrame::NoFrame);
    setReadOnly(false);
    setWordWrapMode(QTextOption::WrapAnywhere);
    setUndoRedoEnabled(false);

    QHBoxLayout* hbox = new QHBoxLayout();
    hbox->addWidget(this);
    self->setLayout(hbox);

    python::gil_scoped_acquire lock;

    mainModule = pythonPlugin->mainModule();
    globalNamespace = pythonPlugin->globalNamespace();

#ifdef _WIN32
    try { interpreter = python::module::import("code").attr("InteractiveConsole")(globalNamespace);
    } catch (...) { /* ignore the exception on windows. this module is loaded already. */}
#else
    interpreter = python::module::import("code").attr("InteractiveConsole")(globalNamespace);
#endif

    pybind11::object consoleOutClass =
        pybind11::class_<PythonConsoleOut>(mainModule, "PythonConsoleOut")
        .def(pybind11::init<>())
        .def("write", &PythonConsoleOut::write);

    consoleOut = consoleOutClass();

    PythonConsoleOut& consoleOut_ = consoleOut.cast<PythonConsoleOut&>();
    consoleOut_.setConsole(this);

    python::object consoleInClass =
        pybind11::class_<PythonConsoleIn>(mainModule, "PythonConsoleIn")
        .def(pybind11::init<>())
        .def("readline", &PythonConsoleIn::readline);

    consoleIn = consoleInClass();

    PythonConsoleIn& consoleIn_ = consoleIn.cast<PythonConsoleIn&>();
    consoleIn_.setConsole(this);

    sys = pythonPlugin->sysModule();

    python::object keyword = python::module::import("keyword");
    pybind11::list kwlist = pybind11::cast<pybind11::list>(keyword.attr("kwlist"));
    for(size_t i = 0; i < pybind11::len(kwlist); ++i){
        keywords.push_back(pybind11::cast<string>(kwlist[i]));
    }

    histIter = history.end();

    putln(QString("Python %1").arg(Py_GetVersion()));

    prompt = ">>> ";
    putPrompt();
}

PythonConsoleView::~PythonConsoleView()
{
    python::gil_scoped_acquire lock;
    delete impl;
}

PythonConsoleView::Impl::~Impl()
{

}

void PythonConsoleView::onActivated()
{
    impl->setFocus();
}

void PythonConsoleView::Impl::setPrompt(const char* newPrompt)
{
    prompt = newPrompt;
}

void PythonConsoleView::Impl::put(const QString& message)
{
    moveCursor(QTextCursor::End);
    insertPlainText(message);
    moveCursor(QTextCursor::End);
}

void PythonConsoleView::Impl::putln(const QString& message)
{
    put(message + "\n");
    MessageView::instance()->flush();
}

void PythonConsoleView::inputCommand(const std::string& command)
{
    impl->put(command.c_str());
    impl->execCommand();
}

SignalProxy<void(const std::string& output)> PythonConsoleView::sigOutput()
{
    return impl->sigOutput;
}

void PythonConsoleView::Impl::putPrompt()
{
    put(prompt);
    sigOutput(prompt.toStdString());
    inputColumnOffset = textCursor().columnNumber();
}

void PythonConsoleView::Impl::execCommand()
{
    python::gil_scoped_acquire lock;

    orgStdout = sys.attr("stdout");
    orgStderr = sys.attr("stderr");
    orgStdin = sys.attr("stdin");

    sys.attr("stdout") = consoleOut;
    sys.attr("stderr") = consoleOut;
    sys.attr("stdin") = consoleIn;

    QString command = getInputString();

    put("\n"); // This must be done after getInputString().

    if(interpreter.attr("push")(command.toStdString()).cast<bool>()){
        setPrompt("... ");
    } else {
        setPrompt(">>> ");
    }

    if(PyErr_Occurred()){
        PyErr_Print();
    }

    sys.attr("stdout") = orgStdout;
    sys.attr("stderr") = orgStderr;
    sys.attr("stdin") = orgStdin;

    addToHistory(command);

    putPrompt();
}

python::object PythonConsoleView::Impl::getMemberObject(std::vector<string>& moduleNames, python::object& parentObject)
{
    if(moduleNames.size() == 0){
        return parentObject;
    }else{
        string moduleName = moduleNames.front();
        moduleNames.erase(moduleNames.begin());
        std::vector<string> memberNames = getMemberNames(parentObject);
        if(std::find(memberNames.begin(),memberNames.end(),moduleName) == memberNames.end()){
            return python::object();
        }else{
            python::object childObject = parentObject.attr(moduleName.c_str());
            return getMemberObject(moduleNames, childObject);
        }
    }
}

std::vector<string> PythonConsoleView::Impl::getMemberNames(python::object& moduleObject)
{
    PyObject* pPyObject = moduleObject.ptr();
    if(pPyObject == NULL){
        return std::vector<string>();
    }
    pybind11::handle h( PyObject_Dir(pPyObject) );
    pybind11::list memberNames = h.cast<pybind11::list>();
    std::vector<string> retNames;
    for(size_t i=0; i < python::len(memberNames); ++i){
        if(!strstr(string(memberNames[i].cast<string>()).c_str(), "__" )){
            retNames.push_back(string(memberNames[i].cast<string>()));
        }
    }
    return retNames;
}

void PythonConsoleView::Impl::tabComplete()
{
    python::gil_scoped_acquire lock;

    orgStdout = sys.attr("stdout");
    orgStderr = sys.attr("stderr");
    orgStdin = sys.attr("stdin");

    sys.attr("stdout") = consoleOut;
    sys.attr("stderr") = consoleOut;
    sys.attr("stdin") = consoleIn;

    QTextCursor cursor = textCursor();
    string beforeCursorString = getInputString().toStdString();
    beforeCursorString = beforeCursorString.substr(0,cursor.columnNumber()-inputColumnOffset);
    QString afterCursorString = getInputString();
    afterCursorString.remove(0, cursor.columnNumber()-inputColumnOffset);
    size_t maxSplitIdx = 0;
    for(std::vector<string>::iterator it = splitStringVec.begin(); it != splitStringVec.end();  ++it){
        size_t splitIdx = beforeCursorString.find_last_of(*it);
        maxSplitIdx = std::max(splitIdx == string::npos ? 0 : splitIdx + 1, maxSplitIdx);
    }
    string lastWord = beforeCursorString.substr(maxSplitIdx);
    beforeCursorString = beforeCursorString.substr(0,maxSplitIdx);

    std::vector<string> dottedStrings;
    boost::split(dottedStrings, lastWord, boost::is_any_of("."));
    string lastDottedString = dottedStrings.back();// word after last dot

    std::vector<string> moduleNames = dottedStrings;// words before last dot
    moduleNames.pop_back();

    python::object targetMemberObject = getMemberObject(moduleNames, mainModule); //member object before last dot
    std::vector<string> memberNames = getMemberNames(targetMemberObject);

    // builtin function and syntax completions
    if(dottedStrings.size() == 1){
        python::object builtinsObject =  mainModule.attr("__builtins__");
        std::vector<string> builtinMethods = getMemberNames(builtinsObject);
        memberNames.insert(memberNames.end(), builtinMethods.begin(), builtinMethods.end());
        memberNames.insert(memberNames.end(), keywords.begin(), keywords.end());
    }

    std::vector<string> completions;
    unsigned long int maxLength = std::numeric_limits<long>::max();
    for(size_t i=0; i < memberNames.size(); ++i){
        if(memberNames[i].substr(0,lastDottedString.size()) == lastDottedString){
            completions.push_back(memberNames[i]);
            maxLength = std::min((unsigned long int)memberNames[i].size(),maxLength);
        }
    }

    if(PyErr_Occurred()){
        PyErr_Print();
    }

    sys.attr("stdout") = orgStdout;
    sys.attr("stderr") = orgStderr;
    sys.attr("stdin") = orgStdin;

    if(completions.size() != 0){
        // max common string among completions
        std::string maxCommonStr = lastDottedString;
        for(size_t i=maxCommonStr.size(); i < maxLength; ++i){
            bool commomFlg = true;
            for(size_t j=1; j < completions.size(); ++j){
                if(completions[0].at(i) != completions[j].at(i)){
                    commomFlg = false;
                    break;
                }
            }
            if( commomFlg ){
                maxCommonStr.push_back(completions[0].at(i));
            } else {
                break;
            }
        }

        string beforeLastDotStr = "";
        for(std::vector<string>::iterator it = dottedStrings.begin(); it != dottedStrings.end()-1; ++it){
            beforeLastDotStr.append(*it);
            beforeLastDotStr.append(".");
        }

        if(lastDottedString == maxCommonStr){
            put("\n"); // This must be done after getInputString().

            string str = "";
            for(size_t i=0; i < completions.size(); ++i){
                str.append(beforeLastDotStr);
                str.append(completions[i]);
                str.append("     ");
            }
            putln(QString(str.c_str()));
            putPrompt();
        }

        string str = "";
        str.append(beforeCursorString);
        str.append(beforeLastDotStr);
        str.append(maxCommonStr);
        str.append(afterCursorString.toStdString());
        setInputString(QString(str.c_str()));
        for(size_t i=0; i < afterCursorString.toStdString().size(); ++i){
            moveCursor(QTextCursor::Left);
        }
    }
}

QString PythonConsoleView::Impl::getInputString()
{
    QTextDocument* doc = document();
    QString line = doc->findBlockByLineNumber(doc->lineCount() - 1).text();
    line.remove(0, inputColumnOffset);
    return line;
}

void PythonConsoleView::Impl::setInputString(const QString& command)
{
    if(getInputString() == command){
        return;
    }

    QTextCursor cursor = textCursor();
    cursor.movePosition(QTextCursor::End);
    cursor.movePosition(QTextCursor::StartOfBlock, QTextCursor::KeepAnchor);
    cursor.movePosition(QTextCursor::Right, QTextCursor::KeepAnchor, inputColumnOffset);
    cursor.removeSelectedText();
    cursor.insertText(command);
    moveCursor(QTextCursor::End);
}

void PythonConsoleView::Impl::addToHistory(const QString& command)
{
    if(!command.isEmpty()){
        if(history.empty() || history.back() != command){
            if(HISTORY_SIZE <= history.size()){
                history.pop_front();
            }
            history.push_back(command);
        }
        histIter = history.end();
    }
}

QString PythonConsoleView::Impl::getPrevHistoryEntry()
{
    if(!history.empty()){
        if(histIter != history.begin()){
            --histIter;
        }
        return *histIter;
    }
    return QString();
}

QString PythonConsoleView::Impl::getNextHistoryEntry()
{
    if(!history.empty()){
        if(histIter != history.end()){
            ++histIter;
            if(histIter != history.end()){
                return *histIter;
            }
        }
    }
    return QString();
}

string PythonConsoleView::Impl::getInputFromConsoleIn()
{
    sys.attr("stdout") = orgStdout;
    sys.attr("stderr") = orgStderr;
    sys.attr("stdin") = orgStdin;

    int result;

    Py_BEGIN_ALLOW_THREADS

    isConsoleInMode = true;
    inputColumnOffset = textCursor().columnNumber();

    result = eventLoop.exec();
    isConsoleInMode = false;

    Py_END_ALLOW_THREADS

    sys.attr("stdout") = consoleOut;
    sys.attr("stderr") = consoleOut;
    sys.attr("stdin") = consoleIn;

    if(result == 0){
        return stringFromConsoleIn + "\n";
    } else {
        put("\n");
        //! \todo put an error message here
        return string();
    }
}

void PythonConsoleView::Impl::fixInput()
{
    stringFromConsoleIn = getInputString().toStdString();
    put("\n");
    eventLoop.exit();
}

void PythonConsoleView::Impl::keyPressEvent(QKeyEvent* event)
{
    bool done = false;

    switch(event->key()){

    case Qt::Key_F:
        if(event->modifiers() == Qt::ControlModifier){
            moveCursor(QTextCursor::Right);
            done = true;
        }
        break;

    case Qt::Key_B:
        if(event->modifiers() == Qt::ControlModifier){
            QTextCursor cursor = textCursor();
            if(cursor.position() > cursor.block().position() + inputColumnOffset ){
                moveCursor(QTextCursor::Left);
            }
            done = true;
        }
        break;

    case Qt::Key_H:
        if(event->modifiers() == Qt::ControlModifier){
            QTextCursor cursor = textCursor();
            if(cursor.position() > cursor.block().position() + inputColumnOffset ){
                cursor.movePosition(QTextCursor::Left, QTextCursor::KeepAnchor, 1);
                cursor.removeSelectedText();
             }
            done = true;
        }
        break;

    case Qt::Key_Left:
    case Qt::Key_Backspace:{
        QTextCursor cursor = textCursor();
        if(cursor.position() <= cursor.block().position() + inputColumnOffset ){
            done = true;
        }
        break;
    }
    case Qt::Key_P:
        if(event->modifiers() != Qt::ControlModifier){
            break;
        }
    case Qt::Key_Up:
        setInputString(getPrevHistoryEntry());
        done = true;
        break;

    case Qt::Key_N:
        if(event->modifiers() != Qt::ControlModifier){
            break;
        }
    case Qt::Key_Down:
        setInputString(getNextHistoryEntry());
        done = true;
        break;

    case Qt::Key_A:
        if(event->modifiers() == Qt::ControlModifier){
            moveCursor(QTextCursor::StartOfBlock);
            for(int i=0; i < inputColumnOffset; ++i) moveCursor(QTextCursor::Right);
            done = true;
        }
        break;

    case Qt::Key_E:
        if( event->modifiers() == Qt::ControlModifier ){
            moveCursor(QTextCursor::End);
        }
        break;

    case Qt::Key_Return:
    case Qt::Key_Enter:
        if(isConsoleInMode){
            fixInput();
        } else {
            execCommand();
        }
        done = true;
        break;

    case Qt::Key_Tab:
        {
            QString inputString = getInputString();
            if(inputString.toStdString().empty() || *(inputString.toStdString().end()-1) == '\t'){
                done = false;
            }else{
                tabComplete();
                done = true;
            }
        }
        break;

    default:
        break;
    }

    if(!done){
        QPlainTextEdit::keyPressEvent(event);
    }
}

/**
   \todo Implement this virtual function to correctly process a pasted text block
*/
void PythonConsoleView::Impl::insertFromMimeData(const QMimeData* source)
{
    if(!source->hasText()){
        QPlainTextEdit::insertFromMimeData(source);

    } else {
        QString text = source->text();
        QStringList lines = text.split(QRegExp("(\r\n|\r|\n)"));
        int n = lines.size();
        if(n > 0){
            for(int i=0; i < n - 1; ++i){
                put(lines[i]);
                execCommand();
            }
            const QString& lastLine = lines[n-1];
            if(!lastLine.isEmpty()){
                put(lastLine);
                if(text.contains(QRegExp("[\r\n]$"))){
                    execCommand();
                }
            }
        }
    }
}