// ArmSimulatorDlg.cpp : 実装ファイル
//

#include "stdafx.h"
#include "ArmSimulator.h"
#include "ArmSimulatorDlg.h"
#include <math.h>
#include "RRT/RRT.h"
#include "RRT/Pos2Joint.h"
#include "RRT/CoordinateTrans.h"
#include "ArmObstaclize.h"
#include "Log.h"

#define ARM_LEFT_X	102.8
#define ARM_LEFT_Y	74.1
#define ARM_LEFT_Z	1187

#ifdef _DEBUG
#define new DEBUG_NEW
#endif


// CArmSimulatorDlg ダイアログ

WNDPROC CArmSimulatorDlg::m_originalWndProc[BUTTON_NUM];

extern "C"
{
int PASCAL WinMain(HINSTANCE inst,HINSTANCE dumb,LPSTR param,int show);
};


int main(int ac,char *av[])
{
	char buf[256];
	HINSTANCE inst;

	inst=(HINSTANCE)GetModuleHandle(NULL);

	buf[0]=0;
	for(int i=1; i<ac; i++)
	{
		strcat(buf,av[i]);
		strcat(buf," ");
	}

	return WinMain(inst,NULL,buf,SW_SHOWNORMAL);
}



CArmSimulatorDlg::CArmSimulatorDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CArmSimulatorDlg::IDD, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CArmSimulatorDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	DDX_Control(pDX, IDC_SLIDER1, m_sldr_Step);
	DDX_Control(pDX, IDC_EDIT1, m_edit_Step);
	DDX_Control(pDX, IDC_CHECK1, m_check_GUI);
	DDX_Control(pDX, IDOK, m_button_End);
	DDX_Control(pDX, IDC_EDIT2, m_edit_ArmInfoLeft);
	DDX_Control(pDX, IDC_EDIT3, m_edit_ArmInfoRight);
	DDX_Control(pDX, IDC_BUTTON2, m_button_Start);
	DDX_Control(pDX, IDC_CHECK2, m_check_manual);
	DDX_Control(pDX, IDC_EDIT4, m_edit_TargetLX);
	DDX_Control(pDX, IDC_EDIT5, m_edit_TargetLY);
	DDX_Control(pDX, IDC_EDIT6, m_edit_TargetLZ);
	DDX_Control(pDX, IDC_EDIT7, m_edit_TargetRX);
	DDX_Control(pDX, IDC_EDIT8, m_edit_TargetRY);
	DDX_Control(pDX, IDC_EDIT9, m_edit_TargetRZ);
	DDX_Control(pDX, IDC_EDIT10, m_edit_PassPath);
	DDX_Control(pDX, IDC_CHECK3, m_Check_slow);
}

BEGIN_MESSAGE_MAP(CArmSimulatorDlg, CDialog)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	//}}AFX_MSG_MAP
	ON_NOTIFY(NM_CUSTOMDRAW, IDC_SLIDER1, &CArmSimulatorDlg::OnNMCustomdrawSlider1)
	ON_EN_CHANGE(IDC_EDIT1, &CArmSimulatorDlg::OnEnChangeEdit1)
	ON_BN_CLICKED(IDC_BUTTON2, &CArmSimulatorDlg::OnBnClickedButton2)
	ON_BN_CLICKED(IDOK, &CArmSimulatorDlg::OnBnClickedOk)
	ON_BN_CLICKED(IDC_CHECK1, &CArmSimulatorDlg::OnBnClickedCheck1)
	ON_BN_CLICKED(IDC_BUTTON31, &CArmSimulatorDlg::OnBnClickedButton31)
	ON_BN_CLICKED(IDC_CHECK2, &CArmSimulatorDlg::OnBnClickedCheck2)
	ON_BN_CLICKED(IDC_BUTTON32, &CArmSimulatorDlg::OnBnClickedButton32)
	ON_BN_CLICKED(IDC_BUTTON33, &CArmSimulatorDlg::OnBnClickedButton33)
	ON_BN_CLICKED(IDC_BUTTON34, &CArmSimulatorDlg::OnBnClickedButton34)
	ON_BN_CLICKED(IDC_BUTTON35, &CArmSimulatorDlg::OnBnClickedButton35)
	ON_BN_CLICKED(IDC_BUTTON36, &CArmSimulatorDlg::OnBnClickedButton36)
	ON_BN_CLICKED(IDC_BUTTON38, &CArmSimulatorDlg::OnBnClickedButton38)
	ON_BN_CLICKED(IDC_BUTTON37, &CArmSimulatorDlg::OnBnClickedButton37)
END_MESSAGE_MAP()


// CArmSimulatorDlg メッセージ ハンドラ

BOOL CArmSimulatorDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// このダイアログのアイコンを設定します。アプリケーションのメイン ウィンドウがダイアログでない場合、
	//  Framework は、この設定を自動的に行います。
	SetIcon(m_hIcon, TRUE);			// 大きいアイコンの設定
	SetIcon(m_hIcon, FALSE);		// 小さいアイコンの設定

	//ShowWindow(SW_MINIMIZE);
	// 閉じるボタン無効化
	CMenu* pMenu	= GetSystemMenu( FALSE );
	pMenu->EnableMenuItem( SC_CLOSE, MF_GRAYED );


	// TODO: 初期化をここに追加します。
	SetDefID(IDC_BUTTON1);

	m_sldr_Step.SetRange(1,100);
	m_sldr_Step.SetTicFreq(5);
	m_sldr_Step.SetPageSize(5);
	m_sldr_Step.SetLineSize(5);
	m_sldr_Step.SetPos(10);
	m_edit_Step.SetWindowTextA("0.01");
	m_check_GUI.SetCheck(1);

	m_ArmSim.m_EndSim = true;

	m_JointAngleL.clear(); // パスの関節角をストックしておくベクター
	m_JointAngleR.clear();

	/////////////// プロシージャの差し替え //////////////
	for(int i=0 ; i<BUTTON_NUM ; i++ )
	{
		CWnd *btn = GetDlgItem( IDC_BUTTON3+i );
		m_originalWndProc[i] = (WNDPROC)SetWindowLong( btn->m_hWnd , GWL_WNDPROC , (LONG)MoveButtonProc );
	
	}

	AfxBeginThread(ShowInfoThread,this); // GUIに情報を表示する

	return TRUE;  // フォーカスをコントロールに設定した場合を除き、TRUE を返します。
}

// ダイアログに最小化ボタンを追加する場合、アイコンを描画するための
//  下のコードが必要です。ドキュメント/ビュー モデルを使う MFC アプリケーションの場合、
//  これは、Framework によって自動的に設定されます。

void CArmSimulatorDlg::OnPaint()
{
	if (IsIconic())
	{
		CPaintDC dc(this); // 描画のデバイス コンテキスト

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// クライアントの四角形領域内の中央
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// アイコンの描画
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

// ユーザーが最小化したウィンドウをドラッグしているときに表示するカーソルを取得するために、
//  システムがこの関数を呼び出します。
HCURSOR CArmSimulatorDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}


void CArmSimulatorDlg::OnNMCustomdrawSlider1(NMHDR *pNMHDR, LRESULT *pResult)
{
	LPNMCUSTOMDRAW pNMCD = reinterpret_cast<LPNMCUSTOMDRAW>(pNMHDR);
	// TODO: ここにコントロール通知ハンドラ コードを追加します。
	CString str;
	double step = (double)m_sldr_Step.GetPos()/1000.0;
	str.Format("%.3lf",step);
	m_edit_Step.SetWindowTextA(str);
	m_ArmSim.m_WorldStep = step;
	*pResult = 0;
}

void CArmSimulatorDlg::OnEnChangeEdit1()
{
	// TODO:  これが RICHEDIT コントロールの場合、
	// まず、CDialog::OnInitDialog() 関数をオーバーライドして、OR 状態の ENM_CHANGE
	// フラグをマスクに入れて、CRichEditCtrl().SetEventMask() を呼び出さない限り、
	// コントロールは、この通知を送信しません。

	// TODO:  ここにコントロール通知ハンドラ コードを追加してください。
	CString str;
	m_edit_Step.GetWindowTextA(str);
	m_sldr_Step.SetPos((int)(atof(str)*1000));
	m_ArmSim.m_WorldStep = atof(str);
}

void CArmSimulatorDlg::OnBnClickedButton2()
{
	// TODO: ODEのシミュレーション開始
	m_button_Start.EnableWindow(FALSE);

	if ( m_check_manual.GetCheck() == 1 ) 
	{
		m_ArmSim.m_UseManual = true;
		//AfxMessageBox("check off");
	}
	else                                  
	{
		m_ArmSim.m_UseManual = false;
		//AfxMessageBox("check on");
	}

	m_ArmSim.m_InitAngleL[0] = 19*M_PI/180;
	m_ArmSim.m_InitAngleL[1] = 64*M_PI/180;
	m_ArmSim.m_InitAngleL[2] = -51*M_PI/180;
	m_ArmSim.m_InitAngleL[3] = 0;
	m_ArmSim.m_InitAngleL[4] = -5.74*M_PI/180;
	m_ArmSim.m_InitAngleL[5] = -89.96*M_PI/180;

	m_ArmSim.m_TargetAngleL[0] = 0*M_PI/180;
	m_ArmSim.m_TargetAngleL[1] = -67*M_PI/180;
	m_ArmSim.m_TargetAngleL[2] = 229*M_PI/180;
	m_ArmSim.m_TargetAngleL[3] = 0;
	m_ArmSim.m_TargetAngleL[4] = -5.74*M_PI/180;
	m_ArmSim.m_TargetAngleL[5] = -89.96*M_PI/180;


	m_ArmSim.m_InitAngleR[0] = M_PI;
	m_ArmSim.m_InitAngleR[1] = -124*M_PI/180;
	m_ArmSim.m_InitAngleR[2] = -34*M_PI/180;
	m_ArmSim.m_InitAngleR[3] = 0;
	m_ArmSim.m_InitAngleR[4] = 0;
	m_ArmSim.m_InitAngleR[5] = 0;

	m_ArmSim.m_TargetAngleR[0] = M_PI;
	m_ArmSim.m_TargetAngleR[1] = -124*M_PI/180;
	m_ArmSim.m_TargetAngleR[2] = -34*M_PI/180;
	m_ArmSim.m_TargetAngleR[3] = 0;
	m_ArmSim.m_TargetAngleR[4] = 0;
	m_ArmSim.m_TargetAngleR[5] = 0;

	// 掴んだ物体
	m_ArmSim.m_Object.x = 0.1;
	m_ArmSim.m_Object.y = 0.1;
	m_ArmSim.m_Object.z = 0.2;
	m_ArmSim.m_Object.m = 1;
	m_ArmSim.m_GraspHand = GRASP_NOTHING;

	// 障害物
	
	m_ArmSim.m_Obstacle.clear();
	Object obstacle;
	obstacle.x = 0.05;
	obstacle.y = 0.05;
	obstacle.z = 0.05;
	obstacle.m = 1;

	CCoordinateTrans trans;
	double robotX,robotY,robotZ;
	char buf[256];

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

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

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

	if ( fp == NULL )
	{
		printf("障害物情報のファイルが読み込めません\n");
		Sleep(3000);
		return;
	}

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

		if ( camX==0 && camY==0 && 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,y,z;
		bool flag = false;
		for ( int i=0;i<13;i++ )
		{
			if ( fabs( (400.0 + (double)i*50.0) - robotX ) < 50 )
			{
				x = i;
				if ( flag ) break;
				else        flag = true;
			}
		}
		flag = false;
		for ( int i=0;i<17;i++ )
		{
			if ( fabs( (-400.0 + (double)i*50.0) - robotY ) < 50 )
			{
				y = i;
				if ( flag ) break;
				else        flag = true;
			}
		}
		flag = false;
		for ( int i=0;i<19;i++ )
		{
			if ( fabs( (800.0 + (double)i*50.0) - robotZ ) < 50 )
			{
				z = i;
				if ( flag ) break;
				else        flag = true;
			}
		}

		if ( x < 0 || x > 12 ) continue;
		if ( y < 0 || y > 16 ) continue;
		if ( z < 0 || z > 18 ) continue;

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

	fclose(fp);

	for ( int i=0;i<13;i++ )
	{
		for ( int j=0;j<17;j++ )
		{
			for ( int k=0;k<19;k++ )
			{
				if ( Vote_p[i][j][k] > 10 ) // 投票数が10個以上
				{
					obstacle.Pos[0] = ((double)i*50.0 + 400) /1000.0;
					obstacle.Pos[1] = ((double)j*50.0 - 400) /1000.0;
					obstacle.Pos[2] = ((double)k*50.0 + 800) /1000.0;
					m_ArmSim.m_Obstacle.push_back(obstacle);
				}
			}
		}
	}

	// 土台を障害物にする
	for ( int i=-9;i<9;i++ )
	{
		obstacle.Pos[0] = 300.0/1000.0;
		obstacle.Pos[1] = i*50.0/1000.0;
		obstacle.Pos[2] = 720.0/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);
	}

	for ( int i=0;i<40;i++ )
	{
		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = 50/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = -50/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = 75/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = -75/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = 90/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = -90/1000.0;
		obstacle.Pos[2] = (730+i*10)/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);
	}

	for ( int i=0;i<30;i++ )
	{
		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = (0+i*10)/1000.0;
		obstacle.Pos[2] = 1200/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);

		obstacle.Pos[0] = 100/1000.0;
		obstacle.Pos[1] = (0-i*10)/1000.0;
		obstacle.Pos[2] = 1200/1000.0;
		m_ArmSim.m_Obstacle.push_back(obstacle);
	}

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



	/*{
		m_ArmSim.m_Obstacle.clear();
		Object obstacle;
		obstacle.x = 0.03;
		obstacle.y = 0.03;
		obstacle.z = 0.03;
		obstacle.m = 1;
		FILE *fp = fopen("obstacle.txt","r");
		if ( fp == NULL )
		{
			AfxMessageBox("ファイルオープンエラー");
			return;
		}
		while ( fgets(buf,256,fp) )
		{
			sscanf(buf,"%lf,%lf,%lf",&obstacle.Pos[0], &obstacle.Pos[1], &obstacle.Pos[2]);
			obstacle.Pos[0] /= 1000;
			obstacle.Pos[1] /= 1000;
			obstacle.Pos[2] /= 1000;
			m_ArmSim.m_Obstacle.push_back(obstacle);
		}
		fclose(fp);
	}
	{
		FILE *fp = fopen("angle.txt","r");
		if ( fp == NULL )
		{
			AfxMessageBox("ファイルオープンエラー");
			return;
		}
		fgets(buf,256,fp);
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf",&m_ArmSim.m_TargetAngleL[0], &m_ArmSim.m_TargetAngleL[1], &m_ArmSim.m_TargetAngleL[2],
						                     &m_ArmSim.m_TargetAngleL[3], &m_ArmSim.m_TargetAngleL[4], &m_ArmSim.m_TargetAngleL[5] );
		fclose(fp);
	}*/

	// 軌道の手先を障害物化
	/*{
		m_ArmSim.m_Obstacle.clear();

		char buf[256];
		float L[6],R[6];
		FILE *fp =fopen(m_DualArmPassPath,"r");
		if ( fp == NULL )
		{
			AfxMessageBox("ファイル読み込みエラー");
		}
		else
		{

			float PL[6],PR[6];
			Object obstacle;

			obstacle.x = 0.04;
			obstacle.y = 0.04;
			obstacle.z = 0.04;
			obstacle.m = 1;

			while ( fgets(buf,256,fp) )
			{
				sscanf(buf, "Left:%f,%f,%f,%f,%f,%f,Right:%f,%f,%f,%f,%f,%f",&L[0], &L[1], &L[2], &L[3], &L[4], &L[5], 
																			 &R[0], &R[1], &R[2], &R[3], &R[4], &R[5] );
				

				CalcKinematics(L,PL);
				CalcKinematics(R,PR);

				// ロボット座標へ変換
				obstacle.Pos[0] = PL[0] + ARM_LEFT_X;
				obstacle.Pos[1] = PL[1] + ARM_LEFT_Y;
				obstacle.Pos[2] = PL[2] + ARM_LEFT_Z;// + 20;

				
				obstacle.Pos[0] /= 1000;
				obstacle.Pos[1] /= 1000;
				obstacle.Pos[2] /= 1000;
				m_ArmSim.m_Obstacle.push_back(obstacle);


				obstacle.Pos[0] = PR[0] + ARM_LEFT_X;
				obstacle.Pos[1] = PR[1] - ARM_LEFT_Y;
				obstacle.Pos[2] = PR[2] + ARM_LEFT_Z;

				obstacle.Pos[0] /= 1000;
				obstacle.Pos[1] /= 1000;
				obstacle.Pos[2] /= 1000;
				m_ArmSim.m_Obstacle.push_back(obstacle);
			}

			fclose(fp);
		}
	}*/

	/*
	m_ArmSim.m_Obstacle.clear();
	Object obstacle;
	obstacle.x = 0.1;
	obstacle.y = 0.15;
	obstacle.z = 0.1;
	obstacle.m = 1;
	
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.0;
	obstacle.Pos[2] = 1.0;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.2;
	obstacle.Pos[2] = 1.0;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.4;
	obstacle.Pos[2] = 1.0;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.6;
	obstacle.Pos[2] = 1.0;
	m_ArmSim.m_Obstacle.push_back(obstacle);

	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.0;
	obstacle.Pos[2] = 0.6;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.2;
	obstacle.Pos[2] = 0.6;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = 0.6;
	obstacle.Pos[2] = 0.6;
	m_ArmSim.m_Obstacle.push_back(obstacle);

	obstacle.Pos[0] = 0.6;
	obstacle.Pos[1] = -0.4;
	obstacle.Pos[2] = 0.8;
	m_ArmSim.m_Obstacle.push_back(obstacle);
	*/
	
	if ( m_ArmSim.m_EndSim == true ) 
	{
		m_ArmSim.m_EndSim = false;
		AfxBeginThread(StartSimulationThread,this);
		AfxBeginThread(ShowInfoThread,       this);
	}
}

UINT __cdecl StartSimulationThread(LPVOID pParam)
{
	CArmSimulatorDlg *p = reinterpret_cast<CArmSimulatorDlg *>(pParam);
	p->m_isCollision = p->m_ArmSim.StartSimulation();
	return 0;
}

// GUIに情報を表示する
UINT __cdecl ShowInfoThread(LPVOID pParam)
{
	CArmSimulatorDlg *p = reinterpret_cast<CArmSimulatorDlg *>(pParam);

	CString L,R;
	CString tmp;

	double sin_min = 0;
	double cos_min = 0;
	double theta_min = 0;
	double InitParamX[6] = {0};
	double InitParamY[6] = {0};
	double InitParamZ[6] = {0};

	Sleep(1000);
	
	while ( p->m_ArmSim.m_EndSim == false )
	{
		L = "";
		R  ="";
		float JL[6] = {0.0};
		float JR[6] = {0.0};
		float PL[6] = {0.0};
		float PR[6] = {0.0};

		for ( int i=0;i<6;i++ ) JL[i] = p->m_ArmSim.m_JointAngleL[i];
		for ( int i=0;i<6;i++ ) JR[i] = p->m_ArmSim.m_JointAngleR[i];

		CalcKinematics(JL,PL);
		PL[0]+=ARM_LEFT_X;
		PL[1]+=ARM_LEFT_Y;
		PL[2]+=ARM_LEFT_Z;

		CalcKinematics(JR,PR);
		PR[0]+=ARM_LEFT_X;
		PR[1]-=ARM_LEFT_Y;
		PR[2]+=ARM_LEFT_Z;

		for ( int i=0;i<6;i++ )
		{
			tmp.Format("%d: %.2lf度, %.1fmm\r\n",i+1,p->m_ArmSim.m_JointAngleL[i]*180/acos(-1.0), PL[i]);
			L += tmp;
			tmp.Format("%d: %.2lf度, %.1fmm\r\n",i+1,p->m_ArmSim.m_JointAngleR[i]*180/acos(-1.0), PR[i]);
			R += tmp;
		}
		p->m_edit_ArmInfoLeft.SetWindowTextA(L);
		p->m_edit_ArmInfoRight.SetWindowTextA(R);

		// debug
		double angle[6];
		double pos[6] = {(double)PL[0],(double)PL[1],(double)PL[2]};

		for ( int i=0;i<6;i++ ) angle[i] = (double)p->m_ArmSim.m_JointAngleL[i];

		/*ArmCollision tmpArmCollision;

		tmpArmCollision.SetAngle(angle);
		tmpArmCollision.isCollision(pos,true);*/

		/*if ( 0 )
		{
			if ( sin_min < fabs(tmpArmCollision.m_dsin) )
			{
				sin_min = fabs(tmpArmCollision.m_dsin);
				printf("sin_max = %lf",sin_min);
			}
			if ( cos_min < (tmpArmCollision.m_dcos) )
			{
				cos_min = fabs(tmpArmCollision.m_dcos);
				printf("cos_max = %lf",cos_min);
			}
			if ( theta_min < fabs(tmpArmCollision.m_Theta) )
			{
				theta_min = fabs(tmpArmCollision.m_Theta);
				printf("theta_max = %lf",theta_min);
			}
		}*/


		Sleep(1000);
	}

	L += "停止中";
	R += "停止中";

	p->m_edit_ArmInfoLeft.SetWindowTextA(L);
	p->m_edit_ArmInfoRight.SetWindowTextA(R);

	return 0;
}

void CArmSimulatorDlg::OnBnClickedOk()
{
	// TODO: 終了ボタン
	m_button_End.SetWindowTextA("終了中...");
	m_button_End.EnableWindow(FALSE);
	m_ArmSim.m_EndSim = true;
	Sleep(500);
	OnOK();
}

void CArmSimulatorDlg::OnBnClickedCheck1()
{
	// TODO: ここにコントロール通知ハンドラ コードを追加します。
	UpdateData(FALSE);
	int check = m_check_GUI.GetCheck();
	if (  check == 1 ) m_ArmSim.m_UseGUI = true;
	else               m_ArmSim.m_UseGUI = false;
}


// ボタン押しっぱなしのための処理
LRESULT CALLBACK CArmSimulatorDlg::MoveButtonProc( HWND hwnd , UINT msg , WPARAM wp , LPARAM lp )
{
	int id = ::GetDlgCtrlID(hwnd);				// idの取得
	int index = id - IDC_BUTTON3;		// idから配列のindexを計算

	switch( msg )
	{
	case WM_LBUTTONDOWN:	// ボタンが押された時の処理
		{
			CArmSimulatorDlg *pThis =  static_cast<CArmSimulatorDlg*>( CWnd::FromHandle( ::GetParent(hwnd) ) );
			
			if      ( index == 0  ) pThis->m_ArmSim.m_VelosityL[0] =  2.0;
			else if ( index == 1  ) pThis->m_ArmSim.m_VelosityL[1] = -2.0;
			else if ( index == 2  ) pThis->m_ArmSim.m_VelosityL[2] = -2.0;
			else if ( index == 3  ) pThis->m_ArmSim.m_VelosityL[3] = -2.0;
			else if ( index == 4  ) pThis->m_ArmSim.m_VelosityL[4] =  2.0;
			else if ( index == 5  ) pThis->m_ArmSim.m_VelosityL[5] = -2.0;
			else if ( index == 6  ) pThis->m_ArmSim.m_VelosityL[6] = -2.0;

			else if ( index == 7  ) pThis->m_ArmSim.m_VelosityL[0] = -2.0;
			else if ( index == 8  ) pThis->m_ArmSim.m_VelosityL[1] =  2.0;
			else if ( index == 9  ) pThis->m_ArmSim.m_VelosityL[2] =  2.0;
			else if ( index == 10 ) pThis->m_ArmSim.m_VelosityL[3] =  2.0;
			else if ( index == 11 ) pThis->m_ArmSim.m_VelosityL[4] = -2.0;
			else if ( index == 12 ) pThis->m_ArmSim.m_VelosityL[5] =  2.0;
			else if ( index == 13 ) pThis->m_ArmSim.m_VelosityL[6] =  2.0;


			else if ( index == 14 ) pThis->m_ArmSim.m_VelosityR[0] =  2.0;
			else if ( index == 15 ) pThis->m_ArmSim.m_VelosityR[1] = -2.0;
			else if ( index == 16 ) pThis->m_ArmSim.m_VelosityR[2] = -2.0;
			else if ( index == 17 ) pThis->m_ArmSim.m_VelosityR[3] = -2.0;
			else if ( index == 18 ) pThis->m_ArmSim.m_VelosityR[4] =  2.0;
			else if ( index == 19 ) pThis->m_ArmSim.m_VelosityR[5] = -2.0;
			else if ( index == 20 ) pThis->m_ArmSim.m_VelosityR[6] = -2.0;

			else if ( index == 21 ) pThis->m_ArmSim.m_VelosityR[0] = -2.0;
			else if ( index == 22 ) pThis->m_ArmSim.m_VelosityR[1] =  2.0;
			else if ( index == 23 ) pThis->m_ArmSim.m_VelosityR[2] =  2.0;
			else if ( index == 24 ) pThis->m_ArmSim.m_VelosityR[3] =  2.0;
			else if ( index == 25 ) pThis->m_ArmSim.m_VelosityR[4] = -2.0;
			else if ( index == 26 ) pThis->m_ArmSim.m_VelosityR[5] =  2.0;
			else if ( index == 27 ) pThis->m_ArmSim.m_VelosityR[6] =  2.0;

			break;
		}
	case WM_LBUTTONUP:		// ボタンが離れたときの処理
		{
			CArmSimulatorDlg *pThis =  static_cast<CArmSimulatorDlg*>( CWnd::FromHandle( ::GetParent(hwnd) ) );
			for ( int i=0;i<8;i++ )
			{
				pThis->m_ArmSim.m_VelosityL[i] = 0;
				pThis->m_ArmSim.m_VelosityR[i] = 0;
			}
			break;
		}
	}

	// 元のウィンドウプロシージャをコール
	return CallWindowProc( m_originalWndProc[id-IDC_BUTTON3] , hwnd , msg , wp , lp );
}
void CArmSimulatorDlg::OnBnClickedButton31()
{
	// TODO: 停止ボタン
	m_ArmSim.m_EndSim = true;
	m_button_Start.EnableWindow(TRUE);
}

void CArmSimulatorDlg::OnBnClickedCheck2()
{
	// TODO: 手動チェックボックス
	if ( m_check_manual.GetCheck() == 1 ) m_ArmSim.m_UseManual = true;
	else                                  m_ArmSim.m_UseManual = false;
}

void CArmSimulatorDlg::OnBnClickedButton32()
{
	// TODO: 経路作成ボタン(Left Arm)
	m_JointAngleL.clear();

	// エディットボックスから数値を読み込む
	int Tx,Ty,Tz;
	CString str;

	m_edit_TargetLX.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tx = atoi(str);

	m_edit_TargetLY.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Ty = atoi(str);

	m_edit_TargetLZ.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tz = atoi(str);

	for ( int i=0;i<(int)m_ArmSim.m_Obstacle.size();i++ )
	{
		if ( m_ArmSim.m_Obstacle[i].Pos[0]*1000 > Tx - 40 &&
			 m_ArmSim.m_Obstacle[i].Pos[0]*1000 < Tx + 40 &&
			 m_ArmSim.m_Obstacle[i].Pos[1]*1000 > Ty - 40 &&
			 m_ArmSim.m_Obstacle[i].Pos[1]*1000 < Ty + 40 &&
			 m_ArmSim.m_Obstacle[i].Pos[2]*1000 > Tz - 40 &&
			 m_ArmSim.m_Obstacle[i].Pos[2]*1000 < Tz + 40  )
		{
			 AfxMessageBox("ゴールが障害物");
			return;
		}
	}

}

void CArmSimulatorDlg::OnBnClickedButton33()
{
	// TODO: Moveボタン(Left)
	int Tx,Ty,Tz;
	CString str;

	m_edit_TargetLX.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tx = atoi(str);

	m_edit_TargetLY.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Ty = atoi(str);

	m_edit_TargetLZ.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tz = atoi(str);


	CSingleConfRRT tmpCSingleConfRRT;

	m_JointAngleL.clear();
	JointAngle tmpJointAngle;
	float pos[6] = {Tx,Ty,Tz};
	float joint[6];

	if ( !Pos2JointL(pos,joint,ARM_POSE_ELBOW_DOWN) ) return;

	//double Start[6] = {0,-85,-114,0,-5.9,-90};
	//double Start[6] = {0,-67,229,0,-5.9,-90}; // ok
	double Start[6] = {0,-85,245,0,-5.9,-89};
	double Goal[6]  = {-46,-71,158,248-87-46};

	//for ( int i=0;i<6;i++ ) Start[i] = m_ArmSim.m_JointAngleL[i];
	for ( int i=0;i<6;i++ ) Goal[i]  = joint[i]*180/M_PI;
	//for ( int i=0;i<6;i++ ) printf("Goal[%d]  = %lf\n",i,joint[i]*180/M_PI);

	if ( tmpCSingleConfRRT.StartRRT(Start,Goal,true) )
	{
		for ( int k=(int)tmpCSingleConfRRT.m_n_result.size()-1;k>=0;k-- )
		{
			for ( int j=0;j<6;j++ )
			{
				m_ArmSim.m_TargetAngleL[j] = tmpCSingleConfRRT.m_n_result[k].pos.m_theta[j]*M_PI/180;
				printf("tmpCSingleConfRRT.m_n_result[%d].pos.m_theta[%d] = %lf\n",k,j,tmpCSingleConfRRT.m_n_result[k].pos.m_theta[j]);
			}
			while ( !m_ArmSim.m_isBalanced ) {Sleep(10);} // 平衡待ち
			Sleep(1000);
		}
	}
}

void CArmSimulatorDlg::OnBnClickedButton34()
{
	// TODO: 経路作成ボタン(RightArm)
	m_JointAngleR.clear();

	// エディットボックスから数値を読み込む
	int Tx,Ty,Tz;
	CString str;

	m_edit_TargetRX.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tx = atoi(str);

	m_edit_TargetRY.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Ty = atoi(str);

	m_edit_TargetRZ.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tz = atoi(str);

	
}

void CArmSimulatorDlg::OnBnClickedButton35()
{
	// TODO: Moveボタン(RightArm)
	// エディットボックスから数値を読み込む
	int Tx,Ty,Tz;
	CString str;

	m_edit_TargetRX.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tx = atoi(str);

	m_edit_TargetRY.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Ty = atoi(str);

	m_edit_TargetRZ.GetWindowTextA(str);
	if ( str.GetLength()<1 ) 
	{
		AfxMessageBox("ゴールを入力してください");
		return;
	}
	Tz = atoi(str);


	m_JointAngleR.clear();
	JointAngle tmpJointAngle;
	float pos[6] = {Tx,Ty,Tz};
	//pos[0] += ARM_LEFT_X;
	//pos[1] += ARM_LEFT_Y * 2;
	//pos[2] -= ARM_LEFT_Z / 2;
	float joint[6];
	if ( !Pos2JointR(pos,joint,ARM_POSE_ELBOW_UP) ) return;
	for ( int i=0;i<6;i++ ) tmpJointAngle.joint[i] = joint[i];
	m_JointAngleR.push_back(tmpJointAngle);

	for ( int i=0;i<(int)m_JointAngleR.size();i++)
	{
		m_ArmSim.m_isBalanced  = false;
		m_ArmSim.m_isCollision = false;

		for ( int j=0;j<6;j++ )
		{
			m_ArmSim.m_TargetAngleR[j] = (double)m_JointAngleR[i].joint[j];
			printf("m_ArmSim.m_TargetAngleL[%d] = %lf\n",j,m_ArmSim.m_TargetAngleR[j]*180/M_PI);
		}
		printf("\n");

		while ( !m_ArmSim.m_isBalanced ) {Sleep(10);} // 平衡待ち
		Sleep(1000);
	}
}

UINT __cdecl CalcDualArmInfo1(LPVOID lpPtr);
UINT __cdecl CalcDualArmInfo2(LPVOID lpPtr);
UINT __cdecl CalcDualArmInfo3(LPVOID lpPtr);
UINT __cdecl CalcDualArmInfo4(LPVOID lpPtr);

char Decimal2Binary(char *Binary)
{
	char sum = 0;

	int length = strlen(Binary);

	for ( int i=0;i<length;i++ )
	{
		if ( Binary[i] == '1' ) sum += (char)pow( 2.0,(length-1-i) );
	}

	return sum;
}

void CArmSimulatorDlg::OnBnClickedButton36()
{
	// TODO: testボタン
}

 // 1つめ
UINT __cdecl CalcDualArmInfo1(LPVOID lpPtr)
{
	// メモリの動的確保
	double *****LeftPose_p = new double ****[6];
	double *****RightPose_p = new double ****[6];
	for ( int i=0;i<6;i++ ) 
	{
		LeftPose_p[i] = new double ***[13];
		RightPose_p[i] = new double ***[13];
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			LeftPose_p[i][j] = new double **[17];
			RightPose_p[i][j] = new double **[17];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				LeftPose_p[i][j][k] = new double *[19];
				RightPose_p[i][j][k] = new double *[19];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					LeftPose_p[i][j][k][l] = new double [2];
					RightPose_p[i][j][k][l] = new double [2];
				}
			}
		}
	}


	// データベース読み込み
	char buf[256];
	double angle[6];
	int x=0,y=0,z=0,p=0;

	// 左
	FILE *file_armposL = fopen("LeftPose_ElbowDown.txt","r");
	while ( fgets(buf,256,file_armposL) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) LeftPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposL);

	// 右
	x=0; y=0; z=0; p=0;
	FILE *file_armposR = fopen("RightPose_ElbowDown.txt","r");
	while ( fgets(buf,256,file_armposR) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) RightPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposR);

	printf("\n(1/4)メモリを無事確保\nデータベースの作成にかかります\n\n");



	// 6次元でのアームのいける場所
	ArmCollision tmpArmCollision;
	vector<double> v_obstacleX;
	vector<double> v_obstacleY;
	vector<double> v_obstacleZ;
	ArmObstaclize tmpArmObstaclize;

	FILE *fp  = fopen("DualArmInfo(L-D,R-D).txt","w");

	// 右固定
	for ( int i=0;i<13;i++ )
	{
		printf("1i : %d/12\n",i);
		for ( int j=0;j<17;j++ )
		{
			for ( int k=0;k<19;k++ )
			{
				// 左の衝突判定と計算可能かチェック
				for ( int a=0;a<13;a++ )
				{
					for ( int b=0;b<17;b++ )
					{
						for ( int c=0;c<19;c++ )
						{
							for ( int e=0;e<2;e++ ) // 左の姿勢の4通り
							{
								// まず右アームの姿勢を決める
								for ( int l=0;l<2;l++ )
								{
									bool fail = false;
									if ( RightPose_p[0][i][j][k][l]==0 && RightPose_p[1][i][j][k][l]==0 && RightPose_p[2][i][j][k][l]==0 &&
										 RightPose_p[3][i][j][k][l]==0 && RightPose_p[4][i][j][k][l]==0 && RightPose_p[5][i][j][k][l]==0   ) // ロボット座標を入れてやればおｋ
									{
										fail = true;
									}
									else
									{
										// その時の右アームを障害物とする
										tmpArmObstaclize.SetAngle(&RightPose_p[0][i][j][k][l]);

										tmpArmObstaclize.MakeArmObstacle(&v_obstacleX, &v_obstacleY, &v_obstacleZ);

										if ( LeftPose_p[0][a][b][c][e]==0 && LeftPose_p[1][a][b][c][e]==0 && LeftPose_p[2][a][b][c][e]==0 &&
										     LeftPose_p[3][a][b][c][e]==0 && LeftPose_p[4][a][b][c][e]==0 && LeftPose_p[5][a][b][c][e]==0   ) // ロボット座標を入れてやればおｋ
									{
											fail = true;
										}
									}

									// 衝突判定と計算可能かどうか
									tmpArmCollision.SetAngle(&LeftPose_p[0][a][b][c][e]);
									for ( int d=0;d<(int)v_obstacleX.size();d++ )
									{
										double JudgePos[3] = {v_obstacleX[d], v_obstacleY[d], v_obstacleZ[d]};
										if ( fail || tmpArmCollision.isCollision(JudgePos,true) )
										{
											// 衝突
											fprintf(fp,"0,");
											break;
										}
										else if ( d==(int)v_obstacleX.size()-1 )
										{
											// 大丈夫
											fprintf(fp,"1,");
										}
									}
								}
							}
							fprintf(fp,"\n");
						}
					}
				}
			}
		}
	}
	fclose(fp);



	// 解放
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					delete [] LeftPose_p[i][j][k][l];
					delete [] RightPose_p[i][j][k][l];
				}
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				delete [] LeftPose_p[i][j][k];
				delete [] RightPose_p[i][j][k];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			delete [] LeftPose_p[i][j];
			delete [] RightPose_p[i][j];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		delete [] LeftPose_p[i];
		delete [] RightPose_p[i];
	}

	printf("1つ目(1/4)のデータベース作成成功\n");

	return 0;
}
// 2つめ
UINT __cdecl CalcDualArmInfo2(LPVOID lpPtr)
{
	// メモリの動的確保
	double *****LeftPose_p = new double ****[6];
	double *****RightPose_p = new double ****[6];
	for ( int i=0;i<6;i++ ) 
	{
		LeftPose_p[i] = new double ***[13];
		RightPose_p[i] = new double ***[13];
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			LeftPose_p[i][j] = new double **[17];
			RightPose_p[i][j] = new double **[17];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				LeftPose_p[i][j][k] = new double *[19];
				RightPose_p[i][j][k] = new double *[19];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					LeftPose_p[i][j][k][l] = new double [2];
					RightPose_p[i][j][k][l] = new double [2];
				}
			}
		}
	}


	// データベース読み込み
	char buf[256];
	double angle[6];
	int x=0,y=0,z=0,p=0;

	// 左
	FILE *file_armposL = fopen("LeftPose_ElbowUp.txt","r");
	while ( fgets(buf,256,file_armposL) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) LeftPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposL);

	// 右
	x=0; y=0; z=0; p=0;
	FILE *file_armposR = fopen("RightPose_ElbowDown.txt","r");
	while ( fgets(buf,256,file_armposR) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) RightPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposR);

	printf("\n(2/4)メモリを無事確保\nデータベースの作成にかかります\n\n");



	// 6次元でのアームのいける場所
	ArmCollision tmpArmCollision;
	vector<double> v_obstacleX;
	vector<double> v_obstacleY;
	vector<double> v_obstacleZ;
	ArmObstaclize tmpArmObstaclize;

	FILE *fp  = fopen("DualArmInfo(L-U,R-D).txt","w");

	// 右固定
	for ( int i=0;i<13;i++ )
	{
		printf("2i : %d/12\n",i);
		for ( int j=0;j<17;j++ )
		{
			for ( int k=0;k<19;k++ )
			{
				// 左の衝突判定と計算可能かチェック
				for ( int a=0;a<13;a++ )
				{
					for ( int b=0;b<17;b++ )
					{
						for ( int c=0;c<19;c++ )
						{
							for ( int e=0;e<2;e++ ) // 左の姿勢の4通り
							{
								// まず右アームの姿勢を決める
								for ( int l=0;l<2;l++ )
								{
									bool fail = false;
									if ( RightPose_p[0][i][j][k][l]==0 && RightPose_p[1][i][j][k][l]==0 && RightPose_p[2][i][j][k][l]==0 &&
										 RightPose_p[3][i][j][k][l]==0 && RightPose_p[4][i][j][k][l]==0 && RightPose_p[5][i][j][k][l]==0   ) // ロボット座標を入れてやればおｋ
									{
										fail = true;
									}
									else
									{
										// その時の右アームを障害物とする
										tmpArmObstaclize.SetAngle(&RightPose_p[0][i][j][k][l]);

										tmpArmObstaclize.MakeArmObstacle(&v_obstacleX, &v_obstacleY, &v_obstacleZ);

										if ( LeftPose_p[0][a][b][c][e]==0 && LeftPose_p[1][a][b][c][e]==0 && LeftPose_p[2][a][b][c][e]==0 &&
										     LeftPose_p[3][a][b][c][e]==0 && LeftPose_p[4][a][b][c][e]==0 && LeftPose_p[5][a][b][c][e]==0   ) // ロボット座標を入れてやればおｋ
									{
											fail = true;
										}
									}

									// 衝突判定と計算可能かどうか
									tmpArmCollision.SetAngle(&LeftPose_p[0][a][b][c][e]);
									for ( int d=0;d<(int)v_obstacleX.size();d++ )
									{
										double JudgePos[3] = {v_obstacleX[d], v_obstacleY[d], v_obstacleZ[d]};
										if ( fail || tmpArmCollision.isCollision(JudgePos,true) )
										{
											// 衝突
											fprintf(fp,"0,");
											break;
										}
										else if ( d==(int)v_obstacleX.size()-1 )
										{
											// 大丈夫
											fprintf(fp,"1,");
										}
									}
								}
							}
							fprintf(fp,"\n");
						}
					}
				}
			}
		}
	}
	fclose(fp);



	// 解放
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					delete [] LeftPose_p[i][j][k][l];
					delete [] RightPose_p[i][j][k][l];
				}
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				delete [] LeftPose_p[i][j][k];
				delete [] RightPose_p[i][j][k];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			delete [] LeftPose_p[i][j];
			delete [] RightPose_p[i][j];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		delete [] LeftPose_p[i];
		delete [] RightPose_p[i];
	}

	printf("2つ目(2/4)のデータベース作成成功\n");

	return 0;
}

// 3つめ
UINT __cdecl CalcDualArmInfo3(LPVOID lpPtr)
{
	// メモリの動的確保
	double *****LeftPose_p = new double ****[6];
	double *****RightPose_p = new double ****[6];
	for ( int i=0;i<6;i++ ) 
	{
		LeftPose_p[i] = new double ***[13];
		RightPose_p[i] = new double ***[13];
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			LeftPose_p[i][j] = new double **[17];
			RightPose_p[i][j] = new double **[17];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				LeftPose_p[i][j][k] = new double *[19];
				RightPose_p[i][j][k] = new double *[19];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					LeftPose_p[i][j][k][l] = new double [2];
					RightPose_p[i][j][k][l] = new double [2];
				}
			}
		}
	}


	// データベース読み込み
	char buf[256];
	double angle[6];
	int x=0,y=0,z=0,p=0;

	// 左
	FILE *file_armposL = fopen("LeftPose_ElbowDown.txt","r");
	while ( fgets(buf,256,file_armposL) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) LeftPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposL);

	// 右
	x=0; y=0; z=0; p=0;
	FILE *file_armposR = fopen("RightPose_ElbowUp.txt","r");
	while ( fgets(buf,256,file_armposR) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) RightPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposR);

	printf("\n(3/4)メモリを無事確保\nデータベースの作成にかかります\n\n");



	// 6次元でのアームのいける場所
	ArmCollision tmpArmCollision;
	vector<double> v_obstacleX;
	vector<double> v_obstacleY;
	vector<double> v_obstacleZ;
	ArmObstaclize tmpArmObstaclize;

	FILE *fp  = fopen("DualArmInfo(L-D,R-U).txt","w");

	// 右固定
	for ( int i=0;i<13;i++ )
	{
		printf("3i : %d/12\n",i);
		for ( int j=0;j<17;j++ )
		{
			for ( int k=0;k<19;k++ )
			{
				// 左の衝突判定と計算可能かチェック
				for ( int a=0;a<13;a++ )
				{
					for ( int b=0;b<17;b++ )
					{
						for ( int c=0;c<19;c++ )
						{
							for ( int e=0;e<2;e++ ) // 左の姿勢の4通り
							{
								// まず右アームの姿勢を決める
								for ( int l=0;l<2;l++ )
								{
									bool fail = false;
									if ( RightPose_p[0][i][j][k][l]==0 && RightPose_p[1][i][j][k][l]==0 && RightPose_p[2][i][j][k][l]==0 &&
										 RightPose_p[3][i][j][k][l]==0 && RightPose_p[4][i][j][k][l]==0 && RightPose_p[5][i][j][k][l]==0   ) // ロボット座標を入れてやればおｋ
									{
										fail = true;
									}
									else
									{
										// その時の右アームを障害物とする
										tmpArmObstaclize.SetAngle(&RightPose_p[0][i][j][k][l]);

										tmpArmObstaclize.MakeArmObstacle(&v_obstacleX, &v_obstacleY, &v_obstacleZ);

										if ( LeftPose_p[0][a][b][c][e]==0 && LeftPose_p[1][a][b][c][e]==0 && LeftPose_p[2][a][b][c][e]==0 &&
										     LeftPose_p[3][a][b][c][e]==0 && LeftPose_p[4][a][b][c][e]==0 && LeftPose_p[5][a][b][c][e]==0   ) // ロボット座標を入れてやればおｋ
									{
											fail = true;
										}
									}

									// 衝突判定と計算可能かどうか
									tmpArmCollision.SetAngle(&LeftPose_p[0][a][b][c][e]);
									for ( int d=0;d<(int)v_obstacleX.size();d++ )
									{
										double JudgePos[3] = {v_obstacleX[d], v_obstacleY[d], v_obstacleZ[d]};
										if ( fail || tmpArmCollision.isCollision(JudgePos,true) )
										{
											// 衝突
											fprintf(fp,"0,");
											break;
										}
										else if ( d==(int)v_obstacleX.size()-1 )
										{
											// 大丈夫
											fprintf(fp,"1,");
										}
									}
								}
							}
							fprintf(fp,"\n");
						}
					}
				}
			}
		}
	}
	fclose(fp);



	// 解放
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					delete [] LeftPose_p[i][j][k][l];
					delete [] RightPose_p[i][j][k][l];
				}
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				delete [] LeftPose_p[i][j][k];
				delete [] RightPose_p[i][j][k];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			delete [] LeftPose_p[i][j];
			delete [] RightPose_p[i][j];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		delete [] LeftPose_p[i];
		delete [] RightPose_p[i];
	}

	printf("3つ目(3/4)のデータベース作成成功\n");

	return 0;
}

// 4つめ
UINT __cdecl CalcDualArmInfo4(LPVOID lpPtr)
{
	// メモリの動的確保
	double *****LeftPose_p = new double ****[6];
	double *****RightPose_p = new double ****[6];
	for ( int i=0;i<6;i++ ) 
	{
		LeftPose_p[i] = new double ***[13];
		RightPose_p[i] = new double ***[13];
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			LeftPose_p[i][j] = new double **[17];
			RightPose_p[i][j] = new double **[17];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				LeftPose_p[i][j][k] = new double *[19];
				RightPose_p[i][j][k] = new double *[19];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					LeftPose_p[i][j][k][l] = new double [2];
					RightPose_p[i][j][k][l] = new double [2];
				}
			}
		}
	}


	// データベース読み込み
	char buf[256];
	double angle[6];
	int x=0,y=0,z=0,p=0;

	// 左
	FILE *file_armposL = fopen("LeftPose_ElbowUp.txt","r");
	while ( fgets(buf,256,file_armposL) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) LeftPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposL);

	// 右
	x=0; y=0; z=0; p=0;
	FILE *file_armposR = fopen("RightPose_ElbowUp.txt","r");
	while ( fgets(buf,256,file_armposR) )
	{
		sscanf(buf,"%lf,%lf,%lf,%lf,%lf,%lf,",&angle[0],&angle[1],&angle[2],&angle[3],&angle[4],&angle[5]);
		for ( int i=0;i<6;i++ ) RightPose_p[i][x][y][z][p] = angle[i];

		p++;
		if ( p==2 )
		{
			p=0;
			z++;
		}
		if ( z==19 ) 
		{
			y++;
			z=0;
		}
		if ( y==17 )
		{
			x++;
			y=0;
		}
	}
	fclose(file_armposR);

	printf("\n(4/4)メモリを無事確保\nデータベースの作成にかかります\n\n");



	// 6次元でのアームのいける場所
	ArmCollision tmpArmCollision;
	vector<double> v_obstacleX;
	vector<double> v_obstacleY;
	vector<double> v_obstacleZ;
	ArmObstaclize tmpArmObstaclize;

	FILE *fp  = fopen("DualArmInfo(L-U,R-U).txt","w");

	// 右固定
	for ( int i=0;i<13;i++ )
	{
		printf("4i : %d/12\n",i);
		for ( int j=0;j<17;j++ )
		{
			for ( int k=0;k<19;k++ )
			{
				// 左の衝突判定と計算可能かチェック
				for ( int a=0;a<13;a++ )
				{
					for ( int b=0;b<17;b++ )
					{
						for ( int c=0;c<19;c++ )
						{
							for ( int e=0;e<2;e++ ) // 左の姿勢の4通り
							{
								// まず右アームの姿勢を決める
								for ( int l=0;l<2;l++ )
								{
									bool fail = false;
									if ( RightPose_p[0][i][j][k][l]==0 && RightPose_p[1][i][j][k][l]==0 && RightPose_p[2][i][j][k][l]==0 &&
										 RightPose_p[3][i][j][k][l]==0 && RightPose_p[4][i][j][k][l]==0 && RightPose_p[5][i][j][k][l]==0   ) // ロボット座標を入れてやればおｋ
									{
										fail = true;
									}
									else
									{
										// その時の右アームを障害物とする
										tmpArmObstaclize.SetAngle(&RightPose_p[0][i][j][k][l]);

										tmpArmObstaclize.MakeArmObstacle(&v_obstacleX, &v_obstacleY, &v_obstacleZ);

										if ( LeftPose_p[0][a][b][c][e]==0 && LeftPose_p[1][a][b][c][e]==0 && LeftPose_p[2][a][b][c][e]==0 &&
										     LeftPose_p[3][a][b][c][e]==0 && LeftPose_p[4][a][b][c][e]==0 && LeftPose_p[5][a][b][c][e]==0   ) // ロボット座標を入れてやればおｋ
									{
											fail = true;
										}
									}

									// 衝突判定と計算可能かどうか
									tmpArmCollision.SetAngle(&LeftPose_p[0][a][b][c][e]);
									for ( int d=0;d<(int)v_obstacleX.size();d++ )
									{
										double JudgePos[3] = {v_obstacleX[d], v_obstacleY[d], v_obstacleZ[d]};
										if ( fail || tmpArmCollision.isCollision(JudgePos,true) )
										{
											// 衝突
											fprintf(fp,"0,");
											break;
										}
										else if ( d==(int)v_obstacleX.size()-1 )
										{
											// 大丈夫
											fprintf(fp,"1,");
										}
									}
								}
							}
							fprintf(fp,"\n");
						}
					}
				}
			}
		}
	}
	fclose(fp);



	// 解放
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				for ( int l=0;l<19;l++ ) 
				{
					delete [] LeftPose_p[i][j][k][l];
					delete [] RightPose_p[i][j][k][l];
				}
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			for ( int k=0;k<17;k++ ) 
			{
				delete [] LeftPose_p[i][j][k];
				delete [] RightPose_p[i][j][k];
			}
		}
	}
	for ( int i=0;i<6;i++ )
	{
		for ( int j=0;j<13;j++ ) 
		{
			delete [] LeftPose_p[i][j];
			delete [] RightPose_p[i][j];
		}
	}
	for ( int i=0;i<6;i++ )
	{
		delete [] LeftPose_p[i];
		delete [] RightPose_p[i];
	}

	printf("4つ目(4/4)のデータベース作成成功\n");

	return 0;
}
void CArmSimulatorDlg::OnBnClickedButton38()
{
	// TODO: Moveボタン(双腕)
	AfxBeginThread(MoveDualArm,this);
}

UINT __cdecl MoveDualArm(LPVOID pParam)
{
	CArmSimulatorDlg *p = (CArmSimulatorDlg*) pParam;

	p->m_JointAngleL.clear();
	p->m_JointAngleR.clear();

	char buf[256];
	JointAngle tmpJointAngleL;
	JointAngle tmpJointAngleR;
	FILE *fp = fopen(p->m_DualArmPassPath,"r");
	if ( fp == NULL ) return 0;

	while ( fgets(buf,256,fp) )
	{
		sscanf(buf,"Left:%f,%f,%f,%f,%f,%f,Right:%f,%f,%f,%f,%f,%f",&tmpJointAngleL.joint[0],&tmpJointAngleL.joint[1],&tmpJointAngleL.joint[2],
			                                                         &tmpJointAngleL.joint[3],&tmpJointAngleL.joint[4],&tmpJointAngleL.joint[5],
																	 &tmpJointAngleR.joint[0],&tmpJointAngleR.joint[1],&tmpJointAngleR.joint[2],
																	 &tmpJointAngleR.joint[3],&tmpJointAngleR.joint[4],&tmpJointAngleR.joint[5]  );
		p->m_JointAngleL.push_back(tmpJointAngleL);
		p->m_JointAngleR.push_back(tmpJointAngleR);
	}
	fclose(fp);

	int speed = 10;
	if (  p->m_Check_slow.GetCheck() ) speed = 500;


	for ( int i=0;i<(int)p->m_JointAngleL.size();i++)
	{
		p->m_ArmSim.m_isBalanced  = false;
		p->m_ArmSim.m_isCollision = false;

		for ( int j=0;j<6;j++ )
		{
			p->m_ArmSim.m_TargetAngleL[j] = (double)p->m_JointAngleL[i].joint[j];
			printf("m_ArmSim.m_TargetAngleL[%d] = %lf\n",j,p->m_ArmSim.m_TargetAngleL[j]*180/M_PI);
			p->m_ArmSim.m_TargetAngleR[j] = (double)p->m_JointAngleR[i].joint[j];
			printf("m_ArmSim.m_TargetAngleR[%d] = %lf\n",j,p->m_ArmSim.m_TargetAngleR[j]*180/M_PI);
		}
		printf("\n");

		while ( !p->m_ArmSim.m_isBalanced ) {Sleep(10);} // 平衡待ち
		Sleep(speed);
	}

	return 0;
}

void CArmSimulatorDlg::OnBnClickedButton37()
{
	// TODO: 参照ボタン
	CFileDialog readDLG(TRUE,NULL,NULL,OFN_HIDEREADONLY|OFN_NOCHANGEDIR, "(*.txt)|*.txt|");
	readDLG.m_ofn.lpstrTitle = "双腕Passのファイルを選択してください";
	if( readDLG.DoModal() == IDOK )
	{
		//ファイルパスの生成
		m_DualArmPassPath = readDLG.GetPathName();
		m_edit_PassPath.SetWindowTextA(m_DualArmPassPath);
	}else
	{
	}
}
