#include "Salamander.h"
#include "Joint.h"

namespace Salamander
{

Joint::Joint()
{
	body1_ = 0;
	body2_ = 0;
}

Joint::Joint(Body* body1, Body* body2, Vec2& anchor, double bias, double softness)
{
	body1_ = body1;
	body2_ = body2;

	Mat2 bodyRotation1(body1_->getRotation());
	Mat2 bodyRotation2(body2_->getRotation());
	bodyRotation1 = bodyRotation1.transpose();
	bodyRotation2 = bodyRotation2.transpose();

	localAnchor1_ = bodyRotation1 * (anchor - body1_->getTranslation());
	localAnchor2_ = bodyRotation2 * (anchor - body2_->getTranslation());

	accImpulse_ = Vec2(0.0f, 0.0f);

	biasFactor_ = bias;
	softness_ = softness;
}

Body* Joint::getBody(unsigned int i)
{
	if(i < 0 || i > 1) return NULL;

	return (i == 0 ? body1_ : body2_);
	
}

bool Joint::setBody(unsigned int i, Body* body)
{
	if(i < 0 || i > 1) return false;

	i == 0 ? body1_ = body : 
		     body2_ = body;

	return true;
}

void Joint::performPrecomputation(double inv_dt)
{
	/*
	// these two values are the distances moved due to rotation
	rotation1_ = Mat2(body1_->getRotation()) * localAnchor1_;
	rotation2_ = Mat2(body2_->getRotation()) * localAnchor2_;

	// calculate the change in velocity for the two objects, and then the change in rotations
	Mat2 K1;
	K1.a_ = body1_->inverseMass_ + body2_->inverseMass_;	
	K1.b_ = 0.0f;
	K1.c_ = 0.0f;												
	K1.d_ = body1_->inverseMass_ + body2_->inverseMass_;

	Mat2 K2;
	K2.a_ =  body1_->inverseMassMomentOfInertia_ * rotation1_.y_ * rotation1_.y_;		
	K2.b_ = -body1_->inverseMassMomentOfInertia_ * rotation1_.x_ * rotation1_.y_;
	K2.c_ = -body1_->inverseMassMomentOfInertia_ * rotation1_.x_ * rotation1_.y_;		
	K2.d_ =  body1_->inverseMassMomentOfInertia_ * rotation1_.x_ * rotation1_.x_;

	Mat2 K3;
	K3.a_ =  body2_->inverseMassMomentOfInertia_ * rotation2_.y_ * rotation2_.y_;		
	K3.b_ = -body2_->inverseMassMomentOfInertia_ * rotation2_.x_ * rotation2_.y_;
	K3.c_ = -body2_->inverseMassMomentOfInertia_ * rotation2_.x_ * rotation2_.y_;		
	K3.d_ =  body2_->inverseMassMomentOfInertia_ * rotation2_.x_ * rotation2_.x_;

	Mat2 K = K1 + K2 + K3;
	K.a_ += softness_;
	K.c_ += softness_;

	transform_ = K.Inverse();

	// find the difference in locations of the two bodies after they have been rotated around the joint
	Vec2 translation1 = body1_->getTranslation() + rotation1_;
	Vec2 translation2 = body2_->getTranslation() + rotation2_;
	Vec2 translationDifference = translation2 - translation1;

	bias_ = translationDifference * inv_dt * -biasFactor_;
	
	// Apply accumulated impulse.
	body1_->velocity_ -= accImpulse_ * body1_->inverseMass_;
	body1_->angularVelocity_ -= body1_->inverseMassMomentOfInertia_ * (rotation1_ ^ accImpulse_);

	body2_->velocity_ += accImpulse_ * body2_->inverseMass_;
	body2_->angularVelocity_ += body2_->inverseMassMomentOfInertia_ * (rotation2_ ^ accImpulse_);*/

}

void Joint::performIteration()
{
	// change in velocity
	/*
    Vec2 dv = body2_->velocity_ + (rotation2_ ^ -body2_->angularVelocity_) - 
			  body1_->velocity_ - (rotation1_ ^ -body1_->angularVelocity_);

	Vec2 impulse = transform_ * (bias_ - dv - (accImpulse_ * softness_));

	body1_->velocity_ -= impulse * body1_->inverseMass_;
	body1_->angularVelocity_ -= body1_->inverseMassMomentOfInertia_ * (rotation1_ ^ impulse);

	body2_->velocity_ += impulse * body2_->inverseMass_;
	body2_->angularVelocity_ += body2_->inverseMassMomentOfInertia_ * (rotation2_ ^ impulse);

	accImpulse_ += impulse;*/
}

}