#pragma once

#include "cv.h"

#define PI 3.14159265

const int ARM_LINK_R	= 800;
const int ARM_LINK_D	= 135;
const int ARM_LINK_L0	= 115;
const int ARM_LINK_L1	= 501;
const int ARM_LINK_L2	= 400;
const int ARM_LINK_L3	= 100;
const int ARM_LINK_L4	= 326;
const int ARM_LINK_LH	= 110;

class CCoordinateTrans
{
public:
	CCoordinateTrans(void);
public:
	~CCoordinateTrans(void);

//パラメータ読み込み
void LoadRobotParameter(char *filename = "RobotParam.txt");

//座標変換(最後のデフォルト引数は変換行列を計算するかどうか)
void Camera2Robot(float pan, float tilt, double camX, double camY, double camZ, double *roboX, double *roboY, double *roboZ, bool flag=true);													//カメラ⇒ロボット
void Robot2World(float roboPosX, float roboPosY, float roboPosTheta, double roboX, double roboY, double roboZ, double *wldX, double *wldY, double *wldZ);										//ロボット⇒ワールド
void Camera2World(float roboPosX, float roboPosY, float roboPosTheta, float pan, float tilt, double camX, double camY, double camZ, double *wldX, double *wldY, double *wldZ, bool flag=true);	//カメラ⇒ワールド
void Robot2LeftArm(double roboX, double roboY, double roboZ, double *armX, double *armY, double *armZ);																							//ロボット⇒左アーム
void Robot2RightArm(double roboX, double roboY, double roboZ, double *armX, double *armY, double *armZ);																						//ロボット⇒右アーム
void LeftArm2LeftHand(float joint[6], double armX, double armY, double armZ, double *handX, double *handY, double *handZ, bool flag=true);														//左アーム⇒左ハンド
void RightArm2RightHand(float joint[6], double armX, double armY, double armZ, double *handX, double *handY, double *handZ, bool flag=true);													//右アーム⇒右ハンド
void Camera2LeftHand(float pan, float tilt, float joint[6], double camX, double camY, double camZ, double *handX, double *handY, double *handZ, bool flag=true);								//カメラ⇒左ハンド
void Camera2RightHand(float pan, float tilt, float joint[6], double camX, double camY, double camZ, double *handX, double *handY, double *handZ, bool flag=true);								//カメラ⇒右ハンド

protected:
	void AllocBuffer();																			//メモリ確保
	void FreeBuffer();																			//メモリ解放
	void CalcTransMatrix(CvMat *trans , double a , double alpha , double d , double theta);		//変換行列作成
	void CalcParallelShiftMatrix(CvMat *trans , double x , double y , double z);				//平行移動行列の作成

	CvMat *m_Pcam;		//カメラ座標
	CvMat *m_Probo;		//ロボット座標
	CvMat *m_Pwld;		//ワールド座標
	CvMat *m_Parm;		//アーム座標
	CvMat *m_Phand;		//ハンド座標

	//カメラ2ロボット
	CvMat *m_Tr_c;
	CvMat *m_Tr_1;
	CvMat *m_T1_2;
	CvMat *m_T2_3;
	CvMat *m_T3_c;

	//アーム2ハンド用
	CvMat *m_Th_a;
	CvMat *m_Th_1;
	CvMat *m_T3_4;
	CvMat *m_T4_5;
	CvMat *m_T5_a;

	//ロボットのパラメータ
	double CAMERA_X;		//パンチルトのチルト軸からレンズ表面までのX座標(ロボット座標系で)
	double CAMERA_Y;		//パンチルトのチルト軸からレンズ表面までのY座標(ロボット座標系で)
	double CAMERA_Z;		//パンチルトのチルト軸からレンズ表面までのZ座標(ロボット座標系で)
	double TILT_SHAFT_X;	// パンチルトのチルト軸のX座標(ロボット座標系で)
	double TILT_SHAFT_Y;	// パンチルトのチルト軸のX座標(ロボット座標系で)
	double TILT_SHAFT_Z;	// パンチルトのチルト軸のZ座標(ロボット座標系で)
	double LEFTARM_X;		// アームの基準点のX座標(ロボット座標系で)
	double LEFTARM_Y;		// アームの基準点のY座標(ロボット座標系で)
	double LEFTARM_Z;		// アームの基準点のZ座標(ロボット座標系で)
	double RIGHTARM_X;		// アームの基準点のX座標(ロボット座標系で)
	double RIGHTARM_Y;		// アームの基準点のY座標(ロボット座標系で)
	double RIGHTARM_Z;		// アームの基準点のZ座標(ロボット座標系で)
};
