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

using namespace std;


SingleConfrrt_vector::SingleConfrrt_vector()
{
	for ( int i=0;i<6;i++ )
	{
		this->m_theta[i] = 0.0;
		this->tmp_theta[i] = 0.0;
	}
}

SingleConfrrt_vector::SingleConfrrt_vector(double theta[6])
{
	for ( int i=0;i<6;i++ ) this->m_theta[i] = theta[i];
}

SingleConfrrt_vector::~SingleConfrrt_vector()
{
}

//---------------//
//	SingleConfrrt_node
//---------------//

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

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

}

// 距離計算
double SingleConfrrt_vector::distance(SingleConfrrt_vector vec)
{
	for ( int i=0;i<6;i++ ) this->tmp_theta[i] = vec.m_theta[i] - this->m_theta[i];

	double sum = 0.0;
	for ( int i=0;i<6;i++ ) sum += this->tmp_theta[i] * this->tmp_theta[i];

	double dist = sqrt( sum );

	return dist;
}

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

CSingleConfRRT::CSingleConfRRT()
{
	srand((unsigned int)time(NULL));
	init_gen_rand((uint32_t)time(NULL));
}

CSingleConfRRT::~CSingleConfRRT()
{
}

// メイン処理関数
bool CSingleConfRRT::MainProcess()
{
	int num_of_node = 0;
	
	// 接続できるまで繰り返す
	while ( !m_reached ) {
		ExtendTree();
		if( (num_of_node++) > MAXNODE )	
		{
			printf("SingleConfノードオーバー\n");
			printf("[0] = %d\n",m_ArmCollision.m_count[0]);
			printf("[1] = %d\n",m_ArmCollision.m_count[1]);
			printf("[2] = %d\n",m_ArmCollision.m_count[2]);
			printf("[3] = %d\n",m_ArmCollision.m_count[3]);
			printf("[4] = %d\n",m_ArmCollision.m_count[4]);
			printf("[5] = %d\n",m_ArmCollision.m_count[5]);
			printf("ノード数:%d\n",(int)m_n_node.size());
			//printf("SingleConfRRT失敗\n");
			return false; // MAXNODE回繰り返してもゴールに達しなかった場合は失敗
		}
		if ( num_of_node%200 == 0 ) printf("num_of_node = %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;
	}

	return true;
}

void CSingleConfRRT::ExtendTree()
{	
	bool rand_flag = true;
	SingleConfrrt_vector q_rand, q_new;
	SingleConfrrt_node n_new;

	// ノード探索ループ
	for ( int i=0;i<20;i++ ) // 回数制限のためのfor文
	{
		//q_randの作成
		if( CalcRandom() > (1.0 - m_GoalRatio) ) // 確率でゴールの位置をセットする
		{
			q_rand = m_q_goal.pos;
		}
		else
		{
			for ( int j=0;j<6;j++ ) q_rand.m_theta[j] = RandomTheta(j);
		}

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

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

		// 新しいノードの座標が決定したので、これをノードとする
		for ( int j=0;j<6;j++ ) n_new.pos.m_theta[j] = q_new.m_theta[j];

		n_new.b_link = near_node_num;

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

		n_new.numLink = numLink+1;

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

		if ( i == 19 ) return;
	}

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

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

	return;
}

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

	min = q_pos.distance( m_n_node[0].pos );
	near_node_num = 0;
	//printf("min = %lf\n",min);

	for ( int i=1;i<(int)m_n_node.size();i++ )
	{
		dist = q_pos.distance( m_n_node[i].pos );
		//printf("dist = %lf\n",dist);

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

// 障害物の領域内に入り込んでいないか true:ぶつからない false:ぶつかる
bool CSingleConfRRT::DetectCollision(SingleConfrrt_node &new_node)
{
	// 衝突判定
	new_node.Pose = 71;

	double Angle[6];

	for ( int i=0;i<6;i++ ) Angle[i] = new_node.pos.m_theta[i] * M_PI / 180;

	m_ArmCollision.SetAngle(Angle);

	for ( int j=0;j<(int)m_v_roboX.size();j++ )
	{
		double P[3] = {m_v_roboX[j], m_v_roboY[j], m_v_roboZ[j]};

		if ( m_ArmCollision.isCollision(P, m_isLeft) ) 
		{
			return false; // 衝突するので不採用
		}
	}

	return true;
}

// スタート
bool CSingleConfRRT::StartRRT(double StartPos[6], double GoalPos[6], bool isLeft, double GoalRatio)
{
	SetObstacle(); // 障害物セット

	m_GoalRatio = GoalRatio;

	m_n_result.clear();

	m_isLeft = isLeft;

	if ( isLeft)
	{
		m_ThetaScopeMin[0] = THETA_00_MIN;
		m_ThetaScopeMin[1] = THETA_01_MIN;
		m_ThetaScopeMin[2] = THETA_02_MIN;
		m_ThetaScopeMin[3] = THETA_03_MIN;
		m_ThetaScopeMin[4] = THETA_04_MIN;
		m_ThetaScopeMin[5] = THETA_05_MIN;

		m_ThetaScopeMax[0] = THETA_00_MAX;
		m_ThetaScopeMax[1] = THETA_01_MAX;
		m_ThetaScopeMax[2] = THETA_02_MAX;
		m_ThetaScopeMax[3] = THETA_03_MAX;
		m_ThetaScopeMax[4] = THETA_04_MAX;
		m_ThetaScopeMax[5] = THETA_05_MAX;
	}
	else
	{
		m_ThetaScopeMin[1] = THETA_07_MIN;
		m_ThetaScopeMin[0] = THETA_06_MIN;
		m_ThetaScopeMin[2] = THETA_08_MIN;
		m_ThetaScopeMin[3] = THETA_09_MIN;
		m_ThetaScopeMin[4] = THETA_10_MIN;
		m_ThetaScopeMin[5] = THETA_11_MIN;

		m_ThetaScopeMax[0] = THETA_06_MAX;
		m_ThetaScopeMax[1] = THETA_07_MAX;
		m_ThetaScopeMax[2] = THETA_08_MAX;
		m_ThetaScopeMax[3] = THETA_09_MAX;
		m_ThetaScopeMax[4] = THETA_10_MAX;
		m_ThetaScopeMax[5] = THETA_11_MAX;
	}

	for ( int i=0;i<6;i++ )
	{
		if ( m_ThetaScopeMin[i] > GoalPos[i] )
		{
			GoalPos[i]+=360;
		}
		if ( m_ThetaScopeMax[i] < GoalPos[i] ) 
		{
			GoalPos[i]-=360;
		}
		if ( m_ThetaScopeMin[i] > GoalPos[i] || m_ThetaScopeMax[i] < GoalPos[i] )  return false;

		if ( m_ThetaScopeMin[i] > StartPos[i] )
		{
			StartPos[i]+=360;
		}
		if ( m_ThetaScopeMax[i] < StartPos[i] ) 
		{
			StartPos[i]-=360;
		}
		if ( m_ThetaScopeMin[i] > StartPos[i] || m_ThetaScopeMax[i] < StartPos[i] )  return false;

		printf("StartJoin[%d] = %lf\n",i,StartPos[i]);
		printf("Goal Join[%d] = %lf\n",i,GoalPos[i]);
	}

	PrepareRRT(StartPos, GoalPos);

	return MainProcess();
}

// 準備
bool CSingleConfRRT::PrepareRRT(double StartPos[6], double GoalPos[6])
{
	m_reached = false;
	m_n_node.clear();

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

	// スタートノード設定
	for ( int j=0;j<6;j++ ) m_q_start.pos.m_theta[j] = StartPos[j];

	// ゴールノード設定
	for ( int j=0;j<6;j++ ) m_q_goal.pos.m_theta[j] = GoalPos[j];

	m_n_node.push_back( m_q_start );

	return true;
}

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

	double dtheta[6];
	for ( int i=0;i<6;i++ ) dtheta[i] = add_pos.m_theta[i] - base_pos.m_theta[i];

	double sum = 0.0;
	for ( int i=0;i<6;i++ ) sum += dtheta[i] * dtheta[i];
	double dist = sqrt(sum);

	for ( int i=0;i<6;i++ )	q_new.m_theta[i] = base_pos.m_theta[i] + EPSILON * dtheta[i] / dist;

	for ( int i=0;i<6;i++ )
	{
		if ( m_ThetaScopeMin[i] > q_new.m_theta[i] )
		{
			printf("dtheta[%d]:%lf\n",i,dtheta[i]);
			printf("dist:%lf\n",dist);
			printf("base_pos.m_theta[i]:%lf\n",base_pos.m_theta[i]);
		}
		if ( m_ThetaScopeMax[i] < q_new.m_theta[i] ) 
		{
			printf("dtheta[%d]:%lf\n",i,dtheta[i]);
		}
	}

	return q_new;
}

// 障害物の設置
void CSingleConfRRT::SetObstacle()
{
	// 土台を障害物とする
	for ( int i=-9;i<9;i++ )
	{
		m_v_roboX.push_back( 320.0  );
		m_v_roboY.push_back( i*50 );
		m_v_roboZ.push_back( 720.0  );
	}

	for ( int i=0;i<20;i++ )
	{
		/*m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( 50 );
		m_v_roboZ.push_back( 730+i*10  );

		m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( -50 );
		m_v_roboZ.push_back( 730+i*10  );*/


		m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( 75 );
		m_v_roboZ.push_back( 730+i*20  );

		m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( -75 );
		m_v_roboZ.push_back( 730+i*20  );


		/*m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( 90 );
		m_v_roboZ.push_back( 730+i*10  );

		m_v_roboX.push_back( 100  );
		m_v_roboY.push_back( -90 );
		m_v_roboZ.push_back( 730+i*10  );*/
	}

	for ( int i=0;i<15;i++ )
	{
		m_v_roboX.push_back( 90  );
		m_v_roboY.push_back( 0+i*20 );
		m_v_roboZ.push_back( 1200  );

		m_v_roboX.push_back( 90  );
		m_v_roboY.push_back( 0-i*20 );
		m_v_roboZ.push_back( 1200  );
	}


	return;
}

// 0〜1の乱数発生
double CSingleConfRRT::CalcRandom()
{
	double r = (double)gen_rand32();
	//double r = (double)rand();
	unsigned int u_int = UINT_MAX;
	if ( r==0 )
	{
	}
	else
	{
		r = r / (double)u_int;
	}
	return r;
}

// 角度の乱数発生
double CSingleConfRRT::RandomTheta(int i)
{
	double  add = (double) ( m_ThetaScopeMax[i] - m_ThetaScopeMin[i] ) * CalcRandom();
	double angle = (double) m_ThetaScopeMin[i] + add;
	return angle;
}

double CSingleConfRRT::AdjustAngle(double angle)
{
	while ( angle < -180 ) angle+=360;
	while ( angle >  180 ) angle-=360;
	return angle;
}
