double RobotController::getRoll(Rotation rot)
{
	// get angles arround y-axis
	double qw = rot.qw();
	double qx = rot.qx();
	double qy = rot.qy();
	double qz = rot.qz();

	double roll  = atan2(2*qy*qw - 2*qx*qz, 1 - 2*qy*qy - 2*qz*qz);

	return roll;
}
double MyController::onAction(ActionEvent &evt) {  

  int actionNumber = 2;
  int functionalFeature = 1;
  int targetType = 3;

  myfile << setprecision(2) << std::fixed;

  // handle of target and tool
  SimObj *target = getObj("box_004");
  SimObj *toolName = getObj("TShapeTool_004");


  if (evt.time() < 5.0)
  {
    // cout << "Time" << endl;
    cout << evt.time() << endl;

    toolName->getPosition(currentToolPos); // get the current tool position
    toolName->getRotation(finalToolRotation);
    toolName->getVelocity(finalToolVel);
    isToolAtRest   = checkEntityMotionStatus(toolName);  // checks whether the tool is moving by calculating its velocity

    target->getPosition(currentTargetPos);
    target->getRotation(finalTargetRotation);
    target->getVelocity(finalTargetVel);
    isTargetAtRest = checkEntityMotionStatus(target);    // checks whether the object is moving by calculating its velocity
    
  }

  if (evt.time() > 5.0)
  {
     insideTimer = false;
     counterOfAction ++ ; 
  }

  if(!insideTimer && counterOfAction == 1 )
  {
           myfile << actionNumber  << " , " << functionalFeature << " , " ;
           myfile << initToolRotation.qw() <<  " , " << initToolRotation.qx() <<  " , " << initToolRotation.qy() <<  " , " << initToolRotation.qz() << " , " ;
           myfile << initTargetRotation.qw() <<  " , " << initTargetRotation.qx() <<  " , " << initTargetRotation.qy() <<  " , " << initTargetRotation.qz() << " , " ;
           myfile << finalTargetRotation.qw() <<  " , " << finalTargetRotation.qx() <<  " , " << finalToolRotation.qy() <<  " , " << finalToolRotation.qz() << " , " ;
           myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
           myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
           myfile << forceOnTool_x  << " , " << forceOnTool_z << " , " ;
           myfile << appliedToolVel.x()  << " , " << appliedToolVel.z() << " , " ;
           myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
           myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
           myfile << currentToolPos.x() << " , " << currentToolPos.z() << " , " ;
           myfile << currentTargetPos.x() << " , " << currentTargetPos.z() << " , " ;
           myfile << finalToolVel.x() << " , " << finalToolVel.z() << " , " ;
           myfile << finalTargetVel.x() << " , " << finalTargetVel.z() << " , " ;
           myfile   << isToolAtRest <<  " , " << isTargetAtRest << " , " ;
           myfile << currentToolPos.x() -  initToolPos.x() << " , " << currentToolPos.z() - initToolPos.z() << " , " ;
           myfile << currentTargetPos.x() -  initTargetPos.x() << " , " << currentTargetPos.z() - initTargetPos.z();
           myfile << "\n"; 
           cout << "The simulation for " << actionNumber << " , " << functionalFeature << " has been recorded" << endl;
           // exit(0);
           flag = false;  
  }

      
  return 0.01;    

 }
double MyController::onAction(ActionEvent &evt) {  

  dlopen("libpython2.7.so", RTLD_LAZY | RTLD_GLOBAL); 
  Py_Initialize();  //initialization of the python interpreter




  //return 1.0;
  SimObj *stick = getObj("robot_test");
  SimObj *box   = getObj("box_001");
  SimObj *goal_001   = getObj("checkpoint_001");
  double torque;





  // The target position
  Vector3d targetPos;
  box->getPosition(targetPos);

  // The Goal Position: The sphere is placed at Goal position. 

  Vector3d goalPos;
  goal_001->getPosition(goalPos);

  // calculating the displacement vector 

  Vector3d displacementVect;
  displacementVect.x(goalPos.x()-targetPos.x());
  displacementVect.y(goalPos.y()-targetPos.y());
  displacementVect.z(goalPos.z()-targetPos.z());



  // stick->addForce(-500,0,500);  // This adds force to on the stick tool.
  // stick->addForce(0,0,5000); 

  // Vector3d angularVel;
  // stick->getAngularVelocity(angularVel); 
  // LOG_MSG((" Current angular Velocity is  : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));

 //  if ( abs(angularVel.y()) > 0.1  )
 //  {
 //      LOG_MSG((" Current angular Velocity is  : %f %f %f ", angularVel.x(), angularVel.y(), angularVel.z() ));
 //      double* ptr1 = NULL;
 //      ptr1 = controlAngularVelocity(angularVel, 3.0, 0, 1.0);
 //      torque  =  ptr1[0] * 500;
 //      // torque =  ptr1[0] * 12000 ;
 //      cout << "The torque applied for controlling angular velocity is " << torque << "   N. m" << endl; 
 //      stick->addTorque( 0 , torque, 0);
 // } 


  // to control the rotation of the tool

  // Rotation currentToolRot;
  // stick->getRotation(currentToolRot);
  // double *ptr = NULL;
  // ptr = controlRotation(initialToolRot, currentToolRot, 20.0, 0.0, 20.0);
  // torque  =  ptr[1] * 200 ;
  // cout << "The torque applied for controlling the rotation = " << torque << endl;
  // stick->addTorque(0, torque,0);





  // double massOfTool;  
  // massOfTool = stick->getMass();  
  // cout << "The mass is " << massOfTool <<endl;

  // Vector3d velocityOfTarget;
  // box->getLinearVelocity(velocityOfTarget);

  // double netVelocityTarget;
  // netVelocityTarget=( pow(velocityOfTarget.x(),2) + pow(velocityOfTarget.y(), 2) + pow(velocityOfTarget.z(), 2 ) );
  // netVelocityTarget = sqrt(netVelocityTarget);


  // stick->getPosition(pos);
  // stick->getPartsPosition(pos, "LINK1");
  // LOG_MSG((" LINK1 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));

  // Vector3d targetPos;
  // box->getPosition(targetPos);

  // Vector3d goalPos;
  // goal_001->getPosition(goalPos);

  // if (abs( goalPos.z() - targetPos.z()) < 1.4 ) 
  // {
  //    cout << "The distance to goal is " << abs( goalPos.z() - targetPos.z()) << endl;
  //    cout << "The goal has been reached " << endl;
  //    exit(0);
  // }

  // if (netVelocityTarget > 0.1 )

  // {

  //   double angle = atan ( (targetPos.z() - startPosition.z()) / (targetPos.x() - startPosition.x()  )  ) * 180 / PI;
  //   cout << "The angle is" << angle; 
  //   storePosition(targetPos);
  // }



  std::vector<std::string> s;

  for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){
    if (it->first != "body")
        s.push_back(it->first);
    }
  
 std::string linkName; 
 Size si;

 cout << "The total links are  " << s.size() << endl;

 for (int i = 0; i < s.size(); i++){

  const char* linkName = s[i].c_str();
  CParts *link = stick->getParts(linkName);
  link->getPosition(pos);
  link->getRotation(linkRotation);

  if (link->getType() == 0){
            BoxParts* box = (BoxParts*) link;
            si = box->getSize();
            // cout <<  linkName << endl;
            cout << linkName << "  position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            // cout << linkName << "  size : x = " << si.x() << "  y = " << si.y() << "  z = " << si.z() << endl;
            // cout << linkName << "  Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy()
            // << " qz = "<< linkRotation.qz() << endl;

            try{  

               py::object main_module = py::import("__main__");
               py::object main_namespace = main_module.attr("__dict__");  
               main_module.attr("linkName") = linkName;
               main_module.attr("length")  = si.x();
               main_module.attr("height")  = si.y();
               main_module.attr("breadth") = si.z();
               main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]";
               main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]";
               
               // calculating the rotation of the tool.
               py::exec("import ast", main_namespace);
               py::exec("import transformations as T", main_namespace);
               py::exec("rotation = ast.literal_eval(rotation)", main_namespace);
               // py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace);
               // py::exec("print angles", main_namespace);

    
             
               py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace);
               py::exec_file("vertices.py", main_namespace, main_namespace );
               py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace);

               main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]";
               main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]";
               py::exec_file("displacementVector.py", main_namespace, main_namespace );
          

            }
            catch(boost::python::error_already_set const &){
                // Parse and output the exception
                std::string perror_str = parse_python_exception();
                std::cout << "Error in Python: " << perror_str << std::endl;
            }
        }

  else if(link->getType() == 1){
            CylinderParts* cyl = (CylinderParts*) link;
            cout << "Cylinder Position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            cout << "Cylinder Length : length = " << cyl->getLength() << endl;
            cout << "Cylinder Radius : rad = " << cyl->getRad() << endl;
        }

  else if(link->getType() == 2){
            SphereParts* sph = (SphereParts*) link;
            cout << "Sphere Position : x = " << pos.x() << "  y = " << pos.y() << "  z = " << pos.z() << endl;
            cout << "Sphere Radius : rad = " << sph->getRad() << endl;
        }


 }


 
  // stick->getPartsPosition(pos, s[1]);
  // stick->getPartsPosition(pos, "LINK1");
  // LOG_MSG((" LINK1 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));

  // stick->getPartsPosition(pos, "LINK2");
  // LOG_MSG((" LINK2 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));


  // stick->getPartsPosition(pos, "LINK3");
  // LOG_MSG((" LINK3 Position is  : %f %f %f ", pos.x(), pos.y(), pos.z() ));


  

 

 return 0.00001;
  }
void getPrimitivesFromRSB(boost::shared_ptr<Primitive3DFloatSet> evPrimitiveSet){
  std::vector<Primitive3DFilter::Primitive3D> rsbPrimitives;
  for(int i = 0; i < evPrimitiveSet->primitives_size(); ++i) {
    Primitive3DFloat primitiveProto = evPrimitiveSet->primitives(i);
    Primitive3DFilter::PrimitiveType primitiveType = Primitive3DFilter::CUBE;
    Primitive3DFloat_PrimitiveType typeRST = primitiveProto.type();
    if(typeRST == Primitive3DFloat_PrimitiveType_CUBE)
        primitiveType = Primitive3DFilter::CUBE;
    else if(typeRST == Primitive3DFloat_PrimitiveType_SPHERE)
        primitiveType = Primitive3DFilter::SPHERE;
    else if(typeRST == Primitive3DFloat_PrimitiveType_CYLINDER)
        primitiveType = Primitive3DFilter::CYLINDER;
    Translation positionProto = primitiveProto.pose().translation();
    Vec primitivePosition(positionProto.x()*1000.0, positionProto.y()*1000.0, positionProto.z()*1000.0, 1);
    Rotation rotation = primitiveProto.pose().rotation();
    Primitive3DFilter::Quaternion primitiveOrientation(Vec3(rotation.qx(), rotation.qy(), rotation.qz()), rotation.qw());
    Vec primitiveScale(primitiveProto.scale().x()*1000.0, primitiveProto.scale().y()*1000.0, primitiveProto.scale().z()*1000.0, 1);
    Primitive3DFilter::Primitive3D primitive(primitiveType, primitivePosition, primitiveOrientation, primitiveScale, 0, primitiveProto.description());
    rsbPrimitives.push_back(primitive);
  }
  primitivesMutex.lock();
  primitives = rsbPrimitives;
  primitivesMutex.unlock();
}