#include <iostream>
#include <fstream>

//許容重心位置誤差
#define eps (0.001)

using namespace std;

//π/180
const float rad = 0.017453f;

//パラメータ定義
//ピッチ間距離
#define pitch_length  (100.0f)
//一番上のピッチ軸から重心位置の高さ
#define pitch_offset  (8.106f)
//足首ピッチから足裏まで
#define pitch_offset2 (34.0f)
//膝ピッチ間（シングルの場合は0)
#define hiza_offset	  (26.0f)
//ももロール中心から重心位置
#define role_offset	  (37.5f)
////ももロール中心からホームポジション重心位置
#define role_home	  (56.146f)

//初期位置
float pitch = 20.0f, role = 5.0f;
float inv_jacobi[2][2], f[2];

//重心の高さを求める関数
void set_function(void)
{
	f[0] = (2*pitch_length*cos(rad*pitch)+hiza_offset)*cos(rad*role)+pitch_offset+pitch_offset2;
	f[1] = (2*pitch_length*cos(rad*pitch)+hiza_offset)*sin(rad*role)+role_offset-role_home;
}

void set_inv_jacobi(void)
{
	inv_jacobi[0][0] = -cos(rad*role)/(200*sin(rad*pitch));  	  inv_jacobi[0][1] = -sin(rad*role)/(200*sin(rad*pitch));
	inv_jacobi[1][0] = -sin(rad*role)/(200*cos(rad*pitch) + 26);  inv_jacobi[1][1] = cos(rad*role)/(200*cos(rad*pitch) + 26);
}

//目標重心位置になる角度を求める(目標重心高さ, 目標重心位置(左右))
void calculate(float target_z, float target_y)
{
	set_function();
	do{
		set_inv_jacobi();
		pitch -= (inv_jacobi[0][0]*(f[0]-target_z)+inv_jacobi[0][1]*(f[1]-target_y));
		role  -= (inv_jacobi[1][0]*(f[0]-target_z)+inv_jacobi[1][1]*(f[1]-target_y));
		set_function();
	}while(fabs(f[0] - target_z) >= eps || fabs(f[1] - target_y) >= eps);
}


int main(void)
{
	float y;
	unsigned char ch;
	fstream file1("kine.csv",  ios::in | ios::out);
	fstream file2("kine2.csv", ios::out);
	
	while(!file1.eof())
	{
		file1 >> y >> ch;
		file1 >> y;
		cout << y << endl;
		calculate(255.231,y);
		file2 << pitch << "," << role << endl;
		cout << pitch << '-' << role << endl;
	}
}