/*
 *  Robot.cpp
 *  Martial
 *
 *  Created by ryoma on 10/01/23.
 *  Copyright 2010 琉球大学. All rights reserved.
 *
 */

#include "Robot.h"

Robot::Robot(char* _name) : Character::Character(_name) {
	name = _name;
	moveCount = 0.0;
	moveFlag = 0;

	reloadFunc = &Robot::reload;

	RobotNode = new osg::Group();
	body = osgDB::readNodeFile("robot_body.osg");

	leftArm = osgDB::readNodeFile("robot_l_arm.osg");
	rightArm = osgDB::readNodeFile("robot_r_arm.osg");
	leftLeg = osgDB::readNodeFile("robot_l_leg.osg");
	rightLeg = osgDB::readNodeFile("robot_r_leg.osg");

//	SearchMatrixTransformVisitor* visitor = new SearchMatrixTransformVisitor("Mesh133004__Black_");
//	body->accept(*visitor);
//	hoge = visitor->getMatrixTransform();

	if( hoge != NULL ) {
		std::cout << "Success!! Matrix name is [" << hoge->getName() << "]\n";
	} else {
		puts("False......");
	}

	bodyForm = new osg::PositionAttitudeTransform();	
	leftArmForm = new osg::PositionAttitudeTransform();
	rightArmForm = new osg::PositionAttitudeTransform();
	leftLegForm = new osg::PositionAttitudeTransform();
	rightLegForm = new osg::PositionAttitudeTransform();

	//ロボットの組み立て

	bodyForm->addChild(body);
	leftArmForm->addChild(leftArm);
	rightArmForm->addChild(rightArm);
	leftLegForm->addChild(leftLeg);
	rightLegForm->addChild(rightLeg);

	mform->addChild(bodyForm);
	bodyForm->addChild(leftArmForm);
	bodyForm->addChild(rightArmForm);
	bodyForm->addChild(leftLegForm);
	bodyForm->addChild(rightLegForm);

	//微調整。 ここはモデリングツールでやろうよ。。まじで。。

	bodyForm->setPosition(osg::Vec3(0.0,0.0,1.5));
	leftArmForm->setPosition(osg::Vec3(0.5,-0.3,0.2));
	rightArmForm->setPosition(osg::Vec3(-0.5,-0.3,0.2));
	leftLegForm->setPosition(osg::Vec3(0.2,0,-1.2));
	rightLegForm->setPosition(osg::Vec3(-0.2,0,-1.2));

	//衝突判定ノードの追加

	CollisionNode* collisionNode = new CollisionNode("Body-Node", new osg::Cylinder(osg::Vec3(0, 0, 0), 0.4, 2.5));
	bodyForm->addChild(collisionNode->getNode());
	collisionNodeList.push_back(collisionNode);

	collisionNode = new CollisionNode("Left-Arm-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 0.3));
	leftArmForm->addChild(collisionNode->getNode());
	collisionNodeList.push_back(collisionNode);

	collisionNode = new CollisionNode("Right-Arm-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 0.3));
	rightArmForm->addChild(collisionNode->getNode());
	collisionNodeList.push_back(collisionNode);
	
	collisionNode = new CollisionNode("Left-Leg-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 0.3));
	leftLegForm->addChild(collisionNode->getNode());
	collisionNodeList.push_back(collisionNode);

	collisionNode = new CollisionNode("Right-Leg-Node", new osg::Sphere(osg::Vec3(0, 0, 0), 0.3));
	rightLegForm->addChild(collisionNode->getNode());
	collisionNodeList.push_back(collisionNode);	

	reload();
}

void Robot::pushLeft() {
	printf("Class Robott [%s] :: PUSH-LEFT\n", name);
	moveFlag = 1;
	setVel(osg::Vec3(1.0,0.0,0.0));
}

void Robot::releaseLeft() {
	printf("Class Robott [%s] :: RELEASE-LEFT\n", name);
	moveFlag = 0;
	setVel(osg::Vec3(0,0.0,0.0));
}

void Robot::pushRight() {
	printf("Class Robott [%s] :: PUSH-RIGHT\n", name);
	moveFlag = 1;
	setVel(osg::Vec3(-1.0,0.0,0.0));
}

void Robot::releaseRight() {
	printf("Class Robott [%s] :: RELASE-RIGHT\n", name);
	moveFlag = 0;
	setVel(osg::Vec3(0,0.0,0.0));
}

void Robot::pushUp() {
	printf("Class Robott [%s] :: PUSH-UP\n", name);
}

void Robot::releaseUp() {
	printf("Class Robott [%s] :: RELEASE-UP\n", name);
}

void Robot::pushDown() {
	printf("Class Robott [%s] :: PUSH-DOWN\n", name);
}

void Robot::releaseDown() {
	printf("Class Robott [%s] :: RELEASE-DOWN\n", name);
}



void Robot::pushA() {
	printf("Class Robott [%s] :: PUSH-A\n", name);
	moveFlag = 1;
	setVel(osg::Vec3(1.0,0.0,0.0));
}

void Robot::releaseA() {
	printf("Class Robott [%s] :: RELEASE-A\n", name);
	moveFlag = 0;
	setVel(osg::Vec3(0,0.0,0.0));
}

void Robot::pushB() {
	printf("Class Robott [%s] :: PUSH-B\n", name);
	turn();
}

void Robot::releaseB() {
	printf("Class Robott [%s] :: RELASE-B\n", name);
	moveFlag = 0;
	setVel(osg::Vec3(0,0.0,0.0));
}

void Robot::pushC() {
	printf("Class Robott [%s] :: PUSH-C\n", name);
}

void Robot::releaseC() {
	printf("Class Robott [%s] :: RELEASE-C\n", name);
}

void Robot::pushD() {
	printf("Class Robott [%s] :: PUSH-D\n", name);
}

void Robot::releaseD() {
	printf("Class Robott [%s] :: RELEASE-D\n", name);
}

void Robot::walk() {
	printf("walk");
	moveFlag = 1;
	setVel(osg::Vec3(-5.0,0.0,0.0));
}

void Robot::turn() {
	angularVelocity = osg::Vec3(angularVelocity.x(), angularVelocity.y(), 0.1);
}

void Robot::stop() {
	velocity = osg::Vec3(0,0,0);
	angularVelocity = osg::Vec3(0,0,0);
}


void Robot::reload() {
	moveCount += (float)moveFlag * 0.5;
	float sinVal = sin(moveCount);
	float cosVal = cos(moveCount);

	if (moveFlag != 0) setPos(osg::Vec3(position.x(), position.y(), position.z()+cos(moveCount)*0.01));
	
	bodyForm->setAttitude(osg::Quat(osg::DegreesToRadians(sinVal*10.0), osg::Vec3(0,0,1)));
	leftArmForm->setAttitude(osg::Quat(osg::DegreesToRadians(sinVal*30.0+10.0), osg::Vec3(1,0,0)));
	rightArmForm->setAttitude(osg::Quat(osg::DegreesToRadians(-sinVal*30.0+10.0), osg::Vec3(1,0,0)));
	leftLegForm->setAttitude(osg::Quat(osg::DegreesToRadians(-sinVal*45.0), osg::Vec3(1,0,0)));
	rightLegForm->setAttitude(osg::Quat(osg::DegreesToRadians(sinVal*45.0), osg::Vec3(1,0,0)));

	//位置の再計算は親クラスのreloadと一緒なのでまかせたい..けどなぜかできない
	// Character::reload(); (MovableObject::reload())
	osg::Matrixd roteMatrix;
	osg::Matrixd dirMatrix;
	osg::Matrixd accMatrix;

	direction += angularVelocity;
	accMatrix.makeTranslate(acceleration);
	roteMatrix.makeRotate(
		osg::Quat(
			direction.x(), osg::Vec3(1,0,0),
			direction.y(), osg::Vec3(0,1,0),
			direction.z(), osg::Vec3(0,0,1)
		)
	);

	velocity += (accMatrix*roteMatrix).getTrans();
	position += velocity;

	accMatrix.makeTranslate(acceleration);
	roteMatrix.makeRotate(
		osg::Quat(
			defaultDirection.x()+direction.x(), osg::Vec3(1,0,0),
			defaultDirection.y()+direction.y(), osg::Vec3(0,1,0),
			defaultDirection.z()+direction.z(), osg::Vec3(0,0,1)
		)
	);

	dirMatrix.makeTranslate(position);
	accMatrix.makeScale(scale);
	mform->setMatrix(accMatrix*roteMatrix*dirMatrix);
}