/*
 * JBox2D - A Java Port of Erin Catto's Box2D
 * 
 * JBox2D homepage: http://jbox2d.sourceforge.net/ 
 * Box2D homepage: http://www.box2d.org
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.jbox2d.dynamics.joints;

import org.jbox2d.common.Mat22;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.TimeStep;
import org.jbox2d.dynamics.World;


//Updated to rev. 56->108 of b2RevoluteJoint.cpp/.h

//Point-to-point constraint
//C = p2 - p1
//Cdot = v2 - v1
//   = v2 + cross(w2, r2) - v1 - cross(w1, r1)
//J = [-I -r1_skew I r2_skew ]
//Identity used:
//w k % (rx i + ry j) = w * (-ry i + rx j)

//Motor constraint
//Cdot = w2 - w1
//J = [0 0 -1 0 0 1]
//K = invI1 + invI2

public class RevoluteJoint extends Joint {
	public Vec2 m_localAnchor1;	// relative
	public Vec2 m_localAnchor2;
	public Vec2 m_pivotForce;
	public float m_motorForce;
	public float m_limitForce;
	public float m_limitPositionImpulse;

	public Mat22 m_pivotMass;		// effective mass for point-to-point constraint.
	public float m_motorMass;	// effective mass for motor/limit angular constraint.
	
	public boolean m_enableMotor;
	public float m_maxMotorTorque;
	public float m_motorSpeed;

	public boolean m_enableLimit;
	public float m_referenceAngle;
	public float m_lowerAngle;
	public float m_upperAngle;
	public int m_limitState;

    public RevoluteJoint(RevoluteJointDef def) {
        super(def);
        m_localAnchor1 = def.localAnchor1.clone();
    	m_localAnchor2 = def.localAnchor2.clone();
    	m_referenceAngle = def.referenceAngle;

    	m_pivotForce = new Vec2(0.0f, 0.0f);
    	m_motorForce = 0.0f;
    	m_limitForce = 0.0f;
    	m_limitPositionImpulse = 0.0f;
    	m_pivotMass = new Mat22();

    	m_lowerAngle = def.lowerAngle;
    	m_upperAngle = def.upperAngle;
    	m_maxMotorTorque = def.maxMotorTorque;
    	m_motorSpeed = def.motorSpeed;
    	m_enableLimit = def.enableLimit;
    	m_enableMotor = def.enableMotor;
    }

    //@Override
    public void initVelocityConstraints(TimeStep step) {
    	Body b1 = m_body1;
    	Body b2 = m_body2;

    	// Compute the effective mass matrix.
    	Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
    	Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

    	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
    	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
    	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
    	float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
    	float invI1 = b1.m_invI, invI2 = b2.m_invI;

    	Mat22 K1 = new Mat22();
    	K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
    	K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;

    	Mat22 K2 = new Mat22();
    	K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
    	K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;

    	Mat22 K3 = new Mat22();
    	K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
    	K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;

    	Mat22 K = K1.addLocal(K2).addLocal(K3);
    	m_pivotMass = K.invert();

    	m_motorMass = 1.0f / (invI1 + invI2);

    	if (m_enableMotor == false) {
    		m_motorForce = 0.0f;
    	}

    	if (m_enableLimit) {
    		float jointAngle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
    		if (Math.abs(m_upperAngle - m_lowerAngle) < 2.0f * Settings.angularSlop) {
    			m_limitState = LimitState.EQUAL_LIMITS;
    		} else if (jointAngle <= m_lowerAngle) {
    			if (m_limitState != LimitState.AT_LOWER_LIMIT) {
    				m_limitForce = 0.0f;
    			}
    			m_limitState = LimitState.AT_LOWER_LIMIT;
    		} else if (jointAngle >= m_upperAngle) {
    			if (m_limitState != LimitState.AT_UPPER_LIMIT) {
    				m_limitForce = 0.0f;
    			}
    			m_limitState = LimitState.AT_UPPER_LIMIT;
    		}else {
    			m_limitState = LimitState.INACTIVE_LIMIT;
    			m_limitForce = 0.0f;
    		}
    	} else {
    		m_limitForce = 0.0f;
    	}

    	if (step.warmStarting) {
    		b1.m_linearVelocity.x -= step.dt * invMass1 * m_pivotForce.x;
    		b1.m_linearVelocity.y -= step.dt * invMass1 * m_pivotForce.y;
    		b1.m_angularVelocity -= step.dt * invI1 * (Vec2.cross(r1, m_pivotForce) + m_motorForce + m_limitForce);

    		b2.m_linearVelocity.x += step.dt * invMass2 * m_pivotForce.x;
    		b2.m_linearVelocity.y += step.dt * invMass2 * m_pivotForce.y;
    		b2.m_angularVelocity += step.dt * invI2 * (Vec2.cross(r2, m_pivotForce) + m_motorForce + m_limitForce);
    	} else {
    		m_pivotForce.setZero();
    		m_motorForce = 0.0f;
    		m_limitForce = 0.0f;
    	}

    	m_limitPositionImpulse = 0.0f;
    }

    //@Override
    public void solveVelocityConstraints(TimeStep step) {
    	Body b1 = m_body1;
    	Body b2 = m_body2;

    	Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
    	Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

    	// Solve point-to-point constraint
    	Vec2 pivotCdot = b2.m_linearVelocity.add( Vec2.cross(b2.m_angularVelocity, r2).subLocal(b1.m_linearVelocity).subLocal(Vec2.cross(b1.m_angularVelocity, r1)));
    	Vec2 pivotForce = Mat22.mul(m_pivotMass, pivotCdot).mulLocal(-step.inv_dt);
    	m_pivotForce.addLocal(pivotForce);

    	Vec2 P = pivotForce.mul(step.dt);
    	b1.m_linearVelocity.x -= b1.m_invMass * P.x;
    	b1.m_linearVelocity.y -= b1.m_invMass * P.y;
    	b1.m_angularVelocity -= b1.m_invI * Vec2.cross(r1, P);

    	b2.m_linearVelocity.x += b2.m_invMass * P.x;
    	b2.m_linearVelocity.y += b2.m_invMass * P.y;
    	b2.m_angularVelocity += b2.m_invI * Vec2.cross(r2, P);

    	if (m_enableMotor && m_limitState != LimitState.EQUAL_LIMITS) {
    		float motorCdot = b2.m_angularVelocity - b1.m_angularVelocity - m_motorSpeed;
    		float motorForce = -step.inv_dt * m_motorMass * motorCdot;
    		float oldMotorForce = m_motorForce;
    		m_motorForce = MathUtils.clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
    		motorForce = m_motorForce - oldMotorForce;

    		float P2 = step.dt * motorForce;
    		b1.m_angularVelocity -= b1.m_invI * P2;
    		b2.m_angularVelocity += b2.m_invI * P2;
    	}

    	if (m_enableLimit && m_limitState != LimitState.INACTIVE_LIMIT) {
    		float limitCdot = b2.m_angularVelocity - b1.m_angularVelocity;
    		float limitForce = -step.inv_dt * m_motorMass * limitCdot;

    		if (m_limitState == LimitState.EQUAL_LIMITS) {
    			m_limitForce += limitForce;
    		} else if (m_limitState == LimitState.AT_LOWER_LIMIT) {
    			float oldLimitForce = m_limitForce;
    			m_limitForce = Math.max(m_limitForce + limitForce, 0.0f);
    			limitForce = m_limitForce - oldLimitForce;
    		} else if (m_limitState == LimitState.AT_UPPER_LIMIT) {
    			float oldLimitForce = m_limitForce;
    			m_limitForce = Math.min(m_limitForce + limitForce, 0.0f);
    			limitForce = m_limitForce - oldLimitForce;
    		}

    		float P2 = step.dt * limitForce;
    		b1.m_angularVelocity -= b1.m_invI * P2;
    		b2.m_angularVelocity += b2.m_invI * P2;
    	}
    }

    //@Override
    public boolean solvePositionConstraints() {
        Body b1 = m_body1;
        Body b2 = m_body2;
        
        float positionError = 0f;

        // Solve point-to-point position error.
        Vec2 r1 = Mat22.mul(b1.m_xf.R, m_localAnchor1.sub(b1.getLocalCenter()));
    	Vec2 r2 = Mat22.mul(b2.m_xf.R, m_localAnchor2.sub(b2.getLocalCenter()));

    	Vec2 p1 = b1.m_sweep.c.add(r1);
    	Vec2 p2 = b2.m_sweep.c.add(r2);
    	Vec2 ptpC = p2.sub(p1);

    	positionError = ptpC.length();

    	// Prevent overly large corrections.
    	//public b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
    	//ptpC = b2Clamp(ptpC, -dpMax, dpMax);

    	float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
    	float invI1 = b1.m_invI, invI2 = b2.m_invI;

        Mat22 K1 = new Mat22();
        K1.col1.x = invMass1 + invMass2;    K1.col2.x = 0.0f;
        K1.col1.y = 0.0f;                   K1.col2.y = invMass1 + invMass2;
        
        Mat22 K2 = new Mat22();
        K2.col1.x =  invI1 * r1.y * r1.y;   K2.col2.x = -invI1 * r1.x * r1.y;
        K2.col1.y = -invI1 * r1.x * r1.y;   K2.col2.y =  invI1 * r1.x * r1.x;

        Mat22 K3 = new Mat22();
        K3.col1.x =  invI2 * r2.y * r2.y;   K3.col2.x = -invI2 * r2.x * r2.y;
        K3.col1.y = -invI2 * r2.x * r2.y;   K3.col2.y =  invI2 * r2.x * r2.x;

        Mat22 K = K1.add(K2).add(K3);
        Vec2 impulse = K.solve(ptpC.negate());

    	b1.m_sweep.c.x -= b1.m_invMass * impulse.x;
    	b1.m_sweep.c.y -= b1.m_invMass * impulse.y;
    	b1.m_sweep.a -= b1.m_invI * Vec2.cross(r1, impulse);

    	b2.m_sweep.c.x += b2.m_invMass * impulse.x;
    	b2.m_sweep.c.y += b2.m_invMass * impulse.y;
    	b2.m_sweep.a += b2.m_invI * Vec2.cross(r2, impulse);

    	b1.synchronizeTransform();
    	b2.synchronizeTransform();

    	// Handle limits.
    	float angularError = 0.0f;


    	if (m_enableLimit && m_limitState != LimitState.INACTIVE_LIMIT) {
    		float angle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
    		float limitImpulse = 0.0f;

    		if (m_limitState == LimitState.EQUAL_LIMITS) {
    			// Prevent large angular corrections
    			float limitC = MathUtils.clamp(angle, -Settings.maxAngularCorrection, Settings.maxAngularCorrection);
    			limitImpulse = -m_motorMass * limitC;
    			angularError = Math.abs(limitC);
    		} else if (m_limitState == LimitState.AT_LOWER_LIMIT) {
    			float limitC = angle - m_lowerAngle;
    			angularError = Math.max(0.0f, -limitC);

    			// Prevent large angular corrections and allow some slop.
    			limitC = MathUtils.clamp(limitC + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f);
    			limitImpulse = -m_motorMass * limitC;
    			float oldLimitImpulse = m_limitPositionImpulse;
    			m_limitPositionImpulse = Math.max(m_limitPositionImpulse + limitImpulse, 0.0f);
    			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
    		} else if (m_limitState == LimitState.AT_UPPER_LIMIT) {
    			float limitC = angle - m_upperAngle;
    			angularError = Math.max(0.0f, limitC);

    			// Prevent large angular corrections and allow some slop.
    			limitC = MathUtils.clamp(limitC - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection);
    			limitImpulse = -m_motorMass * limitC;
    			float oldLimitImpulse = m_limitPositionImpulse;
    			m_limitPositionImpulse = Math.min(m_limitPositionImpulse + limitImpulse, 0.0f);
    			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
    		}

    		b1.m_sweep.a -= b1.m_invI * limitImpulse;
    		b2.m_sweep.a += b2.m_invI * limitImpulse;

    		b1.synchronizeTransform();
    		b2.synchronizeTransform();
    	}

    	return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop;
    }

    public Vec2 getAnchor1() {
    	return m_body1.getWorldPoint(m_localAnchor1);
    }

    public Vec2 getAnchor2() {
    	return m_body2.getWorldPoint(m_localAnchor2);
    }

    public Vec2 getReactionForce() {
    	return m_pivotForce;
    }

    public float getReactionTorque() {
    	return m_limitForce;
    }

    public float getJointAngle() {
    	Body b1 = m_body1;
    	Body b2 = m_body2;
    	return b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
    }

    public float getJointSpeed() {
    	Body b1 = m_body1;
    	Body b2 = m_body2;
    	return b2.m_angularVelocity - b1.m_angularVelocity;
    }

    public boolean isMotorEnabled() {
    	return m_enableMotor;
    }

    public void enableMotor(boolean flag) {
    	m_enableMotor = flag;
    }

    public float getMotorTorque() {
    	return m_motorForce;
    }

    public void setMotorSpeed(float speed) {
    	m_motorSpeed = speed;
    }

    public void setMaxMotorTorque(float torque) {
    	m_maxMotorTorque = torque;
    }

    public boolean isLimitEnabled() {
    	return m_enableLimit;
    }

    public void enableLimit(boolean flag) {
    	m_enableLimit = flag;
    }

    public float getLowerLimit() {
    	return m_lowerAngle;
    }

    public float getUpperLimit() {
    	return m_upperAngle;
    }

    public void setLimits(float lower, float upper) {
    	//assert(lower <= upper);
    	m_lowerAngle = lower;
    	m_upperAngle = upper;
    }
}