#include "RRT/RRT.h"
#include <time.h>
#include "RRT/ArmCollision.h"
#include "RRT/Pos2Joint.h"
#include "RRT/CoordinateTrans.h"

//---------------//
//	CCalcRRT
//---------------//

rrt_vector::rrt_vector()
{
	x = 0;
	y = 0;
	z = 0;
}

rrt_vector::rrt_vector(double xx , double yy , double zz)
{
	x = xx;
	y = yy;
	z = zz;
}

rrt_vector::~rrt_vector()
{
}

//---------------//
//	rrt_node
//---------------//

// コンストラクタ
rrt_node::rrt_node()
{
}
// コンストラクタ
rrt_node::rrt_node(int back_node)
{
	b_link = back_node;
}

// デストラクタ
rrt_node::~rrt_node()
{

}

// 距離計算
double rrt_vector::distance(rrt_vector vec)
{
	vec.x -= x;
	vec.y -= y;
	vec.z -= z;
	return vec.magnitude();
}

//---------------//
//	CCalcRRT
//---------------//

CCalcRRT::CCalcRRT()
{
	srand((unsigned int)time(NULL));
	m_reached = false;
}

CCalcRRT::~CCalcRRT()
{
}

// メイン処理関数
bool CCalcRRT::MainProcess()
{	
	SetObstacle(); // 障害物セット

	int num_of_node = 0;
	
	// 接続できるまで繰り返す
	while ( !m_reached ) {
		ExtendTree();
		if( (num_of_node++) > MAXNODE )	return false; // MAXNODE回繰り返してもゴールに達しなかった場合は失敗
	}

	printf("ノード数:%d\n",num_of_node);
	
	//結果抽出
	int link_num = (int)m_n_node.size() - 1;

	while ( link_num )
	{
		m_n_result.push_back(m_n_node[link_num]);
		link_num = m_n_node[link_num].b_link;
	}

	printf("経路数:%d\n",(int)m_n_result.size());

	return true;
}

void CCalcRRT::ExtendTree()
{	
	bool rand_flag = true;
	rrt_vector q_rand, q_new;
	rrt_node n_new;

	// ノード探索ループ
	for ( int i=0;i<20;i++ ) // 回数制限のためのfor文
	{
		//q_randの作成
		if( CalcRandom() > GOAL_CRATIO ) // 確率でゴールの位置をセットする
		{
			q_rand.x = ((WORKSPACE_X_MAX - WORKSPACE_X_MIN)*CalcRandom()) + WORKSPACE_X_MIN;
			q_rand.y = ((WORKSPACE_Y_MAX - WORKSPACE_Y_MIN)*CalcRandom()) + WORKSPACE_Y_MIN;
			q_rand.z = ((WORKSPACE_Z_MAX - WORKSPACE_Z_MIN)*CalcRandom()) + WORKSPACE_Z_MIN;
		}
		else
		{
			q_rand = m_q_goal.pos;
		}

		// 近接ノードの検索(q_nearの検索)
		int near_node_num =	NearestNeighbor(q_rand);

		//距離を調整
		q_new = AdjustNodeDistance( m_n_node[near_node_num].pos , q_rand );

		// q_newとゴール判定
		if( q_new.distance( m_q_goal.pos ) < EPSILON ) q_new = m_q_goal.pos;

		// 新しいノードの座標が決定したので、これをノードとする
		n_new.pos.x = q_new.x;
		n_new.pos.y = q_new.y;
		n_new.pos.z = q_new.z;
		n_new.b_link = near_node_num;

		int numLink = m_n_node[near_node_num].numLink;	// スタートから数えて何番目のノードか？

		n_new.numLink = numLink+1;

		// 衝突判定
		if( DetectCollision(m_n_node[near_node_num], n_new) ) break; // ぶつからないからbreakして採用

		if ( i == 10 ) return;
	}

	//q_newとゴール判定
	if( n_new.pos.distance( m_q_goal.pos ) < EPSILON )
	{
		m_reached = true;
		m_n_node.push_back( n_new );
		//m_n_node.push_back( m_q_goal );
	}

	//q_newの追加
	m_n_node.push_back( n_new );

	return;
}

// 近接ノードの検索
int CCalcRRT::NearestNeighbor(rrt_vector q_pos)
{
	int near_node_num , node_num;
	double dist,min;

	node_num = (int)m_n_node.size();

	min = q_pos.distance( m_n_node[0].pos );
	near_node_num = 0;

	for ( int i=1;i<node_num;i++ )
	{
		dist = q_pos.distance( m_n_node[i].pos );

		if ( dist < min )
		{
			min = dist;
			near_node_num = i;
		}
	}
	
	return near_node_num;
}

// 障害物の領域内に入り込んでいないか true:ぶつからない false:ぶつかる
bool CCalcRRT::DetectCollision(rrt_node near_node, rrt_node new_node)
{
	rrt_node n;
	n = new_node;

	// Pos→Joint
	float joint[6];
	float pos[6] = {0.0f};
	pos[0] = (float)n.pos.x;
	pos[1] = (float)n.pos.y;
	pos[2] = (float)n.pos.z;
	float tmpPos[6] = {0.0f};
	for ( int i=0;i<6;i++ ) tmpPos[i] = pos[i];
	if ( Pos2JointL(tmpPos,joint,ARM_POSE_ELBOW_DOWN) == false ) 
	{
		return false; // 計算失敗したので不採用
	}

	for ( int i=0;i<(int)m_v_roboX.size();i++ )
	{
		if ( m_v_roboX[i] > pos[0] - 50 && m_v_roboX[i] < pos[0] + 50 &&
			 m_v_roboY[i] > pos[1] - 50 && m_v_roboY[i] < pos[1] + 50 &&
			 m_v_roboZ[i] > pos[2] - 50 && m_v_roboZ[i] < pos[2] + 50  )
		{
			return false;
		}
	}

	// 衝突判定
	double Angle[6] = {(double)joint[0], (double)joint[1], (double)joint[2], (double)joint[3], (double)joint[4], (double)joint[5]};

	m_ArmCollision.SetAngle(Angle);
	for ( int j=0;j<(int)m_v_roboX.size();j++ )
	{
		if ( pos[2] < m_v_roboZ[j] ) continue; // 手先より先の座標ははじく
		if ( pos[0] < m_v_roboX[j] ) continue;
		double P[3] = {m_v_roboX[j]*1000.0, m_v_roboY[j]*1000.0, m_v_roboZ[j]*1000.0};
		if ( m_ArmCollision.isCollision(P, true) ) 
		{
			return false; // 衝突するので不採用
		}
	}

	return true;
}

// スタート
void CCalcRRT::SetStartGoal(double StartPos[3], double GoalPos[3])
{
	//スタートノード設定
	m_n_node.clear();

	m_q_start.pos.x = StartPos[0];
	m_q_start.pos.y = StartPos[1];
	m_q_start.pos.z = StartPos[2];

	m_q_goal.pos.x = GoalPos[0];
	m_q_goal.pos.y = GoalPos[1];
	m_q_goal.pos.z = GoalPos[2];

	m_q_start.b_link = 0;
	m_q_start.numLink = 0;

	m_n_node.push_back( m_q_start );
}

//ノードの距離調整（計算部分）
rrt_vector CCalcRRT::AdjustNodeDistance( rrt_vector base_pos , rrt_vector add_pos )
{
	rrt_vector q_new;

	//変数設定
	double sin[2];
	double cos[2];
	double length[2];

	length[0] = sqrt((add_pos.x-base_pos.x)*(add_pos.x-base_pos.x)+(add_pos.y-base_pos.y)*(add_pos.y-base_pos.y));
	length[1] = sqrt((add_pos.x-base_pos.x)*(add_pos.x-base_pos.x)+(add_pos.y-base_pos.y)*(add_pos.y-base_pos.y)+(add_pos.z-base_pos.z)*(add_pos.z-base_pos.z));
	sin[0]=(add_pos.y-base_pos.y)/length[0];
	cos[0]=(add_pos.x-base_pos.x)/length[0];
	sin[1]=(add_pos.z-base_pos.z)/length[1];
	cos[1]=length[0]/length[1];

	q_new.x = base_pos.x + EPSILON * cos[1] * cos[0];
	q_new.y = base_pos.y + EPSILON * cos[1] * sin[0];
	q_new.z = base_pos.z + EPSILON * sin[1];

	return q_new;
}

void CCalcRRT::SetObstacle()
{
	CCoordinateTrans trans;
	double robotX,robotY,robotZ;
	char buf[256];

	// メモリ動的確保
	int ***Vote_p  = new int **[100];
	for ( int i=0;i<100;i++ ) Vote_p[i] = new int *[100];
	for ( int i=0;i<100;i++ ) 
	{
		for ( int j=0;j<100;j++ )
		{
			Vote_p[i][j] = new int [100];
		}
	}
	

	// 初期化
	for ( int i=0;i<100;i++ )
	{
		for ( int j=0;j<100;j++ )
		{
			for ( int k=0;k<100;k++ )
			{
				Vote_p[i][j][k] = 0;
			}
		}
	}

	FILE *fp = fopen("depth1.txt","r");

	for ( int i=0; fgets(buf, 256, fp); i++ )
	{
		double camX,camY,camZ;
		sscanf(buf, "%lf	%lf	%lf",&camX, &camY, &camZ);

		if ( (int)camX==0 && (int)camY==0 && (int)camZ==0 ) continue; // 値がとれなかった

		if ( i == 0 ) trans.Camera2Robot(0, -35, camX, camY, camZ, &robotX, &robotY, &robotZ, true);
		else          trans.Camera2Robot(0, -35, camX, camY, camZ, &robotX, &robotY, &robotZ, false);

		if ( robotX < WORKSPACE_X_MIN ||
			 robotX > WORKSPACE_X_MAX ||
			 robotY < WORKSPACE_Y_MIN ||
			 robotY > WORKSPACE_Y_MAX ||
			 robotZ < WORKSPACE_Z_MIN ||
			 robotZ > WORKSPACE_Z_MAX  )
		{
			continue;
		}

		int x = (int)robotX / 20;
		int y = (int)robotY / 20 + 50;
		int z = (int)robotZ / 20;

		if ( x < 0 || x > 99 ) continue;
		if ( y < 0 || y > 99 ) continue;
		if ( z < 0 || z > 99 ) continue;

		Vote_p[x][y][z]++;
	}

	fclose(fp);

	for ( int i=0;i<100;i++ )
	{
		for ( int j=0;j<100;j++ )
		{
			for ( int k=0;k<100;k++ )
			{
				if ( Vote_p[i][j][k] > 10 ) // 投票数が10個以上
				{
					m_v_roboX.push_back((double)i*20.0/1000.0);
					m_v_roboY.push_back(((double)j-50.0)*20.0/1000.0);
					m_v_roboZ.push_back((double)k*20.0/1000.0);
				}
			}
		}
	}

	// 解放
	for ( int i=0;i<100;i++ )
	{
		for ( int j=0;j<100;j++ )
		{
			delete [] Vote_p[i][j];
		}
	}
	for ( int i=0;i<100;i++ ) delete [] Vote_p[i];
	delete [] Vote_p;


	printf("m_v_roboX.size() = %d\n",(int)m_v_roboX.size());
	return;
}
