/home/slemaign/softs-local/BioMove3D-git/lightPlanner/proto/ManipulationTestFunctions.hpp

/*
 *  ManipulationTestFunctions.hpp
 *  Move3D-core
 *
 *  Created by Jim Mainprice on 05/11/10.
 *  Copyright 2010 Laas/CNRS. All rights reserved.
 *
 */

#ifndef MANIPULATION_TEST_FUNCTIONS_HPP
#define MANIPULATION_TEST_FUNCTIONS_HPP

//#include "device.h"
//#include "p3d.h"

#include "ManipulationPlanner.hpp"

#include <string>

class ManipulationTestFunctions 
{
        
public:
        ManipulationTestFunctions();
        ManipulationTestFunctions(std::string RobotNameContains);
        virtual ~ManipulationTestFunctions();
        
        bool runTest(int i);

  void setDebugMode(bool value);
  
  void setInitConfiguration(configPt q);
  void setGoalConfiguration(configPt q);
  

  void setObject(std::string name);
  void resetObject();
  
  void setPlacement(std::string name);
  void resetPlacement();
  
  void setSupport(std::string name);
  void resetSupport();
  
  void initManipulationGenom();

  inline ManipulationPlanner* getManipulationPlanner(){return m_manipulation;}
  
  void drawEvalutedWorkspace();
  
  void saveToFileEvalutedWorkspace();
  
  bool readWorkspaceFromFile(std::string fileName);
  
        bool manipTest(MANIPULATION_TASK_TYPE_STR type);
  
  ManipulationPlanner* getManipPlanner() { return m_manipulation; }
  
private:
        bool manipTestGraspingWithDifferentObjectOrientations(bool rotate_only_around_z, double& successRate);
  
  bool evaluateWorkspace();
  
  bool computeTrajectories();
        
        p3d_rob* m_Robot;
        
        configPt m_qInit;
        configPt m_qGoal;
        
        std::string m_OBJECT_NAME;
  std::string m_SUPPORT_NAME;
  std::string m_PLACEMENT_NAME;
  
  std::vector<double> m_objStart, m_objGoto;
  
  unsigned int m_nbOrientations;
  
  std::vector< std::pair<double, std::vector<double> > > m_workspacePoints;
        
        ManipulationPlanner* m_manipulation;
};

#endif