#pragma once

#include "OpenCV.h"
#include <vector>

using namespace std;

#define M_PI acos(-1.0)

// アームの初期位置
#define ARM_ORIGIN_X 102.8
#define ARM_ORIGIN_Z 800.0
#define ARM_ORIGIN_Y 74.1 // Y座標は左右のアームの原点からの距離 ( 左右対称であるため)

class ArmObstaclize
{
public:
	ArmObstaclize();
	~ArmObstaclize();

	bool isCollision(double P[3], bool isLeftArm = true);
	void SetAngle(double Angle[6]);
	void MakeArmObstacle(vector<double> *v_X, vector<double> *v_Y, vector<double> *v_Z, bool isLeft = true);

	// debug用
	int m_count[6];

private:
	//
	// 計算用
	//
	CvMat *m_tmpVector;
	CvMat *m_Pa;

	//
	// 回転前
	//
	// 相対ベクトル
	CvMat *m_Link1_r_before;
	CvMat *m_Link2_r_before;
	CvMat *m_Link3_r_before;
	CvMat *m_Link4_r_before;
	CvMat *m_Link5_r_before;
	CvMat *m_Link6_r_before;

	// 絶対ベクトル
	CvMat *m_Link2_a_before;
	CvMat *m_Link3_a_before;
	CvMat *m_Link4_a_before;
	CvMat *m_Link5_a_before;
	CvMat *m_Link6_a_before;

	//
	// 回転後
	//
	// 相対ベクトル
	CvMat *m_Link1_r_after;
	CvMat *m_Link2_r_after;
	CvMat *m_Link3_r_after;
	CvMat *m_Link4_r_after;
	CvMat *m_Link5_r_after;
	CvMat *m_Link6_r_after;

	// 絶対ベクトル
	CvMat *m_Link2_a_after;
	CvMat *m_Link3_a_after;
	CvMat *m_Link4_a_after;
	CvMat *m_Link5_a_after;
	CvMat *m_Link6_a_after;

	//
	// 回転行列
	//
	// アームの各関節のベクトルを求めるための回転行列
	CvMat *m_R1;
	CvMat *m_R2;
	CvMat *m_R3;
	CvMat *m_R4;
	CvMat *m_R5;
	CvMat *m_R6;

	CvMat *m_R2_all;
	CvMat *m_R3_all;
	CvMat *m_R4_all;
	CvMat *m_R5_all;
	CvMat *m_R6_all;

	//
	// 関数
	//
	void CreateRotationMatrixList();
	void CreateRotationMatrixVector();

	void cvmSetRotationMatrixY(CvMat *rot, double angle);
	void cvmSetRotationMatrixZ(CvMat *rot, double angle);
};
