package mocap.figure;

import java.awt.Color;
import javax.media.j3d.Appearance;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.Material;
import javax.media.j3d.Switch;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.AxisAngle4d;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import com.sun.j3d.utils.geometry.Sphere;

/**
 * Represents a trail point of a given joint.
 *
 * @author Quan Nguyen
 */
public class MotionTrailPoint
{

    private double _dTime;
    private Point3d _position;
    private double _dVelocity = 0.0;
    private Color _color;
    private float _fRadius = 0.01f;
//    private float _fVelocityHeight = 0.001f;
    //	private BranchGroup _bgPoint;
    private BranchGroup _bgMotionTrailPoint;
    private double _dScale;
    private double _velocityScale = 1d;
    private String _sBoneName;
    private TransformGroup _tgSphere;
    private Sphere _sphere;
    //	private BranchGroup _bgVelocity;
//	private Cylinder _cylinderVelocity;
    private Circle _circleVelocity;
    private TransformGroup _tgVelocity;
    private TransformGroup _tgMotionTrailPoint;
    private Switch _velocitySwitch;

    /**
     * Represents the position of the given bone at a specific time.
     *
     * @param bone
     * @param time
     * @param color
     * @param scale
     */
    private MotionTrailPoint(String bone, double time, 
            double velocity, Color color, double scale)
    {
        _sBoneName = bone;
        _dVelocity = velocity;
        _dTime = time;
        _position = new Point3d();
        _color = color;
        _dScale = scale;
        _dVelocity = _fRadius + (velocity * 0.05);

        initSceneObjects();
        createMotionTrailSphere();
        createVelocityVisualisation();
        _velocitySwitch.setWhichChild(Switch.CHILD_ALL);
    }

    
    public MotionTrailPoint(String bone, double time, Color color)
    {

        this(bone, time, 0, color, 1);
    }

    public MotionTrailPoint(String bone, double time, double velocity,
            Color color)
    {

        this(bone, time, velocity, color, 1);
    }

    public MotionTrailPoint(String bone, double time, Color color, double scale)
    {

        this(bone, time, 0, color, scale);
    }

    private void initSceneObjects()
    {
        _bgMotionTrailPoint = new BranchGroup();
        _bgMotionTrailPoint.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
        _bgMotionTrailPoint.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
        _bgMotionTrailPoint.setCapability(BranchGroup.ALLOW_DETACH);

        _tgMotionTrailPoint = new TransformGroup();
        _tgMotionTrailPoint.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        _tgMotionTrailPoint.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        _tgMotionTrailPoint.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
        Transform3D t3dMotionTrailPointTranslation = new Transform3D();
        t3dMotionTrailPointTranslation.setTranslation(new Vector3d(_position));
        _tgMotionTrailPoint.setTransform(t3dMotionTrailPointTranslation);
        Transform3D t3dMotionTrailPointScale = new Transform3D();
        t3dMotionTrailPointScale.setScale(_dScale);

        _bgMotionTrailPoint.addChild(_tgMotionTrailPoint);

        _velocitySwitch = new Switch(Switch.CHILD_MASK);
        _velocitySwitch.setCapability(Switch.ALLOW_SWITCH_WRITE);
        _tgMotionTrailPoint.addChild(_velocitySwitch);
    }

    /**
     * Attaches sphere representation (geometry) to the point in space.
     */
    private void createMotionTrailSphere()
    {
        Material materialStandard = new Material(new Color3f(_color),
                new Color3f(_color), new Color3f(_color), new Color3f(0.7f, 0.7f,
                0.7f), 10.0f);
        Appearance appearance = new Appearance();
        appearance.setMaterial(materialStandard);

        _sphere = new Sphere(_fRadius);
        _sphere.setAppearance(appearance);
        _tgSphere = new TransformGroup();
        _tgSphere.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        _tgSphere.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        _tgSphere.setCapability(TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ);
        Transform3D t3dSphereTranslation = new Transform3D();
        t3dSphereTranslation.setTranslation(new Vector3d(_position));
        _tgSphere.setTransform(t3dSphereTranslation);
        Transform3D t3dSphereScale = new Transform3D();
        t3dSphereScale.setScale(_dScale);
        _tgSphere.addChild(_sphere);
        // set to null for garbage collector
        _velocitySwitch.addChild(_tgSphere); // add the sphere

        materialStandard = null;
        appearance = null;
        t3dSphereTranslation = null;
        t3dSphereScale = null;

    }

    /**
     * Creates a 2D circle to represent velocity.
     *
     * @param pos
     * @param color
     * @param velocity
     * @param scale
     */
    private void createVelocityVisualisation()
    {
        _circleVelocity = new Circle((float) (_velocityScale * _dVelocity), 100, _color);
        _tgVelocity = new TransformGroup();
        _tgVelocity.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        _tgVelocity.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        _tgVelocity.setCapability(TransformGroup.ALLOW_CHILDREN_EXTEND);
        _tgVelocity.setCapability(TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ);
        Transform3D t3dVelocityTranslation = new Transform3D();
        t3dVelocityTranslation.setTranslation(new Vector3d(_position));
        _tgVelocity.setTransform(t3dVelocityTranslation);
        Transform3D t3dVelocityScale = new Transform3D();
        t3dVelocityScale.setScale(_dScale);
        _tgVelocity.addChild(_circleVelocity);
        _velocitySwitch.addChild(_tgVelocity);
        t3dVelocityTranslation = null;
        t3dVelocityScale = null;

    }

    public BranchGroup getObject()
    {
        return _bgMotionTrailPoint;
    }

    public String getBone()
    {
        return _sBoneName;
    }

    public double getTimePointInSeconds()
    {
        return _dTime;
    }

    public Color getColor()
    {
        return _color;
    }

    public void setScale(double scale)
    {
        _dScale = scale;
        Transform3D t3dSphere = new Transform3D();
        _tgMotionTrailPoint.getTransform(t3dSphere);
        t3dSphere.setScale(_dScale);
        _tgMotionTrailPoint.setTransform(t3dSphere);
        t3dSphere = null;
    }

    public double getScale()
    {
        return _dScale;
    }

    public void setVelocityScale(double s) {
        _velocityScale = s;
    }

    public void setPosition(Point3d positionInWorld)
    {
        _position = positionInWorld;
        Transform3D t3dPosition = new Transform3D();
        _tgMotionTrailPoint.getTransform(t3dPosition);
        t3dPosition.setTranslation(new Vector3d(_position));
        _tgMotionTrailPoint.setTransform(t3dPosition);
        //		drawLine(_tgMotionTrailPoint, new Vector3d(0,0,0),
        //				new Vector3d(_p3dPosition), new Color3f(Color.MAGENTA), 0.1f);
        t3dPosition = null;
    }

    public void getPosition(Vector3d positionInWorld)
    {
        Transform3D t3dSphere = new Transform3D();
        _tgMotionTrailPoint.getTransform(t3dSphere);
        t3dSphere.get(positionInWorld);
    }

    public void angleVelocityVisualisation(Point3d positionNextPointInWorld)
    {
        //		Transform3D t3dPosition = new Transform3D();
        //		_tgMotionTrailPoint.setTransform(t3dPosition);
        positionNextPointInWorld.sub(_position);
        Point3d p3dOrigin = new Point3d(0, 0, 0);
        Vector3d v3dNewYAxis = new Vector3d();
        Vector3d v3dOldYAxis = new Vector3d();
        Vector3d v3dCrossProduct_RotationAxis = new Vector3d();

        v3dOldYAxis.add(p3dOrigin, new Vector3d(0, 1, 0));
        v3dOldYAxis.sub(v3dOldYAxis, p3dOrigin);

        // Create the Vector to the current effector pos
        v3dNewYAxis.sub(positionNextPointInWorld, p3dOrigin);

        // normalize the vectors
        v3dNewYAxis.normalize();
        v3dNewYAxis.normalize();

        // the dot product gives me the cosine of the desired angle
        double dCosinus = v3dOldYAxis.angle(v3dNewYAxis);
        //		drawLine(_tgVelocity, new Vector3d(p3dOrigin),
        //				v3dNewYAxis, new Color3f(Color.RED), 0.1f);
        //		drawLine(_tgVelocity, new Vector3d(p3dOrigin),
        //				v3dOldYAxis, new Color3f(Color.BLUE), 0.1f);
        //		drawLine(_tgVelocity, new Vector3d(0,0,0),
        //				new Vector3d(p3dOrigin), new Color3f(Color.GREEN), 0.1f);
        //
        //		drawLine(_tgVelocity, new Vector3d(positionNextPointInWorld),
        //				new Vector3d(p3dOrigin), new Color3f(Color.CYAN), 0.1f);

        // use the cross product to check which way to rotate
        v3dCrossProduct_RotationAxis.cross(v3dOldYAxis, v3dNewYAxis);
        //		drawLine(_tgVelocity, new Vector3d(p3dOrigin),
        //				v3dCrossProduct_RotationAxis, new Color3f(Color.WHITE), 0.1f);
        rotate(v3dCrossProduct_RotationAxis, dCosinus);

        v3dNewYAxis = null;
        v3dOldYAxis = null;
        v3dCrossProduct_RotationAxis = null;

    }

    protected double rotate(Vector3d rotationAxis, double angle)
    {
        // float fAngle = (float) Math.toRadians(dRotationX);

        //
        // System.out.println(">>>> Rotate Axis (world):  " + rotationAxis
        // + " <<<<<< ");

        Transform3D t3dCurrentPositionInWorld = new Transform3D();
        _tgVelocity.getLocalToVworld(t3dCurrentPositionInWorld);

        t3dCurrentPositionInWorld.invert();
        t3dCurrentPositionInWorld.transform(rotationAxis);
        // System.out.println(">>>> Rotate Axis (local):  " + rotationAxis
        // + " <<<<<< ");

        // //System.out.println("Rotate double:" + this + " : " + dRotationX +
        // "(Rad: " + fAngle + ")");
        // axisAngleX.angle = fAngle;

        // if absoute then reset the joint
        //		if (isAbsolute) {
        //			reset();
        //
        //		}
        AxisAngle4d axisAngle = new AxisAngle4d(rotationAxis, angle);

        Transform3D t3dNewRotation = new Transform3D();
        t3dNewRotation.setRotation(axisAngle);

        Transform3D t3dCurrentMainTransform = new Transform3D();
        _tgVelocity.getTransform(t3dCurrentMainTransform);

        t3dCurrentMainTransform.mul(t3dNewRotation);

        _tgVelocity.setTransform(t3dCurrentMainTransform);
        // LineDrawer.drawLine(tgMain, rotationAxis, Color.YELLOW);
        //		t3dCurrentMainTransform.
        // update all Listeners

        t3dCurrentPositionInWorld = null;
        axisAngle = null;
        return angle;
    }

    public void showMotionTrailVelocity(boolean showMotionTrailVelocity)
    {
        if (showMotionTrailVelocity) {
            _velocitySwitch.setWhichChild(Switch.CHILD_ALL);
        } else {
            _velocitySwitch.setWhichChild(0);
        }
    }
}