
#ifndef _COLDET_PAIR_DATA_H
#define _COLDET_PAIR_DATA_H


#include <cnoid/BodyItem>	
#include <boost/make_shared.hpp>

#include "exportdef.h"

namespace grasp{

class EXCADE_API ColdetPairData{
	public:
	ColdetPairData(cnoid::BodyItemPtr bodyItem1,cnoid::BodyItemPtr bodyItem2,bool doComputeDistance=false, bool useRobotSafeBoundingBox=false){
		this->bodyItem1 = bodyItem1;
		this->bodyItem2 = bodyItem2;
		for(int j=0;j<bodyItem1->body()->numLinks();j++){
			cnoid::ColdetModelPtr backup = bodyItem1->body()->link(j)->coldetModel();
			if(useRobotSafeBoundingBox){
				cnoid::ColdetModelPtr temp = getSafeBoundingBox(bodyItem1->body()->link(j)->coldetModel(), cnoid::Vector3(0.03,0.03,0.03) );
        if(temp != NULL) bodyItem1->body()->link(j)->setColdetModel(temp);
			}
			for(int i=0;i<bodyItem2->body()->numLinks();i++){
	#ifdef  CNOID_10_11_12_13
				cnoid::ColdetLinkPairPtr temp= new cnoid::ColdetLinkPair(bodyItem1->body()->link(j), bodyItem2->body()->link(i));
	#else
				cnoid::ColdetLinkPairPtr temp = boost::make_shared<cnoid::ColdetLinkPair>(bodyItem1->body(),bodyItem1->body()->link(j), bodyItem2->body(), bodyItem2->body()->link(i) );
	#endif
				if(doComputeDistance){
					temp->updatePositions();
					int t1,t2;
					double p1[3],p2[3];
					double distance = temp->computeDistance(t1,p1,t2,p2);
					if(distance <1.0e-04) continue;
				}
				coldetLinkPairs.push_back(temp);
			}
			bodyItem1->body()->link(j)->setColdetModel(backup);
		}
	}
	~ColdetPairData(){
		coldetLinkPairs.clear();
	}
	static cnoid::ColdetModelPtr getSafeBoundingBox(cnoid::ColdetModelPtr model, cnoid::Vector3 boundingBoxSafetySize){
//		cnoid::ColdetModelPtr safeBoundingBox;
#ifdef  CNOID_10_11_12_13
		cnoid::ColdetModelPtr safeBoundingBox = new cnoid::ColdetModel();
#else
		cnoid::ColdetModelPtr safeBoundingBox =  boost::make_shared<cnoid::ColdetModel>();
#endif

		int nVerticies = model->getNumVertices();
#ifdef  CNOID_10_11_12_13
		if(nVerticies == 0) return NULL;
#else
    if(nVerticies == 0) {
      safeBoundingBox.reset();
      return safeBoundingBox;
    }
#endif
		
		float out_x, out_y, out_z;
		model->getVertex(0, out_x, out_y, out_z);
		cnoid::Vector3 min(out_x, out_y, out_z),max(out_x, out_y, out_z);
		for(int i=0;i<nVerticies;i++){
			model->getVertex(i, out_x, out_y, out_z);
			if(min[0] > out_x) min[0] = out_x;
			if(min[1] > out_y) min[1] = out_y;
			if(min[2] > out_z) min[2] = out_z;
			if(max[0] < out_x) max[0] = out_x;
			if(max[1] < out_y) max[1] = out_y;
			if(max[2] < out_z) max[2] = out_z;
		}
		min -= boundingBoxSafetySize;
		max += boundingBoxSafetySize;

		for(int i=0;i<8;i++){
			cnoid::Vector3 temp;
			if( (i&1) == 0) temp[0] = min[0];
			else temp[0] = max[0];
			if( (i&2) == 0) temp[1] = min[1];
			else temp[1] = max[1];
			if( (i&4) == 0) temp[2] = min[2];
			else temp[2] = max[2];
			safeBoundingBox->addVertex(temp[0],temp[1],temp[2]);
		}
		
		safeBoundingBox->addTriangle(0,1,2);
		safeBoundingBox->addTriangle(1,3,2);
		safeBoundingBox->addTriangle(5,4,7);
		safeBoundingBox->addTriangle(4,6,7);
		safeBoundingBox->addTriangle(0,2,4);
		safeBoundingBox->addTriangle(2,6,4);
		safeBoundingBox->addTriangle(1,5,3);
		safeBoundingBox->addTriangle(5,7,3);
		safeBoundingBox->addTriangle(0,4,1);
		safeBoundingBox->addTriangle(4,5,1);
		safeBoundingBox->addTriangle(2,3,6);
		safeBoundingBox->addTriangle(3,7,6);
		
		safeBoundingBox->build();
		
		return safeBoundingBox;
	}
	
	cnoid::BodyItemPtr bodyItem1,bodyItem2;
	std::vector<cnoid::ColdetLinkPairPtr> coldetLinkPairs;
//	cnoid::Vector3 boundingBoxSafetySize;
};

}

#endif