
#include "dojo/StickFigure.h"

namespace dojo {

    StickFigure::StickFigure(const std::string& name) : Entity(name, name + std::string("Pelvis")) {//, Entity::FIXED, Entity::STILL) {
    
    SuperShader::Material material;

	if (name == "PLAYER0") {
		material.diffuse = Color3::red() * .8f;
		material.transmit = Color3::black();
		material.reflect = Color3::black();
		material.specular = Color3::white() * .4f;
		material.specularExponent = Color3::white() * 30;
	} else {
		material.diffuse = Color3::blue() * .8f;
		material.transmit = Color3::black();
		material.reflect = Color3::black();
		material.specular = Color3::white() * .4f;
		material.specularExponent = Color3::white() * 30;
	}
    Texture::fromFile("stickfigure/texture.png");

    float density = 1;

#define LOAD_MODEL(name)\
    ArticulatedModelRef name = ArticulatedModel::fromFile("stickfigure/" #name ".ifs", .5f);\
    name->partArray.last().triListArray.last().material = material;\

    static bool initialized = false;

    LOAD_MODEL(pelvis);
    LOAD_MODEL(chest);
    LOAD_MODEL(head);

    LOAD_MODEL(lhumerus);
    LOAD_MODEL(lradius);
    LOAD_MODEL(lhand);
    LOAD_MODEL(lfingers);
    LOAD_MODEL(lthumb);
    LOAD_MODEL(lfemur);
    LOAD_MODEL(ltibia);
    LOAD_MODEL(lfoot);

    LOAD_MODEL(rhumerus);
    LOAD_MODEL(rradius);
    LOAD_MODEL(rhand);
    LOAD_MODEL(rfingers);
    LOAD_MODEL(rthumb);
    LOAD_MODEL(rfemur);
    LOAD_MODEL(rtibia);
    LOAD_MODEL(rfoot);

    //initialized = true;

#   undef LOAD_MODEL

	addGraphicsGeometry(name + std::string("Pelvis"), pelvis);
    addPhysicsGeometry(name + std::string("Pelvis"), new CapsuleShape(Capsule(Vector3(0, 0.08f, 0), Vector3(0.0f, .24f, 0), 0.08f)), density);

    addPart(name + std::string("Chest"), name + std::string("Pelvis"), Vector3(0, 0.325f, 0));
    addGraphicsGeometry(name + std::string("Chest"), chest);
    addPhysicsGeometry(name + std::string("Chest"), new BoxShape(Box(Vector3(-.16f, 0.015f, -.09f), Vector3(.16f, .2f, .09f))), density);
    
    addBallJoint(name + std::string("Waist"), name + std::string("Chest"), name + std::string("Pelvis"), Vector3(0, -0.025f, 0), 0, 0.07f);

    addPart(name + std::string("Head"), name + std::string("Chest"), Vector3(0, 0.225f, 0));
    addGraphicsGeometry(name + std::string("Head"), head);
    addPhysicsGeometry(name + std::string("Head"), new CapsuleShape(Capsule(Vector3(0, .071f, 0), Vector3(0.0f, .17f, 0), 0.07f)), density);

    addBallJoint(name + std::string("Neck"), name + std::string("Head"), name + std::string("Chest"), Vector3(0, 0.035f, 0), 0, 0.07f);

    addPart(name + std::string("Left Humerus"), name + std::string("Chest"), Vector3(-0.25f, 0.15f, 0.0));
    addGraphicsGeometry(name + std::string("Left Humerus"), lhumerus);
    addPhysicsGeometry(name + std::string("Left Humerus"), 
        new CapsuleShape(Capsule(Vector3(0.02f, 0, 0), Vector3(-0.15f, 0, 0), 0.04f)), density);

    addUniversalJoint(name + std::string("Left Shoulder"), name + std::string("Left Humerus"), name + std::string("Chest"), Vector3::unitZ(), 
        Vector3::unitX(), Vector3(0, 0, 0), 0, 0.05f);

    addPart(name + std::string("Left Radius"), name + std::string("Left Humerus"), Vector3(-0.275f, 0.0, 0.0));
    
    addGraphicsGeometry(name + std::string("Left Radius"), lradius);
    addPhysicsGeometry(name + std::string("Left Radius"), 
        new CapsuleShape(Capsule(Vector3(0.02f, 0.01f, 0), Vector3(-0.18f, 0.01f, 0), 0.06f)), density);

    addHinge(name + std::string("Left Elbow"), name + std::string("Left Radius"), name + std::string("Left Humerus"), Vector3::unitY(), Vector3(-0.01f, 0, 0), 0, (float)G3D_PI/2, 0, 0.1f);

    addPart(name + std::string("Left Hand"), name + std::string("Left Radius"), Vector3(-0.225f, 0.0f, 0.0f));
    addGraphicsGeometry(name + std::string("Left Hand"), lhand);
    addPhysicsGeometry(name + std::string("Left Hand"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(Vector3::unitZ(), toRadians(-48))).toWorldSpace(
        Box(Vector3(-.11f, -0.04f, -.05f), Vector3(-0.02f, -0.02f, .05f)))), density);

    addHinge(name + std::string("Left Wrist"), name + std::string("Left Hand"), name + std::string("Left Radius"), Vector3::unitZ(), Vector3(-0.02f, 0, 0), -(float)G3D_HALF_PI/2, (float)G3D_HALF_PI/2, 0, 0.07f);

    addPart(name + std::string("Left Fingers"), name + std::string("Left Hand"), Vector3(-0.197f/2, 0.109f/2, 0.0f));
    addGraphicsGeometry(name + std::string("Left Fingers"), lfingers);
    addPhysicsGeometry(name + std::string("Left Fingers"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(Vector3::unitZ(), toRadians(45)), Vector3(0,.01f,0)).toWorldSpace(
        Box(Vector3(-.08f, -.01f, -.05f), Vector3(0, 0.01f, .05f)))), density);
    addHinge(name + std::string("Left Knuckles"), name + std::string("Left Fingers"), name + std::string("Left Hand"), Vector3::unitZ(), Vector3(0, 0, 0), -toRadians(10), toRadians(10), 0, 0.07f);

    addPart(name + std::string("Left Thumb"), name + std::string("Left Hand"), Vector3(-.05f, 0.005f, 0.0f));
    addGraphicsGeometry(name + std::string("Left Thumb"), lthumb);
    addPhysicsGeometry(name + std::string("Left Thumb"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(Vector3::unitZ(), toRadians(45)), Vector3(0,-.015f,0)).toWorldSpace(
        Box(Vector3(-.06f, -.01f, -.05f), Vector3(0, 0.01f, .05f)))), density);
    addHinge(name + std::string("Left Thumb Joint"), name + std::string("Left Thumb"), name + std::string("Left Hand"), Vector3::unitZ(), Vector3(0, 0, 0), -toRadians(10), toRadians(10), 0, 0.07f);

    addPart(name + std::string("Left Femur"), name + std::string("Pelvis"), Vector3(-0.2f, 0.14f, 0.0f) / 2);
    addGraphicsGeometry(name + std::string("Left Femur"), lfemur);
    addPhysicsGeometry(name + std::string("Left Femur"), 
        new CapsuleShape(Capsule(Vector3(-0.01f, -0.01f, 0), Vector3(-0.01f, -.245f, 0), 0.055f)), density);
    
    addUniversalJoint(name + std::string("Left Hip"), name + std::string("Left Femur"), name + std::string("Pelvis"), Vector3::unitZ(), Vector3::unitX(), Vector3(0, 0.01f, 0), 0, 0.07f);

    addPart(name + std::string("Left Tibia"), name + std::string("Left Femur"), Vector3(0.0f, -0.35f, 0.0f));
    addGraphicsGeometry(name + std::string("Left Tibia"), ltibia);
    addPhysicsGeometry(name + std::string("Left Tibia"), 
        new CapsuleShape(Capsule(Vector3(-0.02f, -0.02f, 0), Vector3(-0.02f, -.19f, 0), 0.07f)), density);

    addHinge(name + std::string("Left Knee"), name + std::string("Left Tibia"), name + std::string("Left Femur"), Vector3::unitX(), Vector3(0, 0, 0), (float)-G3D_PI, 0, 0, 0.07f);

    addPart(name + std::string("Left Foot"), name + std::string("Left Tibia"), Vector3(-0.05f, -0.225f, 0.0f));
    addGraphicsGeometry(name + std::string("Left Foot"), lfoot);
    addPhysicsGeometry(name + std::string("Left Foot"),
        new BoxShape(Box(Vector3(-.11f, -.08f, -0.22f), Vector3(.06f, 0.04f, .05f))), density);

    addHinge(name + std::string("Left Ankle"), name + std::string("Left Foot"), name + std::string("Left Tibia"), Vector3::unitX(), Vector3(0, 0, 0), (float)-G3D_PI/2, 0, 0, 0.07f);


    addPart(name + std::string("Right Humerus"), name + std::string("Chest"), Vector3(0.25f, 0.15f, 0.0f));
    addGraphicsGeometry(name + std::string("Right Humerus"), rhumerus);
    addPhysicsGeometry(name + std::string("Right Humerus"), 
        new CapsuleShape(Capsule(Vector3(-0.02f, 0, 0), Vector3(0.15f, 0, 0), 0.04f)), density);

    addUniversalJoint(name + std::string("Right Shoulder"), name + std::string("Right Humerus"), name + std::string("Chest"), Vector3::unitZ(), 
        Vector3::unitX(), Vector3(0, 0, 0), 0, 0.05f);

    addPart(name + std::string("Right Radius"), name + std::string("Right Humerus"), Vector3(0.275f, 0.0f, 0.0f));
    
    addGraphicsGeometry(name + std::string("Right Radius"), rradius);
    addPhysicsGeometry(name + std::string("Right Radius"), 
        new CapsuleShape(Capsule(Vector3(-0.02f, .01f, 0), Vector3(0.18f, 0.01f, 0), 0.06f)), density);

    addHinge(name + std::string("Right Elbow"), name + std::string("Right Radius"), name + std::string("Right Humerus"), Vector3::unitY(), Vector3(0.01f, 0, 0), 0, (float)G3D_PI/2, 0, 0.1f);

    addPart(name + std::string("Right Hand"), name + std::string("Right Radius"), Vector3(0.225f, 0.0f, 0.0f));
    addGraphicsGeometry(name + std::string("Right Hand"), rhand);
    addPhysicsGeometry(name + std::string("Right Hand"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(-Vector3::unitZ(), toRadians(-48))).toWorldSpace(
        Box(Vector3(.11f, -0.04f, -.05f), Vector3(0.02f, -0.02f, .05f)))), density);

    addHinge(name + std::string("Right Wrist"), name + std::string("Right Hand"), name + std::string("Right Radius"), Vector3::unitZ(), Vector3(0.02f, 0, 0), (float)-G3D_HALF_PI/2, (float)G3D_HALF_PI/2, 0, 0.07f);

    addPart(name + std::string("Right Fingers"), name + std::string("Right Hand"), Vector3(0.197f/2, 0.109f/2, 0.0f));
    addGraphicsGeometry(name + std::string("Right Fingers"), rfingers);
    addPhysicsGeometry(name + std::string("Right Fingers"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(-Vector3::unitZ(), toRadians(45)), Vector3(0,.01f,0)).toWorldSpace(
        Box(Vector3(.08f, -.01f, -.05f), Vector3(0, 0.01f, .05f)))), density);
    addHinge(name + std::string("Right Knuckles"), name + std::string("Right Fingers"), name + std::string("Right Hand"), Vector3::unitZ(), Vector3(0, 0, 0), -toRadians(10), toRadians(10), 0, 0.07f);

    addPart(name + std::string("Right Thumb"), name + std::string("Right Hand"), Vector3(.05f, 0.005f, 0.0f));
    addGraphicsGeometry(name + std::string("Right Thumb"), rthumb);
    addPhysicsGeometry(name + std::string("Right Thumb"), 
        new BoxShape(
        CoordinateFrame(Matrix3::fromAxisAngle(-Vector3::unitZ(), toRadians(45)), Vector3(0,-.015f,0)).toWorldSpace(
        Box(Vector3(.06f, -.01f, -.05f), Vector3(0, 0.01f, .05f)))), density);
    addHinge(name + std::string("Right Thumb Joint"), name + std::string("Right Thumb"), name + std::string("Right Hand"), Vector3::unitZ(), Vector3(0, 0, 0), -toRadians(10), toRadians(10), 0, 0.07f);

    addPart(name + std::string("Right Femur"), name + std::string("Pelvis"), Vector3(0.2f, 0.14f, 0.0f) / 2);
    addGraphicsGeometry(name + std::string("Right Femur"), rfemur);
    addPhysicsGeometry(name + std::string("Right Femur"), 
        new CapsuleShape(Capsule(Vector3(0.01f, -0.01f, 0), Vector3(0.01f, -.245f, 0), 0.055f)), density);
    
    addUniversalJoint(name + std::string("Right Hip"), name + std::string("Right Femur"), name + std::string("Pelvis"), Vector3::unitZ(), Vector3::unitX(), Vector3(0, 0.01f, 0), 0, 0.07f);

    addPart(name + std::string("Right Tibia"), name + std::string("Right Femur"), Vector3(0.0f, -0.35f, 0.0f));
    addGraphicsGeometry(name + std::string("Right Tibia"), rtibia);
    addPhysicsGeometry(name + std::string("Right Tibia"), 
        new CapsuleShape(Capsule(Vector3(0.02f, -0.02f, 0), Vector3(0.02f, -.19f, 0), 0.07f)), density);

    addHinge(name + std::string("Right Knee"), name + std::string("Right Tibia"), name + std::string("Right Femur"), Vector3::unitX(), Vector3(0, 0, 0), (float)-G3D_PI, 0, 0, 0.07f);

    addPart(name + std::string("Right Foot"), name + std::string("Right Tibia"), Vector3(0.05f, -0.225f, 0.0f));
    addGraphicsGeometry(name + std::string("Right Foot"), rfoot);
    addPhysicsGeometry(name + std::string("Right Foot"),
        new BoxShape(Box(Vector3(.11f, -.08f, -0.22f), Vector3(-.06f, 0.04f, .05f))), density);

    addHinge(name + std::string("Right Ankle"), name + std::string("Right Foot"), name + std::string("Right Tibia"), Vector3::unitX(), Vector3(0, 0, 0), -(float)G3D_PI/2, 0, 0, 0.07f);
}

} // namespace

