text:
//
// Created by nima on 4/18/17.
//

#include <custom_conversions/Conversions.h>
#include <vtkCubeSource.h>
#include <boost/thread/thread.hpp>
#include "TaskKidney.h"
#include "src/ar_core/VTKConversions.h"

namespace COLORS {
    double Red[3] {1.0, 0.1, 0.03};
    double OrangeRed[3] {1.0, 0.27, 0.03};
    double Gold[3] {1.0, 0.84, 0.0};
    double Green[3] {0.0, 0.9, 0.03};
    double Pink[3] {1.0, 0.0, 1.0};
    double Orange[3] {0.9, 0.4, 0.1};
    double Gray [3] {0.4, 0.4, 0.4};
    double Turquoise[3]	{0.25, 0.88, 0.82};
    double DeepPink[3] {1.0, 0.08, 0.58};
};

TaskKidney::TaskKidney(const std::string stl_file_dir,
                       const bool show_ref_frames, const bool biman,
                       const bool with_guidance)
        :
    SimTask(NULL, 100),
        stl_files_dir(stl_file_dir),
//        show_ref_frames(show_ref_frames),
        bimanual(biman),
        with_guidance(with_guidance),
        destination_ring_counter(0),
        ac_params_changed(true),
        task_state(TaskKidneyState::Idle),
        number_of_repetition(0),
        n_score_history(10),
        idle_point(KDL::Vector(-0.006, 0.026, 0.029)),
        start_point(KDL::Vector(0.0019, 0.031, 0.030)),
        end_point(KDL::Vector(0.034, 0.043, 0.053)) {

    // -------------------------------------------------------------------------
    //  ACTIVE CONSTRAINT
    // -------------------------------------------------------------------------
    // these parameters could be set as ros parameters too but since
    // they change during the task I am hard coding them here.
    ac_parameters.method = 0; // 0 for visco/elastic
    ac_parameters.active = 0;

    ac_parameters.max_force = 4.0;
    ac_parameters.linear_elastic_coeff = 1000.0;
    ac_parameters.linear_damping_coeff = 10.0;

    ac_parameters.max_torque = 0.03;
    ac_parameters.angular_elastic_coeff = 0.04;
    ac_parameters.angular_damping_coeff = 0.002;

    // -------------------------------------------------------------------------
    //  INITIALIZING GRAPHICS ACTORS
    // -------------------------------------------------------------------------

    ring_actor[0] = vtkSmartPointer<vtkActor>::New();
    tool_current_frame_axes[0] = vtkSmartPointer<vtkAxesActor>::New();
    tool_desired_frame_axes[0] = vtkSmartPointer<vtkAxesActor>::New();
    tool_current_pose[0] = vtkSmartPointer<vtkMatrix4x4>::New();

    if(bimanual){
        ring_actor[1] = vtkSmartPointer<vtkActor>::New();
        tool_current_frame_axes[1] = vtkSmartPointer<vtkAxesActor>::New();
        tool_desired_frame_axes[1] = vtkSmartPointer<vtkAxesActor>::New();
        tool_current_pose[1] = vtkSmartPointer<vtkMatrix4x4>::New();

    }

    cellLocator = vtkSmartPointer<vtkCellLocator>::New();

    mesh_actor = vtkSmartPointer<vtkActor>::New();

    // -------------------------------------------------------------------------
    // TOOL RINGS
    ring_radius = 0.004;
    double ring_cross_section_radius = 0.0005;
    double source_scales = 0.006;

    vtkSmartPointer<vtkParametricTorus> parametricObject =
            vtkSmartPointer<vtkParametricTorus>::New();
    parametricObject->SetCrossSectionRadius(
            ring_cross_section_radius / source_scales);
    parametricObject->SetRingRadius(ring_radius / source_scales);

    vtkSmartPointer<vtkParametricFunctionSource> parametricFunctionSource =
            vtkSmartPointer<vtkParametricFunctionSource>::New();
    parametricFunctionSource->SetParametricFunction(parametricObject);
    parametricFunctionSource->Update();

    // to transform the data
    vtkSmartPointer<vtkTransformPolyDataFilter>
            ring_local_transform_filter[2];
    vtkSmartPointer<vtkTransform> ring_local_transform[2];
    vtkSmartPointer<vtkPolyDataMapper> ring_mapper[2];

    for (int k = 0; k < 1 + (int)bimanual; ++k) {

        ring_local_transform_filter[k] =
                vtkSmartPointer<vtkTransformPolyDataFilter>::New();
        ring_local_transform_filter[k]->SetInputConnection(
                parametricFunctionSource->GetOutputPort());

        ring_local_transform[k] = vtkSmartPointer<vtkTransform>::New();

        if(k == 0){
            ring_local_transform[k]->RotateX(90);
            ring_local_transform[k]->Translate(0.0, ring_radius /
                                                    source_scales, 0.0);
        }
        else{
//            ring_local_transform[k]->RotateX(0);
            ring_local_transform[k]->Translate(0.0, ring_radius /
                                                    source_scales, 0.0);
        }

        ring_local_transform_filter[k]->SetTransform(ring_local_transform[k]);
        ring_local_transform_filter[k]->Update();

        ring_mapper[k] = vtkSmartPointer<vtkPolyDataMapper>::New();
        ring_mapper[k]->SetInputConnection(
                ring_local_transform_filter[k]->GetOutputPort());

        ring_actor[k]->SetMapper(ring_mapper[k]);
        ring_actor[k]->SetScale(source_scales);
        ring_actor[k]->GetProperty()->SetColor(COLORS::Turquoise);
        ring_actor[k]->GetProperty()->SetSpecular(0.7);
    }

    // -------------------------------------------------------------------------
    // FRAMES
    vtkSmartPointer<vtkAxesActor> task_coordinate_axes =
            vtkSmartPointer<vtkAxesActor>::New();

    task_coordinate_axes->SetXAxisLabelText("");
    task_coordinate_axes->SetYAxisLabelText("");
    task_coordinate_axes->SetZAxisLabelText("");
    task_coordinate_axes->SetTotalLength(0.01, 0.01, 0.01);
    task_coordinate_axes->SetShaftType(vtkAxesActor::CYLINDER_SHAFT);

    for (int k = 0; k < 1 + (int)bimanual; ++k) {

        tool_current_frame_axes[k]->SetXAxisLabelText("");
        tool_current_frame_axes[k]->SetYAxisLabelText("");
        tool_current_frame_axes[k]->SetZAxisLabelText("");
        tool_current_frame_axes[k]->SetTotalLength(0.007, 0.007, 0.007);
        tool_current_frame_axes[k]->SetShaftType(vtkAxesActor::CYLINDER_SHAFT);

        tool_desired_frame_axes[k]->SetXAxisLabelText("");
        tool_desired_frame_axes[k]->SetYAxisLabelText("");
        tool_desired_frame_axes[k]->SetZAxisLabelText("");
        tool_desired_frame_axes[k]->SetTotalLength(0.007, 0.007, 0.007);
        tool_desired_frame_axes[k]->SetShaftType(vtkAxesActor::CYLINDER_SHAFT);

    }
    // -------------------------------------------------------------------------
    // Stand MESH hq
    std::stringstream input_file_dir;
    input_file_dir << stl_files_dir << std::string("task_kidney_tumor2.stl");

    vtkSmartPointer<vtkSTLReader> stand_mesh_reader =
            vtkSmartPointer<vtkSTLReader>::New();
   ROS_DEBUG("Loading stl file from: %s" , input_file_dir.str().c_str());
    stand_mesh_reader->SetFileName(input_file_dir.str().c_str());
    stand_mesh_reader->Update();

    // transform
    vtkSmartPointer<vtkTransform> kidney_mesh_transform =
            vtkSmartPointer<vtkTransform>::New();
    kidney_mesh_transform->Translate(0.090, 0.020, 0.0);
//    kidney_mesh_transform->RotateX(180);
//    kidney_mesh_transform->RotateZ(150);
    kidney_mesh_transform->Scale(0.01, 0.01, 0.01);

    vtkSmartPointer<vtkTransformPolyDataFilter> kidney_mesh_transformFilter =
            vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    kidney_mesh_transformFilter->SetInputConnection(
            stand_mesh_reader->GetOutputPort());
    kidney_mesh_transformFilter->SetTransform(kidney_mesh_transform);
    kidney_mesh_transformFilter->Update();

    vtkSmartPointer<vtkPolyDataMapper> kidney_mesh_mapper =
            vtkSmartPointer<vtkPolyDataMapper>::New();
    kidney_mesh_mapper->SetInputConnection(
            kidney_mesh_transformFilter->GetOutputPort());

    mesh_actor->SetMapper(kidney_mesh_mapper);
    mesh_actor->GetProperty()->SetColor(COLORS::Red);
    mesh_actor->GetProperty()->SetOpacity(1.0);
    //    stand_mesh_actor->GetProperty()->SetSpecular(0.8);

    // CLOSEST POINT will be found on the low quality mesh
    cellLocator->SetDataSet(kidney_mesh_transformFilter->GetOutput());
    cellLocator->BuildLocator();

    // -------------------------------------------------------------------------
    // Create a cube for the floor
    vtkSmartPointer<vtkCubeSource> box_source =
        vtkSmartPointer<vtkCubeSource>::New();
    double box_dimensions[3] = {0.0154, 0.0154, 0.0154};
    box_source->SetXLength(box_dimensions[0]);
    box_source->SetYLength(box_dimensions[1]);
    box_source->SetZLength(box_dimensions[2]);
    vtkSmartPointer<vtkPolyDataMapper> box_mapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
    box_mapper->SetInputConnection(box_source->GetOutputPort());
    vtkSmartPointer<vtkActor> box1_actor = vtkSmartPointer<vtkActor>::New();
    box1_actor->SetMapper(box_mapper);
    box1_actor->SetPosition(box_dimensions[0] / 2, box_dimensions[1] / 2,
                             box_dimensions[2]/2);
    box1_actor->GetProperty()->SetOpacity(1.0);
    box1_actor->GetProperty()->SetColor(COLORS::Pink);

    vtkSmartPointer<vtkActor> box2_actor = vtkSmartPointer<vtkActor>::New();
    box2_actor->SetMapper(box_mapper);
    box2_actor->SetPosition(box_dimensions[0] / 2 + box_dimensions[0]*2,
                            box_dimensions[1] / 2,
                             box_dimensions[2]/2);
    box2_actor->GetProperty()->SetOpacity(1.0);
    box2_actor->GetProperty()->SetColor(COLORS::Pink);

    vtkSmartPointer<vtkActor> box3_actor = vtkSmartPointer<vtkActor>::New();
    box3_actor->SetMapper(box_mapper);
    box3_actor->SetPosition(box_dimensions[0] / 2 + box_dimensions[0]*4,
                            box_dimensions[1] / 2,
                             box_dimensions[2]/2);
    box3_actor->GetProperty()->SetOpacity(1.0);
    box3_actor->GetProperty()->SetColor(COLORS::Pink);
    // -------------------------------------------------------------------------
    // Add all graphics_actors to a vector
    if (show_ref_frames) {
        graphics_actors.push_back(task_coordinate_axes);

        for (int k = 0; k < 1 + (int)bimanual; ++k) {
            graphics_actors.push_back(tool_current_frame_axes[k]);
            graphics_actors.push_back(tool_desired_frame_axes[k]);
        }
    }

    graphics_actors.push_back(mesh_actor);
    //graphics_actors.push_back(box1_actor);
    //graphics_actors.push_back(box2_actor);
    //graphics_actors.push_back(box3_actor);
//    graphics_actors.push_back(ring_actor[0]);
//    graphics_actors.push_back(ring_actor[0]);
//    if(bimanual){
//        graphics_actors.push_back(ring_actor[1]);
//        graphics_actors.push_back(line1_actor);
//        graphics_actors.push_back(line2_actor);
//    }
//    graphics_actors.push_back(destination_cone_actor);
//    graphics_actors.push_back(destination_ring_actor);
//    for (int j = 0; j < score_sphere_actors.size(); ++j) {
//        graphics_actors.push_back(score_sphere_actors[j]);
//    }

    //    graphics_actors.push_back(ring_guides_mesh_actor);
    //    graphics_actors.push_back(cornerAnnotation);

}

//------------------------------------------------------------------------------
//std::vector<vtkSmartPointer<vtkProp> > TaskKidney::GetActors() {
//    return graphics_actors;
//}

//------------------------------------------------------------------------------
void TaskKidney::StepWorld() {

    // -------------------------------------------------------------------------
    // Find closest points and update frames
    for (int k = 0; k < 1 + (int)bimanual; ++k) {
        tool_current_frame_axes[k]->SetUserMatrix(tool_current_pose[k]);
        ring_actor[k]->SetUserMatrix(tool_current_pose[k]);
    }

//    ring_guides_mesh_actor->SetUserMatrix(tool_current_pose[0]);

    vtkSmartPointer<vtkMatrix4x4> tool_desired_pose[2];

    for (int k = 0; k < 1 + (int)bimanual; ++k) {
        tool_desired_pose[k] =
                vtkSmartPointer<vtkMatrix4x4>::New();
        VTKConversions::KDLFrameToVTKMatrix(tool_desired_pose_kdl[k],
                                            tool_desired_pose[k]);
        tool_desired_frame_axes[k]->SetUserMatrix(tool_desired_pose[k]);
    }

    // -------------------------------------------------------------------------
    // Task logic
    // -------------------------------------------------------------------------
    double positioning_tolerance = 0.003;
    KDL::Vector destination_cone_position;

    // when the active constraints is suddenly enabled the tool makes some
    // initial large movements. To prevent always having a peak at the
    // beginning of the error, data we first activate the active constraint
    // and wait till the error is small before we start the task.
    //
    // First: if the tool is placed close to the starting point while being idle
    // we enable the active constraint.
    if (task_state == TaskKidneyState::Idle && with_guidance &&
        (ring_center[0] - start_point).Norm() <
        positioning_tolerance) {
        // Make sure the active constraint is inactive
        if (ac_parameters.active == 0) {
            ac_parameters.active = 1;
            ac_params_changed = true;
        }
    }

    // then when the error is small we start the task.
    // if guidance is off, then we don't need a restriction on the error
    // sorry it is totally unreadable!
    if (task_state == TaskKidneyState::Idle
        && ( (position_error_norm[0] < 0.001 && ac_parameters.active == 1)
             ||
             (!with_guidance && (ring_center[0] - start_point).Norm() <
                                positioning_tolerance)  ))
    {
        task_state = TaskKidneyState::ToEndPoint;
        //increment the repetition number
        number_of_repetition ++;
        // save starting time
        start_time = ros::Time::now();
        // reset score related vars
        ResetOnGoingEvaluation();
    }

        // If the tool reaches the end point the user needs to go back to
        // the starting point. This counts as a separate repetition of the task
    else if (task_state == TaskKidneyState::ToEndPoint &&
             (ring_center[0] - end_point).Norm() <
             positioning_tolerance) {
        task_state = TaskKidneyState::ToStartPoint;
        //increment the repetition number
        number_of_repetition++;

        // calculate and save the score of this repetition
        CalculateAndSaveError();

        // save starting time
        start_time = ros::Time::now();
        // reset score related vars
        ResetOnGoingEvaluation();
    }
        // If the tool reaches the start point while in ToStartPoint state,
        // we can mark the task complete
    else if (task_state == TaskKidneyState::ToStartPoint &&
             (ring_center[0] - start_point).Norm() <
             positioning_tolerance) {
        task_state = TaskKidneyState::RepetitionComplete;
        // calculate and save the score of this repetition
        CalculateAndSaveError();

        ac_parameters.active = 0;
        ac_params_changed = true;
    }

        // User needs to get away from the starting point to switch to idle
        // and in case another repetition is to be performed, the user can
        // flag that by going to the starting position again
    else if (task_state == TaskKidneyState::RepetitionComplete &&
             (ring_center[0] - idle_point).Norm() <
             positioning_tolerance) {
        task_state = TaskKidneyState::Idle;
    }

    // show the destination to the user
    double dt = sin(2 * M_PI * double(destination_ring_counter) / 120);
    destination_ring_counter++;
    mesh_actor->SetScale(1+ 0.03*dt);

    // -------------------------------------------------------------------------
    // Performance Metrics
    UpdateTubeColor();

    // Populate the task state message
    task_state_msg.task_name = "BuzzWire";
    task_state_msg.task_state = (uint8_t)task_state;
    task_state_msg.number_of_repetition = number_of_repetition;
    if (task_state == TaskKidneyState::ToStartPoint
        || task_state == TaskKidneyState::ToEndPoint) {

        task_state_msg.time_stamp = (ros::Time::now() - start_time).toSec();
        task_state_msg.error_field_1 = position_error_norm[0];
        task_state_msg.error_field_1 = orientation_error_norm[0];
        //        if(bimanual)
        //            task_state_msg.error_field_2 = position_error_norm[1];

        // calculate score to show to user
        if (bimanual) {
            posit_error_sum +=
                    0.5 * (position_error_norm[0] +
                           position_error_norm[1]);
            orient_error_sum +=
                    0.5 *(orientation_error_norm[0] +
                          orientation_error_norm[1]);
        }
        else {
            posit_error_sum += position_error_norm[0];
            orient_error_sum += orientation_error_norm[0];
        }

        if(posit_error_max < position_error_norm[0])
            posit_error_max = position_error_norm[0];

        sample_count++;

    } else {
        task_state_msg.time_stamp = 0.0;
        task_state_msg.error_field_1 = 0.0;
        task_state_msg.error_field_2 = 0.0;
    }

}

//------------------------------------------------------------------------------
void TaskKidney::CalculatedDesiredToolPose() {
    // NOTE: All the closest points are on the wire mesh

    //---------------------------------------------------------------------
    // Find the desired orientation
    // We use two vectors to estimate the tangent of the direction of the
    // wire. One is from the grip point (current tool tip) to its closest
    // point on the wire and the other is from a point on the side of the
    // ring (90 deg from the grip point). The estimated tangent is the
    // cross product of these two (after normalization). This is just a
    // quick the non-ideal approximation.
    // Note that we could have used the central point instead of the tool
    // point but that vector gets pretty small and unstable when we're
    // close to the desired pose.
    // WHen I added the second tool things got a little bit messy!
    // Since I had to rotate the sing for 90 degrees the diserd axes
    // were different for each ring and it ended up with too many
    // hardcoded transforms, I will hopefully clean this out later. TODO

    for (int k = 0; k < 1 + (int)bimanual; ++k) {

        // make a copy of the current pose
        KDL::Frame tool_current_pose;

        //Find the closest cell to the grip point
        double grip_point[3] = {(tool_current_pose).p[0],
                                (tool_current_pose).p[1],
                                (tool_current_pose).p[2]};

        double closest_point[3] = {0.0, 0.0, 0.0};
        double closestPointDist2; //the squared distance to the closest point
        vtkIdType cell_id; //the cell id of the cell containing the closest point
        int subId;

        cellLocator->Update();
        cellLocator->FindClosestPoint(grip_point, closest_point, cell_id, subId,
                                      closestPointDist2);
        closest_point_to_grip_point[k] = KDL::Vector(closest_point[0],
                                                     closest_point[1],
                                                     closest_point[2]);

        //Find the closest cell to the the central point
        double ring_central_point[3] = {ring_center[k][0],
                                        ring_center[k][1],
                                        ring_center[k][2]};
        cellLocator->Update();
        cellLocator->FindClosestPoint(ring_central_point, closest_point,
                                      cell_id,
                                      subId, closestPointDist2);
        closest_point_to_ring_center[k] = KDL::Vector(closest_point[0],
                                                      closest_point[1],
                                                      closest_point[2]);

        //Find the closest cell to the radial tool point
        KDL::Vector radial_tool_point_kdl;
        if(k==0)
            radial_tool_point_kdl =
                    tool_current_pose *
                    KDL::Vector(ring_radius, 0.0, ring_radius);
        else
            radial_tool_point_kdl =
                    tool_current_pose *
                    KDL::Vector(ring_radius, ring_radius, 0.0);

        double radial_tool_point[3] = {radial_tool_point_kdl[0],
                                       radial_tool_point_kdl[1],
                                       radial_tool_point_kdl[2]};

        cellLocator->Update();
        cellLocator->FindClosestPoint(radial_tool_point, closest_point, cell_id,
                                      subId, closestPointDist2);
        closest_point_to_radial_point[k] = KDL::Vector(closest_point[0],
                                                       closest_point[1],
                                                       closest_point[2]);

        // Find the vector from ring center to the corresponding closest point on
        // the wire
        KDL::Vector ring_center_to_cp =
                closest_point_to_ring_center[k] - ring_center[k];

        // desired pose only when the ring is close to the wire.if it is too
        // far we don't want fixtures
        if (ring_center_to_cp.Norm() < 5 * ring_radius) {

            // Desired position is one that puts the center of the wire on the
            // center of the ring.

            //---------------------------------------------------------------------
            // Find the desired position
            KDL::Vector wire_center = ring_center[k] + ring_center_to_cp;
            // Turns out trying to estimate the center of the wire makes things
            // worse so I removed the following term that was used in finding
            // wire_center:
            // wire_radius_ * (ring_center_to_cp/ring_center_to_cp.Norm());

            tool_desired_pose_kdl[k].p =
                    (tool_current_pose).p + wire_center -
                    ring_center[k];

            KDL::Vector radial_to_cp =
                    closest_point_to_radial_point[k] - radial_tool_point_kdl;

            KDL::Vector desired_z, desired_y, desired_x;

            KDL::Vector grip_to_cp =
                    closest_point_to_grip_point[k] -
                    (tool_current_pose).p;
            if(k==0) {
                desired_z = grip_to_cp / grip_to_cp.Norm();
                desired_x = -radial_to_cp / radial_to_cp.Norm();
                desired_y = desired_z * desired_x;

                // make sure axes are perpendicular and normal
                desired_y = desired_y / desired_y.Norm();
                desired_x = desired_y * desired_z;
                desired_x = desired_x / desired_x.Norm();
                desired_z = desired_x * desired_y;
                desired_z = desired_z / desired_z.Norm();
            }
            else {
                desired_y = grip_to_cp / grip_to_cp.Norm();
                desired_x = -radial_to_cp / radial_to_cp.Norm();
                desired_z = desired_x * desired_y;

                // make sure axes are perpendicular and normal
                desired_z = desired_z / desired_z.Norm();
                desired_x = desired_y * desired_z;
                desired_x = desired_x / desired_x.Norm();
                desired_y = desired_z * desired_x;
                desired_y = desired_y / desired_y.Norm();
            }
            tool_desired_pose_kdl[k].M = KDL::Rotation(desired_x, desired_y,
                                                       desired_z);

            //------------------------------------------------------------------
            // Calculate errors
            position_error_norm[k] = (wire_center - ring_center[k]).Norm();
            KDL::Vector rpy;
            (tool_desired_pose_kdl[k].M *
             (tool_current_pose).M.Inverse() ).GetRPY(rpy[0],
                                                      rpy[1],
                                                      rpy[2]);
            orientation_error_norm[k] = rpy.Norm();

        } else {
            tool_desired_pose_kdl[k] = tool_current_pose;
            // due to the delay in teleop loop this will create some wrneches if
            // the guidance is still active
        }

    }

}

//------------------------------------------------------------------------------
void TaskKidney::UpdateTubeColor() {

    double max_pos_error = 0.002;
    double max_orient_error = 0.3;
    // orientation error is tricky to perceive, so we weigh it half the
    // position error
    double error_ratio = ( (orientation_error_norm[0] / max_orient_error)
                           + 2* (position_error_norm[0] / max_pos_error)) /3;

    if (error_ratio > 1.3)
        error_ratio = 1.3;
    else if(error_ratio < 0.3)
        error_ratio = 0.3;

//    score_sphere_actors->GetProperty()->SetColor(error_ratio, 1 - error_ratio,
//                                                0.1);
//    if(task_state== TaskKidneyState::ToEndPoint
//       || task_state== TaskKidneyState::ToStartPoint)
//        tube_mesh_actor->GetProperty()->SetColor(0.9,
//                                                 0.5- 0.4*(error_ratio-0.3),
//                                                 0.1);

}

custom_msgs::TaskState TaskKidney::GetTaskStateMsg() {
    return task_state_msg;
}

void TaskKidney::ResetTask() {
    ROS_INFO("Resetting the task.");
    number_of_repetition = 0;
    task_state = TaskKidneyState::RepetitionComplete;
    ResetOnGoingEvaluation();
    ResetScoreHistory();
}

void TaskKidney::ResetCurrentAcquisition() {
    ROS_INFO("Resetting current acquisition.");
    if(task_state== TaskKidneyState::ToEndPoint ||
       task_state == TaskKidneyState::ToStartPoint){

        ResetOnGoingEvaluation();
        if(number_of_repetition>0)
            number_of_repetition--;
        task_state = TaskKidneyState::RepetitionComplete;

    }
}

void TaskKidney::HapticsThread() {

    ros::Publisher pub_desired[2];

    ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
    pub_desired[0] = node->advertise<geometry_msgs::PoseStamped>
            ("/PSM1/tool_pose_desired", 10);
    if(bimanual)
        pub_desired[1] = node->advertise<geometry_msgs::PoseStamped>
                ("/PSM2/tool_pose_desired", 10);

    ros::Rate loop_rate(200);

    while (ros::ok())
    {
        VTKConversions::KDLFrameToVTKMatrix(*tool_current_pose_kdl[0],
                                            tool_current_pose[0]);
        // find the center of the ring
        ring_center[0] = *tool_current_pose_kdl[0] *
                         KDL::Vector(0.0, 0.0,ring_radius);

        if(bimanual){
            VTKConversions::KDLFrameToVTKMatrix(*tool_current_pose_kdl[1],
                                                tool_current_pose[1]);
            ring_center[1] = *tool_current_pose_kdl[1] *
                             KDL::Vector(0.0, ring_radius, 0.0);
        }

        CalculatedDesiredToolPose();

        // publish desired poses
        for (int n_arm = 0; n_arm < 1+ int(bimanual); ++n_arm) {

            // convert to pose message
            geometry_msgs::PoseStamped pose_msg;
            tf::poseKDLToMsg(tool_desired_pose_kdl[n_arm], pose_msg.pose);
            // fill the header
            pose_msg.header.frame_id = "/task_space";
            pose_msg.header.stamp = ros::Time::now();
            // publish
            pub_desired[n_arm].publish(pose_msg);
        }

        ros::spinOnce();
        loop_rate.sleep();
        boost::this_thread::interruption_point();
    }
}

void TaskKidney::CalculateAndSaveError() {

    double duration = (ros::Time::now() - start_time).toSec();
    double posit_error_avg = posit_error_sum/(double)sample_count;
    double orient_error_avg = orient_error_sum/(double)sample_count;

    double posit_error_avg_ideal = 0.0005;
    double orient_error_avg_ideal = 0.35;
    double posit_error_max_ideal = 0.0015;
    double duration_ideal = 9.0;

    // put a threshold on the values
    if (posit_error_avg < posit_error_avg_ideal)
        posit_error_avg = posit_error_avg_ideal;

    if (orient_error_avg < orient_error_avg_ideal)
        orient_error_avg = orient_error_avg_ideal;

    if (posit_error_max < posit_error_max_ideal)
        posit_error_max = posit_error_max_ideal;

    if (duration < duration_ideal)
        duration = duration_ideal;

    double score = ( posit_error_avg_ideal/posit_error_avg
                     + posit_error_max_ideal/posit_error_max
                     + duration_ideal/duration
                     + orient_error_avg_ideal/orient_error_avg)
                   * 100 / 4;

    // when the history gets full we start a new set
    if (score_history.size() == n_score_history)
        ResetScoreHistory();

    score_history.push_back(score);

    ROS_INFO("posit_error_max: %f", posit_error_max);
    ROS_INFO("duration: %f", duration);
    ROS_INFO("posit_error_avg: %f", posit_error_avg);
    ROS_INFO("orient_error_avg: %f", orient_error_avg);
    ROS_INFO("Score: %f", score);
    ROS_INFO("  ");
}

double * TaskKidney::GetScoreColor(const double score) {

    //decide the color
    if(score > 90){
        return COLORS::Green;
    }
    else if (score > 80)
        return COLORS::Gold;
    else if(score > 60)
        return COLORS::Orange;
    else
        return COLORS::Red;

}

void TaskKidney::ResetOnGoingEvaluation() {
    posit_error_sum = 0.0;
    posit_error_max = 0.0;
    orient_error_sum = 0.0;
    sample_count = 0;
}

void TaskKidney::ResetScoreHistory() {
    score_history.clear();

}
//TaskKidney::~TaskKidney() {
//    CloseODE();
//}

max_stars_repo_name: neemoh/ART

max_issues_repo_name: liuxia-zju/ATAR

max_forks_repo_name: liuxia-zju/ATAR


text:
/**
 * @brief Framebuffer for Vulkan
 */

#include "VK/Framebuffer.h"

#include <array>
#include <boost/assert.hpp>

#include "VK/Common.h"
#include "VK/Device.h"
#include "VK/Initializer.h"
#include "VK/Utils.h"

void Framebuffer::Destroy(const Device &device) const {
  vkDestroyFramebuffer(device, framebuffer, nullptr);
  vkDestroyRenderPass(device, renderPass, nullptr);
  vkDestroySampler(device, sampler, nullptr);
  for (const auto &attachment : attachments) {
    vkDestroyImageView(device, attachment.view, nullptr);
    vkFreeMemory(device, attachment.memory, nullptr);
    vkDestroyImage(device, attachment.image, nullptr);
  }
}

uint32_t
Framebuffer::AddAttachment(const Device &device,
                           const AttachmentCreateInfo &attachmentCreateInfo) {
  FramebufferAttachment framebufferAttachment{};
  framebufferAttachment.format = attachmentCreateInfo.format;

  VkImageAspectFlags aspectMask = 0;
  // カラーアタッチメント
  if (attachmentCreateInfo.usage & VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT) {
    aspectMask = VK_IMAGE_ASPECT_COLOR_BIT;
  }
  // デプスステンシルアタッチメント
  if (attachmentCreateInfo.usage &
      VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT) {
    if (framebufferAttachment.HasDepth()) {
      aspectMask = VK_IMAGE_ASPECT_DEPTH_BIT;
    }
    if (framebufferAttachment.HasStencil()) {
      aspectMask |= VK_IMAGE_ASPECT_STENCIL_BIT;
    }
  }
  BOOST_ASSERT(aspectMask > 0);

  VK_CHECK_RESULT(CreateImage(
      device, framebufferAttachment.image, framebufferAttachment.memory,
      attachmentCreateInfo.format, VK_IMAGE_TYPE_2D, attachmentCreateInfo.width,
      attachmentCreateInfo.height, 1, 1, attachmentCreateInfo.layerCount,
      VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT, attachmentCreateInfo.usage,
      VK_IMAGE_TILING_OPTIMAL, attachmentCreateInfo.imageSampleCount));

  framebufferAttachment.subresourceRange = {};
  framebufferAttachment.subresourceRange.aspectMask = aspectMask;
  framebufferAttachment.subresourceRange.levelCount = 1;
  framebufferAttachment.subresourceRange.layerCount =
      attachmentCreateInfo.layerCount;
  const VkImageViewType imageViewType = attachmentCreateInfo.layerCount == 1
                                            ? VK_IMAGE_VIEW_TYPE_2D
                                            : VK_IMAGE_VIEW_TYPE_2D_ARRAY;
  VK_CHECK_RESULT(CreateImageView(
      device, framebufferAttachment.view, framebufferAttachment.image,
      imageViewType, attachmentCreateInfo.format,
      framebufferAttachment.subresourceRange.aspectMask,
      framebufferAttachment.subresourceRange.baseMipLevel,
      framebufferAttachment.subresourceRange.levelCount,
      framebufferAttachment.subresourceRange.baseArrayLayer,
      framebufferAttachment.subresourceRange.layerCount));

  framebufferAttachment.description = {};
  framebufferAttachment.description.samples =
      attachmentCreateInfo.imageSampleCount;
  framebufferAttachment.description.loadOp = VK_ATTACHMENT_LOAD_OP_CLEAR;
  framebufferAttachment.description.storeOp =
      (attachmentCreateInfo.usage & VK_IMAGE_USAGE_SAMPLED_BIT)
          ? VK_ATTACHMENT_STORE_OP_STORE
          : VK_ATTACHMENT_STORE_OP_DONT_CARE;
  framebufferAttachment.description.stencilLoadOp =
      VK_ATTACHMENT_LOAD_OP_DONT_CARE;
  framebufferAttachment.description.stencilStoreOp =
      VK_ATTACHMENT_STORE_OP_DONT_CARE;
  framebufferAttachment.description.format = attachmentCreateInfo.format;
  framebufferAttachment.description.initialLayout = VK_IMAGE_LAYOUT_UNDEFINED;
  // 最終的なレイアウトはアタッチメントのタイプによって異なります。
  framebufferAttachment.description.finalLayout =
      framebufferAttachment.IsDepthStencil()
          ? VK_IMAGE_LAYOUT_DEPTH_STENCIL_ATTACHMENT_OPTIMAL
          : VK_IMAGE_LAYOUT_SHADER_READ_ONLY_OPTIMAL;
  attachments.emplace_back(framebufferAttachment);
  return static_cast<uint32_t>(attachments.size() - 1);
}

VkResult Framebuffer::CreateSampler(const Device &device, VkFilter magFilter,
                                    VkFilter minFilter,
                                    VkSamplerAddressMode addressMode) {
  return ::CreateSampler(device, sampler, magFilter, minFilter, VK_FALSE,
                         VK_COMPARE_OP_LESS_OR_EQUAL, addressMode, addressMode,
                         addressMode, VK_SAMPLER_MIPMAP_MODE_LINEAR, 0.0f,
                         1.0f);
}

VkResult Framebuffer::CreateRenderPass(const Device &device) {
  std::vector<VkAttachmentDescription> attachmentDescriptions;
  for (const auto &attachment : attachments) {
    attachmentDescriptions.emplace_back(attachment.description);
  }

  std::vector<VkAttachmentReference> colorReferences;
  VkAttachmentReference depthReference{};
  bool hasDepth = false;
  bool hasColor = false;
  uint32_t attachmentIdx = 0;
  for (const auto &attachment : attachments) {
    if (attachment.IsDepthStencil()) {
      // 深度アタッチメントは一つのみ許されます。
      BOOST_ASSERT(!hasDepth);
      depthReference.attachment = attachmentIdx;
      depthReference.layout = VK_IMAGE_LAYOUT_DEPTH_STENCIL_ATTACHMENT_OPTIMAL;
      hasDepth = true;
    } else {
      colorReferences.emplace_back(VkAttachmentReference{
          attachmentIdx, VK_IMAGE_LAYOUT_COLOR_ATTACHMENT_OPTIMAL});
      hasColor = true;
    }
    attachmentIdx++;
  }

  // デフォルトのレンダーパス設定では、1つのサブパスのみが使用されます。
  VkSubpassDescription subpass{};
  subpass.pipelineBindPoint = VK_PIPELINE_BIND_POINT_GRAPHICS;
  if (hasColor) {
    subpass.pColorAttachments = colorReferences.data();
    subpass.colorAttachmentCount =
        static_cast<uint32_t>(colorReferences.size());
  }
  if (hasDepth) {
    subpass.pDepthStencilAttachment = &depthReference;
  }

  // アタッチメントのレイアウト遷移にサブパスの依存関係を使用します。
  std::array<VkSubpassDependency, 2> subpassDependencies{};

  subpassDependencies[0].srcSubpass = VK_SUBPASS_EXTERNAL;
  subpassDependencies[0].dstSubpass = 0;
  subpassDependencies[0].srcStageMask = VK_PIPELINE_STAGE_BOTTOM_OF_PIPE_BIT;
  subpassDependencies[0].dstStageMask =
      VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT;
  subpassDependencies[0].srcAccessMask = VK_ACCESS_MEMORY_READ_BIT;
  subpassDependencies[0].dstAccessMask = VK_ACCESS_COLOR_ATTACHMENT_READ_BIT |
                                         VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT;
  subpassDependencies[0].dependencyFlags = VK_DEPENDENCY_BY_REGION_BIT;

  subpassDependencies[1].srcSubpass = 0;
  subpassDependencies[1].dstSubpass = VK_SUBPASS_EXTERNAL;
  subpassDependencies[1].srcStageMask =
      VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT;
  subpassDependencies[1].dstStageMask = VK_PIPELINE_STAGE_BOTTOM_OF_PIPE_BIT;
  subpassDependencies[1].srcAccessMask = VK_ACCESS_COLOR_ATTACHMENT_READ_BIT |
                                         VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT;
  subpassDependencies[1].dstAccessMask = VK_ACCESS_MEMORY_READ_BIT;
  subpassDependencies[1].dependencyFlags = VK_DEPENDENCY_BY_REGION_BIT;

  // レンダーパスを生成します。
  VkRenderPassCreateInfo renderPassCreateInfo =
      Initializer::RenderPassCreateInfo();
  renderPassCreateInfo.pAttachments = attachmentDescriptions.data();
  renderPassCreateInfo.attachmentCount =
      static_cast<uint32_t>(attachmentDescriptions.size());
  renderPassCreateInfo.subpassCount = 1;
  renderPassCreateInfo.pSubpasses = &subpass;
  renderPassCreateInfo.dependencyCount =
      static_cast<uint32_t>(subpassDependencies.size());
  renderPassCreateInfo.pDependencies = subpassDependencies.data();
  VK_CHECK_RESULT(
      vkCreateRenderPass(device, &renderPassCreateInfo, nullptr, &renderPass));

  std::vector<VkImageView> attachmentViews;
  for (const auto &attachment : attachments) {
    attachmentViews.emplace_back(attachment.view);
  }

  uint32_t maxLayers = 0;
  for (const auto &attachment : attachments) {
    if (attachment.subresourceRange.layerCount > maxLayers) {
      maxLayers = attachment.subresourceRange.layerCount;
    }
  }

  VkFramebufferCreateInfo framebufferCreateInfo =
      Initializer::FramebufferCreateInfo();
  framebufferCreateInfo.renderPass = renderPass;
  framebufferCreateInfo.pAttachments = attachmentViews.data();
  framebufferCreateInfo.attachmentCount =
      static_cast<uint32_t>(attachmentViews.size());
  framebufferCreateInfo.width = width;
  framebufferCreateInfo.height = height;
  framebufferCreateInfo.layers = maxLayers;
  VK_CHECK_RESULT(vkCreateFramebuffer(device, &framebufferCreateInfo, nullptr,
                                      &framebuffer));

  return VK_SUCCESS;
}

ext: cc

max_stars_repo_name: mnrn/ReVK

max_issues_repo_name: mnrn/ReVK

max_forks_repo_name: mnrn/ReVK


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

#include <vector>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

#include "base/config.h"
#include "base/logger.h"
#include "base/request.h"
#include "base/timer.h"
#include "crypto/base64.h"
#include "crypto/encoder.h"
#include "crypto/hmac_sha1.h"
#include "crypto/private_file.h"
#include "services/aws/file_transfer.h"
#include "services/aws/impl.h"

using boost::is_any_of;
using boost::shared_ptr;
using boost::token_compress_on;
using std::ifstream;
using std::runtime_error;
using std::string;
using std::vector;

using s3::base::config;
using s3::base::header_map;
using s3::base::request;
using s3::base::timer;
using s3::crypto::base64;
using s3::crypto::encoder;
using s3::crypto::hmac_sha1;
using s3::crypto::private_file;
using s3::services::file_transfer;
using s3::services::aws::impl;

namespace
{
  const string HEADER_PREFIX = "x-amz-";
  const string HEADER_META_PREFIX = "x-amz-meta-";

  const string EMPTY = "";

  const string & safe_find(const header_map &map, const char *key)
  {
    header_map::const_iterator itor = map.find(key);

    return (itor == map.end()) ? EMPTY : itor->second;
  }
}

impl::impl()
{
  ifstream f;
  string line;
  vector<string> fields;

  private_file::open(config::get_aws_secret_file(), &f);
  getline(f, line);

  split(fields, line, is_any_of(string(" \t")), token_compress_on);

  if (fields.size() != 2) {
    S3_LOG(
      LOG_ERR,
      "impl::impl",
      "expected 2 fields for aws_secret_file, found %i.\n",
      fields.size());

    throw runtime_error("error while parsing auth data for AWS.");
  }

  _key = fields[0];
  _secret = fields[1];

  _endpoint = config::get_aws_use_ssl() ? "https://" : "http://";
  _endpoint += config::get_aws_service_endpoint();

  _bucket_url = string("/") + request::url_encode(config::get_bucket_name());
}

const string & impl::get_header_prefix()
{
  return HEADER_PREFIX;
}

const string & impl::get_header_meta_prefix()
{
  return HEADER_META_PREFIX;
}

const string & impl::get_bucket_url()
{
  return _bucket_url;
}

bool impl::is_next_marker_supported()
{
  return true;
}

void impl::sign(request *req)
{
  const header_map &headers = req->get_headers();
  string date, to_sign;
  uint8_t mac[hmac_sha1::MAC_LEN];

  date = timer::get_http_time();

  req->set_header("Date", date);

  to_sign =
    req->get_method() + "\n" +
    safe_find(headers, "Content-MD5") + "\n" +
    safe_find(headers, "Content-Type") + "\n" +
    date + "\n";

  for (header_map::const_iterator itor = headers.begin(); itor != headers.end(); ++itor)
    if (!itor->second.empty() && itor->first.substr(0, HEADER_PREFIX.size()) == HEADER_PREFIX)
      to_sign += itor->first + ":" + itor->second + "\n";

  to_sign += req->get_url();

  hmac_sha1::sign(_secret, to_sign, mac);
  req->set_header("Authorization", string("AWS ") + _key + ":" + encoder::encode<base64>(mac, hmac_sha1::MAC_LEN));
}

string impl::adjust_url(const string &url)
{
  return _endpoint + url;
}

void impl::pre_run(request *r, int iter)
{
  sign(r);
}

shared_ptr<file_transfer> impl::build_file_transfer()
{
  return shared_ptr<file_transfer>(new aws::file_transfer());
}


text:
#include <CCeilPI.h>
#include <COSNaN.h>
#include <CInvNorm.h>
#include <boost/math/special_functions/erf.hpp>
#include <cerrno>

void
ClParserReal::
copy(const ClParserObj &obj)
{
  assert(base_type_ == obj.getBaseType());

  const ClParserReal &rhs = castObj(obj);

  *this = rhs;
}

ClParserReal *
ClParserReal::
dup() const
{
  return new ClParserReal(*this);
}

bool
ClParserReal::
isNanValue() const
{
  if (COSNaN::is_nan(real_))
    return true;

  return false;
}

long
ClParserReal::
getInteger() const
{
  // TODO: range check

  if (! isNanValue())
    return long(real_);
  else
    return 0;
}

bool
ClParserReal::
toBool() const
{
  return (! isNanValue() && real_ != 0.0);
}

//-----------------

// Inline Ops

const ClParserObj &
ClParserReal::
increment()
{
  if (! isNanValue())
    ++real_;

  return *this;
}

const ClParserObj &
ClParserReal::
decrement()
{
  if (! isNanValue())
    --real_;

  return *this;
}

//-----------------

// Unary Ops

ClParserValuePtr
ClParserReal::
unaryPlus() const
{
  return ClParserValueMgrInst->createValue( real_);
}

ClParserValuePtr
ClParserReal::
unaryMinus() const
{
  return ClParserValueMgrInst->createValue(-real_);
}

ClParserValuePtr
ClParserReal::
bitNot() const
{
  if (! isNanValue())
    return ClParserValueMgrInst->createValue(~getInteger());
  else
    return ClParserValueMgrInst->createValue( long(0    ));
}

//-----------------

// Binary Ops

ClParserValuePtr
ClParserReal::
plus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ + real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
minus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ - real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
times(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ * real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
divide(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result;

  if      (real.real_ == 0.0) {
    if (ClParserInst->getMathFail())
      ClErrThrow(ClErr::DIVIDE_BY_ZERO);

    if (real_ < 0.0)
      SetNegInf(result);
    else
      SetPosInf(result);
  }
  else if (isNanValue() || real.isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ / real.real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
modulus(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  errno = 0;

  double result = CMathGen::modulus(real_, real.real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
power(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  errno = 0;

  double result = CMathGen::pow(real_, real.real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
approxEqual(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(::fabs(real_ - real.real_) < ClParserInst->getTolerance());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitAnd(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() & real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitOr(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() | real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitXor(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  long result = long(getInteger() ^ real.getInteger());

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitLShift(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result = real_*CMathGen::pow(2.0, real.real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
bitRShift(const ClParserObj &obj) const
{
  const ClParserReal &real = castObj(obj);

  double result = real_/CMathGen::pow(2.0, real.real_);

  return ClParserValueMgrInst->createValue(result);
}

//-----------------

// Intern Fn

ClParserValuePtr
ClParserReal::
abs() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else if (real_ < 0.0)
    result = -real_;
  else
    result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
ceil() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::ceil(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
floor() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::floor(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sign() const
{
  double result;

  if      (isNanValue())
    COSNaN::set_nan(&result);
  else if (real_ > 0.0)
    result =  1.0;
  else if (real_ < 0.0)
    result = -1.0;
  else
    result =  0.0;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sqr() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = real_ * real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sqrt() const
{
  errno = 0;

  double result = CMathGen::sqrt(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
cos() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::cos(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sin() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::sin(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
tan() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::tan(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
acos() const
{
  errno = 0;

  double result = ClParserInst->fromRadians(CMathGen::acos(real_));

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
asin() const
{
  errno = 0;

  double result = ClParserInst->fromRadians(CMathGen::asin(real_));

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
atan() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ClParserInst->fromRadians(::atan(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
atan(double real) const
{
  double result;

  if (isNanValue() || COSNaN::is_nan(real))
    COSNaN::set_nan(&result);
  else
    result = ClParserInst->fromRadians(::atan2(real_, real));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
exp() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::exp(real_);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
log() const
{
  errno = 0;

  double result = CMathGen::log(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
log10() const
{
  errno = 0;

  double result = CMathGen::log10(real_);

  if (errno != 0 && ClParserInst->getMathFail())
    ClErrThrow(ClErr::DIVIDE_BY_ZERO);

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
cosh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::cosh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sinh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::sinh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
tanh() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else
    result = ::tanh(ClParserInst->toRadians(real_));

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
norm() const
{
  double result = real_;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else {
    result = 0.5*std::sqrt(2.0)*result;
    result = 0.5*erfc(-result);
  }

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
invnorm() const
{
  double result;

  if (isNanValue())
    COSNaN::set_nan(&result);
  else {
    result = CInvNorm::calc(real_);

    if (IsPosInf(result) || IsNegInf(result))
      SetNaN(result);
  }

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toChar() const
{
  std::string result(" ");

  result[0] = (char) getInteger();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toInt() const
{
  long result = getInteger();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toReal() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toString() const
{
  std::string result = asString();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
isNan() const
{
  long result = isNanValue();

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
toLower() const
{
  int c = (int) getInteger();

  if (isupper(c))
    c = tolower(c);

  long value = (long) c;

  return ClParserValueMgrInst->createValue(value);
}

ClParserValuePtr
ClParserReal::
toUpper() const
{
  int c = (int) getInteger();

  if (islower(c))
    c = toupper(c);

  long value = (long) c;

  return ClParserValueMgrInst->createValue(value);
}

ClParserValuePtr
ClParserReal::
min() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
max() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
sum() const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
index(const ClParserObj &) const
{
  ClErrThrow(ClErr::INVALID_TYPE_FOR_OPERATOR);

  return ClParserValuePtr();
}

ClParserValuePtr
ClParserReal::
rindex(const ClParserObj &) const
{
  ClErrThrow(ClErr::INVALID_TYPE_FOR_OPERATOR);

  return ClParserValuePtr();
}

ClParserValuePtr
ClParserReal::
sort(ClParserSortDirection) const
{
  double result = real_;

  return ClParserValueMgrInst->createValue(result);
}

ClParserValuePtr
ClParserReal::
doAssert() const
{
  assert(real_);

  return ClParserValuePtr();
}

//-----------------

int
ClParserReal::
cmp(const ClParserObj &obj) const
{
  if (base_type_ != obj.getBaseType())
    return CMathGen::sign((long) (base_type_ - obj.getBaseType()));

  const ClParserReal &rhs = castObj(obj);

  if      (isNanValue() && rhs.isNanValue())
    return 1;
  else if (isNanValue())
    return 1;
  else if (rhs.isNanValue())
    return -1;
  else if (real_ < rhs.real_)
    return -1;
  else if (real_ > rhs.real_)
    return 1;
  else
    return 0;
}

//------

std::string
ClParserReal::
asString() const
{
  if      (IsPosInf(real_))
    return "+Inf";
  else if (IsNegInf(real_))
    return "-Inf";
  else if (isNanValue())
    return "NaN";
  else
    return ClParserInst->toString(real_);
}

void
ClParserReal::
print() const
{
  ClParserInst->output("%s", asString().c_str());
}

void
ClParserReal::
print(std::ostream &os) const
{
  os << asString();
}

void
ClParserReal::
debugPrint() const
{
  fprintf(stderr, " %s", asString().c_str());
}

ext: cpp


text:
#pragma once

#include "Conversions.h"

#include <boost/multiprecision/cpp_int.hpp>

namespace rail
{
    template<typename Num, typename ByteArray>
    Num Conversions::hexBytesToInt(const ByteArray& input)
    {
        Num outputInteger(0);
        auto shiftBy = 0;
        for (auto byte : input)
        {
            outputInteger <<= shiftBy;
            outputInteger |= byte;
            shiftBy = CHAR_BIT;
        }
        return outputInteger;
    }
}

ext: ipp

max_stars_repo_name: CathalT/Rail

max_issues_repo_name: CathalT/Rail

max_forks_repo_name: CathalT/Rail


text:
#include <gtest/gtest.h>

#include "converter/fixml2fix_converter.hxx"
#include "converter/xml_element_helper.hxx"
#include "converter/fix_helper.hxx"
#include "util/fix_env.hxx"
#include "tools/test_util.hxx"

#include <boost/log/trivial.hpp>

#include <quickfix/fix50sp2/StreamAssignmentReportACK.h>

#include <list>
#include <set>
#include <string>
#include <utility>

using namespace std;
using namespace fix2xml;

TEST ( StreamAssignmentReportACK, set_fields)
{

  fixml2fix_converter converter {"../spec/fix/FIX50SP2.xml", "../spec/xsd/fixml-main-5-0-SP2.xsd"};
  auto& fixml_dict = converter.fixml_dico();
  ASSERT_TRUE(converter.init());
  ASSERT_TRUE(converter.parse_fixt_dico("../spec/fix/FIXT11.xml"));
  FIX50SP2::StreamAssignmentReportACK msg;

  list<multiset<string>> all_values;
  multiset<string> all_compo_names;
  multiset<string> StreamAssignmentReportACK_0;
  set_field(msg, FIX::EncodedText{"DATA_1970694520"}, StreamAssignmentReportACK_0);
  set_field(msg, FIX::EncodedTextLen{1169773738}, StreamAssignmentReportACK_0);
  set_field(msg, FIX::StreamAsgnAckType{0}, StreamAssignmentReportACK_0);
  set_field(msg, FIX::StreamAsgnRejReason{99}, StreamAssignmentReportACK_0);
  set_field(msg, FIX::StreamAsgnRptID{"STRING_1087523569"}, StreamAssignmentReportACK_0);
  set_field(msg, FIX::Text{"STRING_1794885114"}, StreamAssignmentReportACK_0);
  all_values.push_back(StreamAssignmentReportACK_0);

  all_compo_names.insert("StreamAssignmentReportACK");

  // header
  multiset<string> header_94;
  set_header_field(msg.getHeader(), FIX::ApplVerID{"STRING_7"}, header_94);
  set_header_field(msg.getHeader(), FIX::BeginString{"STRING_537021290"}, header_94);
  set_header_field(msg.getHeader(), FIX::BodyLength{11805103}, header_94);
  set_header_field(msg.getHeader(), FIX::CstmApplVerID{"STRING_1348937329"}, header_94);
  set_header_field(msg.getHeader(), FIX::DeliverToCompID{"STRING_1699337286"}, header_94);
  set_header_field(msg.getHeader(), FIX::DeliverToLocationID{"STRING_1881462531"}, header_94);
  set_header_field(msg.getHeader(), FIX::DeliverToSubID{"STRING_553057900"}, header_94);
  set_header_field(msg.getHeader(), FIX::LastMsgSeqNumProcessed{1992841511}, header_94);
  set_header_field(msg.getHeader(), FIX::MessageEncoding{"STRING_UTF-8"}, header_94);
  set_header_field(msg.getHeader(), FIX::MsgSeqNum{1768275025}, header_94);
  set_header_field(msg.getHeader(), FIX::OnBehalfOfCompID{"STRING_1055573177"}, header_94);
  set_header_field(msg.getHeader(), FIX::OnBehalfOfLocationID{"STRING_968674906"}, header_94);
  set_header_field(msg.getHeader(), FIX::OnBehalfOfSubID{"STRING_962358282"}, header_94);
  set_header_field(msg.getHeader(), FIX::OrigSendingTime{FIX::UTCTIMESTAMP(22, 42, 28, 4, 9, 2000)}, header_94);
  set_header_field(msg.getHeader(), FIX::PossDupFlag{true}, header_94);
  set_header_field(msg.getHeader(), FIX::PossResend{false}, header_94);
  set_header_field(msg.getHeader(), FIX::SecureData{"DATA_821430918"}, header_94);
  set_header_field(msg.getHeader(), FIX::SecureDataLen{1198896122}, header_94);
  set_header_field(msg.getHeader(), FIX::SenderCompID{"STRING_1116262728"}, header_94);
  set_header_field(msg.getHeader(), FIX::SenderLocationID{"STRING_644641790"}, header_94);
  set_header_field(msg.getHeader(), FIX::SenderSubID{"STRING_221186212"}, header_94);
  set_header_field(msg.getHeader(), FIX::SendingTime{FIX::UTCTIMESTAMP(8, 41, 55, 7, 8, 2003)}, header_94);
  set_header_field(msg.getHeader(), FIX::TargetCompID{"STRING_2052080309"}, header_94);
  set_header_field(msg.getHeader(), FIX::TargetLocationID{"STRING_1397584710"}, header_94);
  set_header_field(msg.getHeader(), FIX::TargetSubID{"STRING_179307022"}, header_94);
  set_header_field(msg.getHeader(), FIX::XmlData{"DATA_457654561"}, header_94);
  set_header_field(msg.getHeader(), FIX::XmlDataLen{1242942573}, header_94);
  all_values.push_back(header_94);
  all_compo_names.insert(".header");

  xml_element elt;
  converter.fix2fixml(msg, elt);
  BOOST_LOG_TRIVIAL(debug) << "The resulting XML is";
  cout << "////////////////////////////////////////////" << endl;
  cout << elt.to_string() << endl;
  cout << "////////////////////////////////////////////" << endl << endl;

  BOOST_LOG_TRIVIAL(debug) << "Quickfix XML representation is";
  cout << "////////////////////////////////////////////" << endl;
  cout << msg.toXML() << endl;
  cout << "////////////////////////////////////////////" << endl << endl;
  list<multiset<string>> elt_lists;
  elt.to_list(elt_lists);
  EXPECT_EQ(elt_lists.size(), all_values.size());

  if (elt_lists.size() != all_values.size())  {
    multiset<string> elt_compo_name;
    elt.all_components(elt_compo_name);
    BOOST_LOG_TRIVIAL(debug) << "XML Elements are:";
    cout << "	[";
    copy(elt_compo_name.begin(), elt_compo_name.end(), ostream_iterator<string>(cout, " "));    cout << "]" << endl;

    BOOST_LOG_TRIVIAL(debug) << "FIX Components are:";
    cout << "	[";
    copy(all_compo_names.begin(), all_compo_names.end(), ostream_iterator<string>(cout, " "));    cout << "]" << endl;

  }
  BOOST_LOG_TRIVIAL(debug) << "All FIX components";
  for (const auto& l : all_values) {
    cout << "	[";
    copy(l.begin(), l.end(), ostream_iterator<string>(cout, " "));
    cout << "]" << endl;
  }
  BOOST_LOG_TRIVIAL(debug) << "All XML components";
  for (const auto& l : elt_lists) {
    cout << "	[";
    copy(l.begin(), l.end(), ostream_iterator<string>(cout, " "));
    cout << "]" << endl;

  }

  for (const auto& xml_l : elt_lists) {
    bool found = false;
    for (const auto& l : all_values) {
      if (includes(l.begin(), l.end(), xml_l.begin(), xml_l.end())) {
        found = true;
        break;
      } // end if includes
    } // end for all_values
    EXPECT_TRUE(found);
    if ( ! found) {
      BOOST_LOG_TRIVIAL(debug) << "[TO CHECK] This XML component was not found in FIX message";
      cout << "	 ---> [";
      copy(xml_l.begin(), xml_l.end(), ostream_iterator<string>(cout, " "));      cout << "]" << endl << endl;
    } // end if ! found
  } // end for elt_lists
}


text:
#include <bluetoe/characteristic.hpp>
#include <bluetoe/service.hpp>
#include <bluetoe/encryption.hpp>
#include <bluetoe/client_characteristic_configuration.hpp>

#define BOOST_TEST_MODULE
#include <boost/test/included/unit_test.hpp>
#include "hexdump.hpp"
#include "test_attribute_access.hpp"
#include "test_characteristics.hpp"
#include <boost/mpl/list.hpp>

using cccd_indices = std::tuple<>;
using suuid = bluetoe::service_uuid16< 0x4711 >;

using srv = bluetoe::server<>;

BOOST_AUTO_TEST_SUITE( characteristic_value_access )

    BOOST_FIXTURE_TEST_CASE( simple_value_can_be_read, simple_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t buffer[ 100 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( read, 1 ) );

        static const std::uint8_t expected_value[] = { 0xdd, 0xcc, 0xbb, 0xaa };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( expected_value ), std::end( expected_value ), read.buffer, read.buffer + read.buffer_size );
    }

    BOOST_FIXTURE_TEST_CASE( char_value_returns_invalid_offset_when_read_behind_the_data, simple_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t buffer[ 100 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 5 );

        BOOST_CHECK( bluetoe::details::attribute_access_result::invalid_offset == value_attribute.access( read, 1 ) );
    }

    BOOST_FIXTURE_TEST_CASE( simple_value_can_be_read_with_offset, simple_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t buffer[ 100 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 2 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( read, 1 ) );

        static const std::uint8_t expected_value[] = { 0xbb, 0xaa };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( expected_value ), std::end( expected_value ), read.buffer, read.buffer + read.buffer_size );
    }

    BOOST_FIXTURE_TEST_CASE( simple_value_can_be_read_with_offset_equal_to_length, simple_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t buffer[ 100 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 4 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( read, 1 ) );
        BOOST_CHECK_EQUAL( read.buffer_size, 0u );
    }

    BOOST_FIXTURE_TEST_CASE( simple_value_can_be_written, simple_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t new_value[] = { 0x01, 0x02, 0x03, 0x04 };

        auto write = bluetoe::details::attribute_access_arguments::write( new_value );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( write, 1 ) );

        std::uint8_t buffer[ 4 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( read, 1 ) );

        static const std::uint8_t expected_value[] = { 0x01, 0x02, 0x03, 0x04 };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( expected_value ), std::end( expected_value ), read.buffer, read.buffer + read.buffer_size );
    }

    BOOST_FIXTURE_TEST_CASE( simple_const_value_can_be_read, simple_const_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t buffer[ 100 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::success == value_attribute.access( read, 1 ) );

        static const std::uint8_t expected_value[] = { 0xdd, 0xcc, 0xbb, 0xaa };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( expected_value ), std::end( expected_value ), read.buffer, read.buffer + read.buffer_size );
    }

    BOOST_FIXTURE_TEST_CASE( simple_const_value_can_not_be_writte, simple_const_char )
    {
        const bluetoe::details::attribute value_attribute = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );
        std::uint8_t new_value[] = { 0x01, 0x02, 0x03, 0x04 };

        auto write = bluetoe::details::attribute_access_arguments::write( new_value );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::write_not_permitted == value_attribute.access( write, 1 ) );
    }

    BOOST_AUTO_TEST_CASE( simple_value_without_read_access_can_not_be_read )
    {
        bluetoe::characteristic<
            bluetoe::characteristic_uuid< 0xD0B10674, 0x6DDD, 0x4B59, 0x89CA, 0xA009B78C956B >,
            bluetoe::bind_characteristic_value< std::uint32_t, &simple_value >,
            bluetoe::no_read_access
        > simple_char_without_write_access;

        const bluetoe::details::attribute value_attribute = simple_char_without_write_access.attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );

        std::uint8_t old_value[ 4 ];

        auto read = bluetoe::details::attribute_access_arguments::read( old_value, 0 );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::read_not_permitted == value_attribute.access( read, 1 ) );
    }

    BOOST_AUTO_TEST_CASE( simple_value_without_write_access_can_not_be_written )
    {
        bluetoe::characteristic<
            bluetoe::characteristic_uuid< 0xD0B10674, 0x6DDD, 0x4B59, 0x89CA, 0xA009B78C956B >,
            bluetoe::bind_characteristic_value< std::uint32_t, &simple_value >,
            bluetoe::no_write_access
        > simple_char_without_read_access;

        const bluetoe::details::attribute value_attribute = simple_char_without_read_access.attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 );

        std::uint8_t new_value[] = { 0x01, 0x02, 0x03, 0x04 };

        auto write = bluetoe::details::attribute_access_arguments::write( new_value );

        BOOST_REQUIRE( bluetoe::details::attribute_access_result::write_not_permitted == value_attribute.access( write, 1 ) );
    }

    static std::uint32_t write_to_large_value = 15;

    BOOST_AUTO_TEST_CASE( write_to_large )
    {
        bluetoe::characteristic<
            bluetoe::characteristic_uuid< 0xD0B10674, 0x6DDD, 0x4B59, 0x89CA, 0xA009B78C956B >,
            bluetoe::bind_characteristic_value< std::uint32_t, &write_to_large_value >
        > characteristic;

        std::uint8_t new_value[] = { 0x01, 0x02, 0x03, 0x04, 0x05 };

        auto write = bluetoe::details::attribute_access_arguments::write( new_value );

        BOOST_REQUIRE( ( bluetoe::details::attribute_access_result::invalid_attribute_value_length == characteristic.attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 ) ) );
        BOOST_CHECK_EQUAL( write_to_large_value, 15u );
    }

    std::uint8_t writable_value[ 4 ] = { 0x01, 0x02, 0x03, 0x04 };

    struct writable_value_char :
        bluetoe::characteristic<
            bluetoe::characteristic_uuid< 0xD0B10674, 0x6DDD, 0x4B59, 0x89CA, 0xA009B78C956B >,
            bluetoe::bind_characteristic_value< decltype( writable_value ), &writable_value >
        >
    {
        writable_value_char()
        {
            static const std::uint8_t init_value[ 4 ] = { 0x01, 0x02, 0x03, 0x04 };
            std::copy( std::begin( init_value ), std::end( init_value ), std::begin( writable_value ) );
        }
    };

    BOOST_FIXTURE_TEST_CASE( write_with_offset, writable_value_char )
    {
        static const std::uint8_t new_value[] = { 0x22, 0x33 };
        auto write = bluetoe::details::attribute_access_arguments::write( new_value, 1 );
        auto rc    = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 );

        BOOST_CHECK( rc == bluetoe::details::attribute_access_result::success );
        static const std::uint8_t expected_value[] = { 0x01, 0x22, 0x33, 0x04 };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( writable_value ), std::end( writable_value ), std::begin( expected_value ), std::end( expected_value ) );
    }

    BOOST_FIXTURE_TEST_CASE( write_over_end_with_offset, writable_value_char )
    {
        static const std::uint8_t new_value[] = { 0x22, 0x33 };
        auto write = bluetoe::details::attribute_access_arguments::write( new_value, 3 );
        auto rc    = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 );

        BOOST_CHECK( rc == bluetoe::details::attribute_access_result::invalid_attribute_value_length );
        static const std::uint8_t expected_value[] = { 0x01, 0x02, 0x03, 0x04 };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( writable_value ), std::end( writable_value ), std::begin( expected_value ), std::end( expected_value ) );
    }

    BOOST_FIXTURE_TEST_CASE( write_behind_the_end, writable_value_char )
    {
        static const std::uint8_t new_value[] = { 0x22, 0x33 };
        auto write = bluetoe::details::attribute_access_arguments::write( new_value, 5 );
        auto rc    = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 );

        BOOST_CHECK( rc == bluetoe::details::attribute_access_result::invalid_offset );
        static const std::uint8_t expected_value[] = { 0x01, 0x02, 0x03, 0x04 };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( writable_value ), std::end( writable_value ), std::begin( expected_value ), std::end( expected_value ) );
    }

    BOOST_FIXTURE_TEST_CASE( can_write_zero_bytes_at_the_end, writable_value_char )
    {
        std::uint8_t c;
        auto write = bluetoe::details::attribute_access_arguments::write( &c, &c, 4,
            bluetoe::details::client_characteristic_configuration(),
            bluetoe::connection_security_attributes(),
            nullptr );
        auto rc    = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 );
        BOOST_CHECK( rc == bluetoe::details::attribute_access_result::success );
    }

    BOOST_FIXTURE_TEST_CASE( can_write_last_byte, writable_value_char )
    {
        std::uint8_t c = 0xff;
        auto write = bluetoe::details::attribute_access_arguments::write( &c, &c + 1, 3,
            bluetoe::details::client_characteristic_configuration(),
            bluetoe::connection_security_attributes(),
            nullptr );
        auto rc    = attribute_at< cccd_indices, 0, bluetoe::service< suuid >, srv >( 1 ).access( write, 1 );

        BOOST_CHECK( rc == bluetoe::details::attribute_access_result::success );
        static const std::uint8_t expected_value[] = { 0x01, 0x02, 0x03, 0xff };
        BOOST_CHECK_EQUAL_COLLECTIONS( std::begin( writable_value ), std::end( writable_value ), std::begin( expected_value ), std::end( expected_value ) );
    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( fixed_value_tests )

    typedef bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_value< std::uint16_t, 0x0815 >
    > fixed_16bit;

    typedef bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_value< std::int32_t, 0x08151213 >
    > fixed_32bit;

    BOOST_FIXTURE_TEST_CASE( find_char, access_attributes< fixed_16bit > )
    {
        attribute_by_type( 0xD0B1 );
    }

    BOOST_FIXTURE_TEST_CASE( properties_are_read_only, read_characteristic_properties< fixed_16bit > )
    {
        BOOST_CHECK_EQUAL( properties, 0x02 );
    }

    BOOST_FIXTURE_TEST_CASE( read_value, access_attributes< fixed_16bit > )
    {
        compare_characteristic( { 0x15, 0x08 }, 0xd0b1 );
    }

    BOOST_FIXTURE_TEST_CASE( read_only, access_attributes< fixed_32bit > )
    {
        const auto attr = attribute_by_type( 0xD0B1 );
        std::uint8_t buffer[ 4 ];
        auto write = bluetoe::details::attribute_access_arguments::write( buffer );

        BOOST_CHECK( attr.access( write, 1 ) == bluetoe::details::attribute_access_result::write_not_permitted );
    }

    BOOST_FIXTURE_TEST_CASE( read_with_offset, access_attributes< fixed_32bit > )
    {
        BOOST_CHECK( read_attribute_at( { 0x12, 0x15, 0x08 }, 1, 1, 3 )
            == bluetoe::details::attribute_access_result::success );
    }

    BOOST_FIXTURE_TEST_CASE( read_truncated_with_offset, access_attributes< fixed_32bit > )
    {
        BOOST_CHECK( read_attribute_at( { 0x12, 0x15 }, 1, 1, 2 )
            == bluetoe::details::attribute_access_result::success );
    }

    typedef bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_value< std::int32_t, 0x08151213 >,
        bluetoe::no_read_access
    > fixed_without_read_access;

    BOOST_FIXTURE_TEST_CASE( read_without_read_access, access_attributes< fixed_without_read_access > )
    {
        const auto attr = attribute_by_type( 0xD0B1 );
        std::uint8_t buffer[ 4 ];
        auto read = bluetoe::details::attribute_access_arguments::read( buffer, 0 );

        BOOST_CHECK( attr.access( read, 1 ) == bluetoe::details::attribute_access_result::read_not_permitted );
    }

    typedef bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_value< std::int32_t, 0x08151213 >,
        bluetoe::notify
    > fixed_notifiable;

    BOOST_FIXTURE_TEST_CASE( fixed_notifiable_must_have_a_client_characteristic_configuration, access_attributes< fixed_notifiable > )
    {
        attribute_by_type( 0x2902 );
    }

BOOST_AUTO_TEST_SUITE_END()

namespace {
    const std::uint8_t test_blob[] = { 0x00, 0x12, 0x00, 0xab, 0x05 };
}

BOOST_AUTO_TEST_SUITE( fixed_blob_tests )

    using blob_char = bluetoe::characteristic<
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_blob_value< test_blob, sizeof( test_blob ) >
    >;

    BOOST_FIXTURE_TEST_CASE( read_only_attribute, read_characteristic_properties< blob_char > )
    {
        BOOST_CHECK_EQUAL( properties, 0x02 );
    }

    BOOST_FIXTURE_TEST_CASE( read_value, access_attributes< blob_char > )
    {
        compare_characteristic( { 0x00, 0x12, 0x00, 0xab, 0x05 }, 0xd0b1 );
    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( encryption_tests )

    using fixed_value = bluetoe::characteristic<
        bluetoe::requires_encryption,
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::fixed_value< std::int8_t, 42 >
    >;

    std::uint8_t dummy_value = 43;

    using bound_value = bluetoe::characteristic<
        bluetoe::requires_encryption,
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::bind_characteristic_value< std::uint8_t, &dummy_value >
    >;

    struct cstring_holder {
        static constexpr const char* value() {
            return "Hallo";
        }

        static constexpr std::size_t size() {
            return 5u;
        }
    };

    using cstring_value = bluetoe::characteristic<
        bluetoe::requires_encryption,
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::cstring_wrapper< cstring_holder >
    >;

    static std::uint8_t test_handler( std::size_t /* read_size */, std::uint8_t* out_buffer, std::size_t& out_size )
    {
        *out_buffer = 44;
        out_size    = 1;

        return bluetoe::error_codes::success;
    }

    /*
     * All other handlers share the same base implementation
     */
    using other_handlers = bluetoe::characteristic<
        bluetoe::requires_encryption,
        bluetoe::characteristic_uuid16< 0xD0B1 >,
        bluetoe::free_read_handler< test_handler >
    >;

    using characteristic_types = boost::mpl::list< fixed_value, bound_value, cstring_value, other_handlers >;

    BOOST_AUTO_TEST_SUITE( unpaired_unencrypted )

        BOOST_AUTO_TEST_CASE_TEMPLATE( authentication_required, char_t, characteristic_types )
        {
            access_attributes< char_t > c;

            BOOST_CHECK_EQUAL(
                static_cast< int >( c.read_attribute_at( {}, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::insufficient_authentication ) );
        }

        BOOST_AUTO_TEST_CASE_TEMPLATE( make_sure_access_rights_are_tested_first, char_t, characteristic_types )
        {
            access_attributes< char_t > c;

            BOOST_CHECK_EQUAL(
                static_cast< int >( c.write_attribute_at( { 0x11 } ) ),
                static_cast< int >( bluetoe::error_codes::insufficient_authentication ) );
        }

    BOOST_AUTO_TEST_SUITE_END()

    BOOST_AUTO_TEST_SUITE( paired_unencrypted )

        static const auto paired_unencrypted_sec = bluetoe::connection_security_attributes( false, bluetoe::device_pairing_status::unauthenticated_key );

        BOOST_AUTO_TEST_CASE_TEMPLATE( authentication_required, char_t, characteristic_types )
        {
            access_attributes< char_t > c( paired_unencrypted_sec );

            BOOST_CHECK_EQUAL(
                static_cast< int >( c.read_attribute_at( {}, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::insufficient_encryption ) );
        }

        BOOST_AUTO_TEST_CASE_TEMPLATE( make_sure_access_rights_are_tested_first, char_t, characteristic_types )
        {
            access_attributes< char_t > c( paired_unencrypted_sec );

            BOOST_CHECK_EQUAL(
                static_cast< int >( c.write_attribute_at( { 0x11 } ) ),
                static_cast< int >( bluetoe::error_codes::insufficient_encryption ) );
        }

    BOOST_AUTO_TEST_SUITE_END()

    BOOST_AUTO_TEST_SUITE( paired_encrypted )

        static const auto paired_encrypted_sec = bluetoe::connection_security_attributes( true, bluetoe::device_pairing_status::unauthenticated_key );

        template < class Char >
        struct paired_encrypted_access_attributes : access_attributes< Char > {
            paired_encrypted_access_attributes() : access_attributes< Char >( paired_encrypted_sec )
            {}
        };

        BOOST_FIXTURE_TEST_CASE( reading_fixed_value, paired_encrypted_access_attributes< fixed_value > )
        {
            BOOST_CHECK_EQUAL(
                static_cast< int >( read_attribute_at( { 42 }, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::success ) );
        }

        BOOST_FIXTURE_TEST_CASE( reading_bound_value, paired_encrypted_access_attributes< bound_value > )
        {
            BOOST_CHECK_EQUAL(
                static_cast< int >( read_attribute_at( { 43 }, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::success ) );
        }

        BOOST_FIXTURE_TEST_CASE( reading_const_string_value, paired_encrypted_access_attributes< cstring_value > )
        {
            BOOST_CHECK_EQUAL(
                static_cast< int >( read_attribute_at( { 'H', 'a', 'l', 'l', 'o' }, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::success ) );
        }

        BOOST_FIXTURE_TEST_CASE( reading_other_handlers_value, paired_encrypted_access_attributes< other_handlers > )
        {
            BOOST_CHECK_EQUAL(
                static_cast< int >( read_attribute_at( { 44 }, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::success ) );
        }

    BOOST_AUTO_TEST_SUITE_END()

    BOOST_AUTO_TEST_SUITE( may_require_encrytion_tests )

        using maybe_char = bluetoe::characteristic<
            bluetoe::may_require_encryption,
            bluetoe::characteristic_uuid16< 0xD0B1 >,
            bluetoe::free_read_handler< test_handler >
        >;

        BOOST_AUTO_TEST_CASE( read_maybe_encrypted )
        {
            access_attributes< maybe_char > c;

            BOOST_CHECK_EQUAL(
                static_cast< int >( c.read_attribute_at( { 44 }, 1, 0, 23 ) ),
                static_cast< int >( bluetoe::error_codes::success ) );
        }

    BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: TorstenRobitzki/bluetoe

max_issues_repo_name: TorstenRobitzki/bluetoe

max_forks_repo_name: TorstenRobitzki/bluetoe


text:
//
// Copyright (c) 2021 Richard Hodges (hodges.r@gmail.com)
//
// 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)
//
// Official repository: https://github.com/beast-issues/2340
//

#include <boost/asio.hpp>
#include <boost/beast.hpp>

#include <iostream>

// type aliases
namespace asio = boost::asio;
namespace beast = boost::beast;

using tcp = asio::ip::tcp;
using executor_type = asio::io_context::executor_type;
using stream_type = beast::basic_stream<tcp, executor_type>;
//using wss_stream_type=websock::stream<beast::ssl_stream<stream_type>, true>

asio::awaitable<tcp::resolver::results_type>
resolve()
{
    auto resolver = tcp::resolver(co_await asio::this_coro::executor);
    auto results = co_await resolver.async_resolve("localhost", "8080",
                                                   asio::use_awaitable);
    co_return results;
}

asio::awaitable<void>
run_test()
try
{
    auto exec = co_await asio::this_coro::executor;

    auto ws = beast::websocket::stream<tcp::socket>(exec);

    beast::websocket::permessage_deflate opt;
    opt.client_enable = true;// for clients
    opt.server_enable = true;// for servers
    ws.set_option(opt);

    auto endpoint = co_await async_connect(ws.next_layer(), co_await resolve(),
                           asio::use_awaitable);

    std::cout << "connected: " << endpoint << "\n";

    co_await ws.async_handshake("localhost", "/", asio::use_awaitable);

    static const char hello[] = "Hello, World!";

    co_spawn(exec, ws.async_write(asio::buffer(hello), asio::use_awaitable),
             asio::detached);

    beast::flat_buffer rxbuf;
    beast::error_code ec;
    while (!ec)
    {
        std::size_t bytes = co_await ws.async_read(
            rxbuf, asio::redirect_error(asio::use_awaitable, ec));
        if (ec) break;
        if (ws.got_text())
            std::cout << "response: " << beast::buffers_to_string(rxbuf.data())
                      << "\n";
        else
            std::cout << "response: <binary>\n";
        rxbuf.consume(bytes);
    }

    std::cout << "websocket closed: " << ec.message() << std::endl;
    std::cout << "websocket closed: " << ws.reason() << std::endl;

} catch (std::exception &e)
{
    std::cout << __func__ << ": exception: " << e.what() << "\n";
    throw;
}

int
main()
{
    auto ioc = asio::io_context();

    asio::co_spawn(ioc, run_test(), asio::detached);

    ioc.run();
}


text:
#include <cstdio>
#include <fcntl.h>

#include <sys/stat.h>
#include <sys/types.h>

#include <boost/scoped_ptr.hpp>

#include "msrp/System.hxx"
#include "msrp/IncomingMessage.hxx"
#include "msrp/OutgoingMessage.hxx"
#include "msrp/SessionFactory.hxx"
#include "msrp/Session.hxx"

using namespace msrp;
using namespace std;
using namespace boost;
using namespace resip;
using namespace asio;

#define RESIPROCATE_SUBSYSTEM Subsystem::TEST

class OfferSession
{
   public:
      OfferSession(SessionFactory& sf, const string& file) :
         mFactory(sf), mFile(file), mTimer(sf.service())
      {
         mFd = open(file.c_str(), O_RDONLY);
         assert(mFd >= 0);

         struct stat sb = { 0 };
         fstat(mFd, &sb);

         mSize = sb.st_size;
         mRead = 0;
      }

      void create()
      {
         ip::tcp::endpoint bound(ip::address_v4::any(), 9955);

         mSession = mFactory.offer(bound, Uri("msrp:127.0.0.1:9955"));

         InfoLog(<< "offer session created, bound to " << bound);

         assert(mSession);
         assert(mSession->connection());

         shared_ptr<Connection> c = mSession->connection();

         c->onConnect().connect(bind(&OfferSession::onAccept, this, _1));
      }

   private:
      void onAccept(const ip::tcp::endpoint& peer)
      {
         InfoLog(<< "OfferSession::onAccept: from " << peer);

         Message m;

         mSession->prepare(m);

         m.method() = Message::SEND;
         m.status() = Message::Complete;

         m.header("Filename") = "testFile";

         ByteRangeTuple& br = m.header<ByteRange>();
         br.total = mSize;

         mOutgoing = mSession->stream(m);

         mOutgoing->onDataRequired().connect(bind(&OfferSession::onData, this, _1, _2));
      }

      void onData(size_t reqd, OutgoingMessage::StreamFunctor& stream)
      {
         DebugLog(<< "onDataRequired: " << reqd);

         char readbuf[256] = { 0 };

         int r = ::read(mFd, readbuf, min<unsigned int>(mSize - mRead, 256));

         mRead += r;

         const_buffer buf((const char*)readbuf, r);

         stream(buf);
      }

      SessionFactory& mFactory;

      shared_ptr<Session> mSession;

      shared_ptr<OutgoingMessage> mOutgoing;

      const string mFile;

      unsigned int mSize;
      unsigned int mRead;

      int mFd;

      deadline_timer mTimer;
};

class AnswerSession
{
   public:
      AnswerSession(SessionFactory& sf) :
         mFactory(sf)
      {}

      void create()
      {
         mSession = mFactory.answer(
               Uri("msrp:127.0.0.1:9955"),
               Uri(),
               bind(&AnswerSession::onSession, this, _1, _2));

         if (mSession)
         {
            InfoLog(<< "answer session created, target msrp:127.0.0.1:9955");

            connectHandlers();
         }
         else
         {
            InfoLog(<< "waiting for DNS");
         }
      }

   private:
      void onSession(shared_ptr<Session> s, const asio::error& e)
      {
         if (e)
         {
            ErrLog(<< "session creation failed: " << e);
         }
         else
         {
            InfoLog(<< "answer session created, target msrp:127.0.0.1:9955");

            mSession = s;

            connectHandlers();
         }
      }

      void connectHandlers()
      {
         assert(mSession);
         assert(mSession->connection());

         mSession->onMessage().connect(bind(&AnswerSession::onMessage, this, _1));
         mSession->onMessageSession().connect(bind(&AnswerSession::onMessageSession, this, _1));

         shared_ptr<Connection> c = mSession->connection();

         c->onConnect().connect(bind(&AnswerSession::onConnect, this, _1));
      }

      void onConnect(const ip::tcp::endpoint& endpoint)
      {
         InfoLog(<< "AnswerSession: connected to " << endpoint);
      }

      void onMessage(shared_ptr<const Message> msg)
      {
         InfoLog(<< "received: " << *msg);
      }

      bool onMessageSession(shared_ptr<IncomingMessage> ims)
      {
         const Message& m = ims->message();

         mFilename = m.header("Filename");

         InfoLog(<< "incoming message session, file: " << mFilename);

         mFd = ::open(mFilename.c_str(), O_CREAT | O_TRUNC | O_APPEND | O_WRONLY, 0600);
         assert(mFd >= 0);

         ims->onContext().connect(bind(&AnswerSession::onMessageSessionContext, this, _1));
         ims->onContents().connect(bind(&AnswerSession::onMessageSessionContents, this, _1));
         ims->onComplete().connect(bind(&AnswerSession::onMessageSessionComplete, this));

         onMessageSessionContext(ims->message());

         return true;
      }

      void onMessageSessionContext(const Message& m)
      {
         InfoLog(<< "message session context: " << m);
      }

      void onMessageSessionContents(const asio::const_buffer& b)
      {
         const string contents(buffer_cast<const char*>(b), buffer_size(b));

         InfoLog(<< "message session contents: {" << contents << "}");

         ::write(mFd, contents.c_str(), contents.size());
      }

      void onMessageSessionComplete()
      {
         InfoLog(<< "transmission complete");

         ::close(mFd);
         mFd = -1;

         mFactory.shutdown();
      }

      SessionFactory& mFactory;

      shared_ptr<Session> mSession;

      string mFilename;

      int mFd;
};

int
main(int argc, char** argv)
{
   Log::initialize(Log::Cout, Log::Debug, argv[0]);

   asio::io_service fifo;

   SessionFactory sf(fifo);

   scoped_ptr<OfferSession> os(new OfferSession(sf, "testSessionFactory.cxx"));
   os->create();

   scoped_ptr<AnswerSession> as(new AnswerSession(sf));
   as->create();

   fifo.run();

   return 0;
}

ext: cxx

max_stars_repo_name: cbond/msrp

max_issues_repo_name: cbond/msrp

max_forks_repo_name: cbond/msrp