package de.kleppmann.maniation.geometry;

import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.Set;

import de.kleppmann.maniation.dynamics.Body;
import de.kleppmann.maniation.dynamics.GeneralizedBody;
import de.kleppmann.maniation.dynamics.MeshBody;
import de.kleppmann.maniation.maths.Quaternion;
import de.kleppmann.maniation.maths.Vector3D;
import de.kleppmann.maniation.scene.Bone;
import de.realityinabox.util.Pair;

public class ArticulatedLimb extends AnimateMesh {
    
    private ArticulatedMesh wholeMesh;
    private MeshTriangle[] triangles;
    private Bone bone;
    private ArticulatedLimb parent;
    private CollisionVolume volume;
    private MeshBody dynamicBody;
    private Body.State dynamicState;
    private Vector3D baseRest, baseCurrent;
    private Quaternion orientRest, orientCurrent;
    private Vector3D axis;
    private List<Pair<Vector3D,Double>> bubbles;
    
    public ArticulatedLimb(Set<MeshTriangle> triangles, Bone bone, ArticulatedLimb parent,
            ArticulatedMesh wholeMesh) {
        super(null);
        this.triangles = triangles.toArray(new MeshTriangle[triangles.size()]);
        this.bone = bone;
        this.parent = parent;
        this.wholeMesh = wholeMesh;
        this.volume = new CollisionVolume(this.triangles);
        // Determine rest position and orientation
        if (parent != null) {
            baseRest = parent.baseRest; orientRest = parent.orientRest;
            baseCurrent = parent.baseCurrent; orientCurrent = parent.orientCurrent;
        } else {
            baseRest = new Vector3D(); orientRest = new Quaternion();
            baseCurrent = wholeMesh.getCurrentLocation();
            orientCurrent = wholeMesh.getCurrentOrientation();
        }
        Vector3D local = new Vector3D(bone.getBase().getX(), bone.getBase().getY(), bone.getBase().getZ());
        baseRest = baseRest.add(orientRest.transform(local));
        orientRest = orientRest.mult(bone.getOrientation().getValue());
        baseCurrent = baseCurrent.add(orientCurrent.transform(local));
        orientCurrent = orientCurrent.mult(bone.getOrientation().getValue());
        if (bone.getPose() != null) orientCurrent = orientCurrent.mult(bone.getPose().getValue());
    }
    
    public Vector3D currentVertexPosition(Vector3D pos) {
        Vector3D local = orientRest.getInverse().transform(pos.subtract(baseRest));
        return orientCurrent.transform(local).add(baseCurrent);
    }
    
    public void updateBubbles() {
        axis = orientCurrent.transform(new Vector3D(0,1,0));
        Vector3D[] points = new Vector3D[triangles.length*3];
        for (int i=0; i<triangles.length; i++)
            for (int j=0; j<3; j++)
                points[3*i+j] = triangles[i].getVertices()[j].getPosition();
        Arrays.sort(points, new Comparator<Vector3D>() {
            public int compare(Vector3D v1, Vector3D v2) {
                double l1 = v1.subtract(baseCurrent).mult(axis);
                double l2 = v2.subtract(baseCurrent).mult(axis);
                if (l1 < l2) return -1;
                if (l1 > l2) return +1; else return 0;
            }
        });
        bubbles = new java.util.ArrayList<Pair<Vector3D,Double>>();
        double size = 0.0, start = 0.0;
        for (Vector3D point : points) {
            Vector3D diff = point.subtract(baseCurrent);
            double apos = diff.mult(axis); // axial
            double rpos = diff.cross(axis).magnitude(); // radial
            if (rpos > size) size = rpos;
            if (apos - start > size) {
                bubbles.add(new Pair<Vector3D,Double>(baseCurrent.add(axis.mult(start+size)), size));
                start += 2.0*size/3.0;
                size = rpos;
            }
        }
        if (size > 0.0) bubbles.add(new Pair<Vector3D,Double>(baseCurrent.add(axis.mult(start+size)), size));
    }

    @Override
    public boolean isInsideVolume(Vector3D point) {
        CollisionVolume vol = wholeMesh.getCollisionVolume();
        Vector3D outside = new Vector3D(vol.getBBox().minx - vol.size(),
                vol.getBBox().miny - vol.size(), vol.getBBox().minz - vol.size());
        return (vol.intersections(point, outside) % 2) == 1;
    }

    @Override
    public MeshBody getDynamicBody() {
        return dynamicBody;
    }
    
    @Override
    public void setDynamicBody(GeneralizedBody dynamicBody) {
        this.dynamicBody = (MeshBody) dynamicBody;
        for (MeshTriangle tri : triangles) tri.setBody(this.dynamicBody);
    }
    
    @Override
    public Body.State getDynamicState() {
        return dynamicState;
    }
    
    @Override
    public void setDynamicState(GeneralizedBody.State state, Vector3D com) {
        if (!(state instanceof Body.State)) throw new IllegalArgumentException();
        com = dynamicBody.getCentreOfMass();
        this.dynamicState = (Body.State) state;
        this.orientCurrent = dynamicState.getOrientation();
        this.baseCurrent = dynamicState.getCoMPosition().subtract(
                orientCurrent.transform(com));
    }
    
    @Override
    public Vector3D getRestLocation() {
        return baseRest;
    }

    @Override
    public Quaternion getRestOrientation() {
        return orientRest;
    }

    @Override
    public Vector3D getCurrentLocation() {
        return baseCurrent;
    }

    @Override
    public Quaternion getCurrentOrientation() {
        return orientCurrent;
    }

    @Override
    public CollisionVolume getCollisionVolume() {
        return volume;
    }
    
    @Override
    public MeshTriangle[] getTriangles() {
        return triangles;
    }
    
    public Bone getBone() {
        return bone;
    }

    public ArticulatedLimb getParent() {
        return parent;
    }
    
    public List<Pair<Vector3D,Double>> getBubbles() {
        return bubbles;
    }
    
    @Override
    public String toString() {
        /*return orientCurrent.getW() + " " + orientCurrent.getX() + " " + 
                orientCurrent.getY() + " " + orientCurrent.getZ();*/
        return getDynamicBody().toString();
    }
}

@@ -39,62 +39,8 @@
 y;%0A%0A
-import java.util.Arrays;%0Aimport java.util.Comparator;%0A
 impo
@@ -367,16 +367,60 @@
 e.Bone;%0A
+import de.kleppmann.maniation.scene.Bubble;%0A
 import d
@@ -844,35 +844,8 @@
 nt;%0A
-    private Vector3D axis;%0A
     
@@ -2497,1357 +2497,294 @@
     
-axis = orientCurrent.transform(new Vector3D(0,1,0));%0A        Vector3D%5B%5D points = new Vector3D%5Btriangles.length*3%5D;%0A        for (int i=0; i%3Ctriangles.length; i++)%0A            for (int j=0; j%3C3; j++)%0A                points%5B3*i+j%5D = triangles%5Bi%5D.getVertices()%5Bj%5D.getPosition();%0A        Arrays.sort(points, new Comparator%3CVector3D%3E() %7B%0A            public int compare(Vector3D v1, Vector3D v2) %7B%0A                double l1 = v1.subtract(baseCurrent).mult(axis);%0A                double l2 = v2.subtract(baseCurrent).mult(axis);%0A                if (l1 %3C l2) return -1;%0A                if (l1 %3E l2) return +1; else return 0;%0A            %7D%0A        %7D);%0A        bubbles = new java.util.ArrayList%3CPair%3CVector3D,Double%3E%3E();%0A        double size = 0.0, start = 0.0;%0A        for (Vector3D point : points) %7B%0A            Vector3D diff = point.subtract(baseCurrent);%0A            double apos = diff.mult(axis); // axial%0A            double rpos = diff.cross(axis).magnitude(); // radial%0A            if (rpos %3E size) size = rpos;%0A            if (apos - start %3E size) %7B%0A                bubbles.add(new Pair%3CVector3D,Double%3E(baseCurrent.add(axis.mult(start+size)), size));%0A                start += 2.0*size/3.0;%0A                size = rpos;%0A            %7D%0A        %7D%0A        if (size %3E 0.0) bubbles.add(new Pair%3CVector3D,Double%3E(baseCurrent.add(axis.mult(start+size)), size));
+bubbles = new java.util.ArrayList%3CPair%3CVector3D,Double%3E%3E();%0A        for (Bubble b : bone.getBubbles()) %7B%0A            Vector3D centre = currentVertexPosition(new Vector3D(b.getX(), b.getY(), b.getZ()));%0A            bubbles.add(new Pair%3CVector3D,Double%3E(centre, b.getRadius()));%0A        %7D
 %0A   

Load bubbles from file instead of using heuristic