/*

コンフィグレーション空間でのRRT
スタートとゴールを度で与えてください
結果も度です

*/


#pragma once
#include <math.h>
#include <vector>
#include "afxcmn.h"
#include "afxwin.h"
#include "ArmCollision.h"
#include "../SFMT/SFMT.h"

using namespace std;

#define MAXNODE 8000       // ノードの最大値
#define	GOAL_RATIO 0.6   // ゴール選択確率
#define	EPSILON 10   // 移動距離

#define WORKSPACE_X_MIN 400.0		//作業座標
#define WORKSPACE_X_MAX 1000.0	//作業座標
#define WORKSPACE_Y_MIN	-400.0	//作業座標
#define WORKSPACE_Y_MAX	400.0		//作業座標
#define WORKSPACE_Z_MIN	800.0		//作業座標
#define WORKSPACE_Z_MAX	1700.0	//作業座標

// 各関節角の可動範囲
#define THETA_00_MIN  -50
#define THETA_00_MAX   20
#define THETA_01_MIN  -86
#define THETA_01_MAX   80
#define THETA_02_MIN  -60
#define THETA_02_MAX  250
#define THETA_03_MIN -180
#define THETA_03_MAX  180
#define THETA_04_MIN -120
#define THETA_04_MAX    0
#define THETA_05_MIN -180
#define THETA_05_MAX  180

#define THETA_06_MIN  160
#define THETA_06_MAX  230
#define THETA_07_MIN   90
#define THETA_07_MAX  270
#define THETA_08_MIN  -70
#define THETA_08_MAX  250
#define THETA_09_MIN -180
#define THETA_09_MAX  180
#define THETA_10_MIN -120
#define THETA_10_MAX    0
#define THETA_11_MIN -180
#define THETA_11_MAX  180


// ベクトルクラス
class SingleConfrrt_vector{
public:
	SingleConfrrt_vector();
	SingleConfrrt_vector(double theta[6]);
	~SingleConfrrt_vector();

	double m_theta[6]; // 計算用
	double distance(SingleConfrrt_vector); // ベクトル同士の距離
private:
	double tmp_theta[6]; // データ保持用
};

// ノードデータ
class SingleConfrrt_node{
public:
	//コンストラクタ
	SingleConfrrt_node();									
	SingleConfrrt_node(int back_node);
	//デストラクタ
	~SingleConfrrt_node();

	SingleConfrrt_vector pos;		// 座標データ
	int b_link;			// Backリンク
	int numLink;		// スタートから数えて何個目のノードか
	char Pose;          // 姿勢の種類
};

// RRT本体
class CSingleConfRRT
{
public:
	CSingleConfRRT();
	~CSingleConfRRT();

	bool StartRRT(double StartPos[6], double GoalPos[6], bool isLeft, double GoalRatio=GOAL_RATIO);
	vector<SingleConfrrt_node> m_n_result; // 結果データノード
	vector<SingleConfrrt_node> m_n_node;	 // 全ノード

private:
	// 初期値
	double m_GoalRatio;
	SingleConfrrt_node m_q_start;			 // スタートノード
	SingleConfrrt_node m_q_goal;			 // ゴールノード

	bool m_reached; //ゴールに達したか？
	bool m_isLeft;
	double m_ThetaScopeMin[6]; // 関節の可動範囲 左右で切り替える
	double m_ThetaScopeMax[6];

	bool MainProcess(); // メイン処理関数
	void SetObstacle();
	bool PrepareRRT(double StartPos[6], double GoalPos[6]); // 準備
	void ExtendTree();
	bool DetectCollision(SingleConfrrt_node &node); // 衝突判定
	int NearestNeighbor(SingleConfrrt_vector); // 近接ノード検索
	SingleConfrrt_vector AdjustNodeDistance( SingleConfrrt_vector base_pos , SingleConfrrt_vector add_pos ); // 距離調整

	ArmCollision m_ArmCollision;
private:
	double CalcRandom();//{ return (double)gen_rand32/65535.0;};//return (double)rand()/32768.0;}; // 0〜1の乱数を発生
	double RandomTheta(int i); // 作業領域の角度の乱数を発生
	double AdjustAngle(double angle);
public:
	// 障害物
	vector<double> m_v_roboX;
	vector<double> m_v_roboY;
	vector<double> m_v_roboZ;
};
