package org.simantics.proconf.g3d.base;

import java.util.Collection;
import java.util.HashMap;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;

import org.simantics.db.Graph;
import org.simantics.db.Resource;
import org.simantics.layer0.utils.EntityFactory;
import org.simantics.layer0.utils.IEntity;
import org.simantics.proconf.g3d.Resources;
import org.simantics.proconf.g3d.stubs.Orientation;
import org.simantics.proconf.g3d.stubs.Position;

public class TransformationTools {
	
	private static boolean DEBUG = false;
	
	private Resource childRelation;
	private Resource parentRelation;
	
	public TransformationTools(Resource childRelation, Resource parentRelation) {
		this.childRelation = childRelation;
		this.parentRelation = parentRelation;
	}
	
	public IEntity getParent(IEntity node) {
		return node.getAtMostOneRelatedObject(parentRelation);
	}
	
	public Point3d getLocalFromWorld(IEntity node, Point3d worldCoord) {
    	IEntity parent = getParent(node);
        if (parent == null) {// this is a rootnode ( has no transformation) 
        	return worldCoord;
        }

        Point3d local = getLocalFromWorld2(parent,worldCoord);
        return local;
    }
    
    
    private Point3d getLocalFromWorld2(IEntity node, Point3d worldCoord) {
    	IEntity parent = getParent(node);
        if (parent == null) {// this is a root node ( has no transformation) 
        	return worldCoord;
        }

        Point3d local = getLocalFromWorld2(parent,worldCoord);
        if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
            local.sub(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            q.inverse();
            MathTools.rotate(q, local, local);
        }
        return local;
    }
    
    public Point3d getWorldFromLocal(IEntity node,Point3d localCoord) {
    	IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return localCoord;
        return getWorldFromLocal2(parent, localCoord);
    }
    
    private Point3d getWorldFromLocal2(IEntity node,Point3d localCoord) {
    	
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            MathTools.rotate(q, localCoord, localCoord);
        }
        if (node.hasStatement(Resources.g3dResource.HasLocalPosition))
            localCoord.add(G3DTools.getPoint(node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition)));
        
        IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return localCoord;
        
        return getWorldFromLocal2(parent,localCoord);
    }
    
    
    public AxisAngle4d getLocalFromWorld(IEntity node, AxisAngle4d worldRot) {
    	IEntity parent = getParent(node);
    	if (parent == null) // this is a rootnode ( has no transformation)
            return worldRot;

        AxisAngle4d local = getLocalFromWorld2(parent,worldRot);

        return local;
    }
    
    private AxisAngle4d getLocalFromWorld2(IEntity node, AxisAngle4d worldRot) {
    	IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return worldRot;
        AxisAngle4d local  = getLocalFromWorld2(parent,worldRot);
        
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            q.inverse();
            Quat4d q2 = new Quat4d();
            q2.set(local);
            q.mul(q2);
            local.set(q);
        }

        return local;
    }
    
    public AxisAngle4d getWorldFromLocal(IEntity node,AxisAngle4d localRot) {
    	IEntity parent = getParent(node);
    	if (parent == null)
    		return localRot;

        return getWorldFromLocal2(parent,localRot);
    }
    
    private AxisAngle4d getWorldFromLocal2(IEntity node,AxisAngle4d localRot) {
    	
        //System.out.print("wtl " + node.getResource() + " " + localCoord);
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            Quat4d q2 = new Quat4d();
            q2.set(localRot);
            q.mul(q2);
            localRot.set(q);
        }
        
        IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return localRot;
        
        //System.out.println(" " + localCoord);
        return getWorldFromLocal2(parent,localRot);
    }
    
    public Point3d getLocalFromWorldR(IEntity node, Point3d worldCoord) {
    	
        Point3d local = getLocalFromWorldR(null,worldCoord);
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            q.inverse();
            MathTools.rotate(q, local, local);
        }
        
        IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return worldCoord;
        
        return local;
    }
    
    public Point3d getWorldFromLocalR(IEntity node,Point3d localCoord) {
    	
        if (node.hasStatement(Resources.g3dResource.HasLocalOrientation)) {
            Quat4d q = new Quat4d();
            q.set(G3DTools.getOrientation(node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation)));
            MathTools.rotate(q, localCoord, localCoord);
        } 
        
        IEntity parent = getParent(node);
        if (parent == null) // this is a rootnode ( has no transformation)
            return localCoord;
        
        return getWorldFromLocalR(parent,localCoord);
    }
    
    /**
     * Updates transformations of all children of given node
     * @param node
     */
    public void propagateTransformChange(IEntity node) {
        Collection<IEntity> children = node.getRelatedObjects(childRelation);
    	IEntity wp = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
        IEntity wr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
        Quat4d rot = new Quat4d();
        if (wr != null)
            rot.set(G3DTools.getOrientation(wr));
        else
            rot.w = 1.0;
        Point3d pos = new Point3d();
        if (wp != null)
            pos = G3DTools.getPoint(wp);
        if (DEBUG) {
        	if (wr != null)
        		System.out.println("propagate transform " + node.getResource() + " " + pos + " " + G3DTools.getOrientation(wr));
        	else
        		System.out.println("propagate transform " + node.getResource() + " " + pos);
        }
        for (IEntity n : children) {
            IEntity lPos = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
            IEntity lRot = n.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
            if (DEBUG) System.out.print(n);
            if (lRot != null) {
            	AxisAngle4d la = G3DTools.getOrientation(lRot);
                AxisAngle4d wa = getWorldFromLocal(n, la);
                IEntity wo = n.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
                G3DTools.setOrientation(wo,wa);
                storeProperty(wo.getResource(), wa);
            }
            if (lPos != null) {
                Point3d lp = G3DTools.getPoint(lPos);
                if (DEBUG) System.out.println(lp);
                MathTools.rotate(rot, lp, lp);
                lp.add(pos);
                IEntity nwp = n.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
                G3DTools.setTuple3(nwp, lp);
                if (DEBUG) System.out.print(" " + lp);
                storeProperty(nwp.getResource(), lp);
            }
            if (DEBUG) System.out.println();
            propagateTransformChange(n);
            
        }
    }
    
    /**
     * Updates transformation of one child node without changing its local transformation.
     * @param parent
     * @param node
     */
    public void propagateLocalTransformChange(IEntity parent, IEntity node) {
        //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
    	IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
        IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
        Quat4d parentWRot = new Quat4d();
        if (parentWR != null) {
            parentWRot.set(G3DTools.getOrientation(parentWR));
           
        } else
            parentWRot.w = 1.0;
        Point3d parentWPos = new Point3d();
        if (parentWP != null) {
            parentWPos = G3DTools.getPoint(parentWP);
        }
        if (DEBUG) {
        	if (parentWR != null)
        		System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
        	else
        		System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
        }
        //for (IEntity n : children) {
        //	if (!n.equals(node))
        //		continue;
            IEntity lPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition);
            IEntity lRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
            if (DEBUG) System.out.print(node);
            if (lRot != null) {
            	AxisAngle4d aa = G3DTools.getOrientation(lRot);
            	storeProperty(lRot.getResource(), aa);
                AxisAngle4d la = getWorldFromLocal(node, aa);
                IEntity wo = node.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation);
                G3DTools.setOrientation(wo,la);
                storeProperty(wo.getResource(), la);
            }
            if (lPos != null) {
                Point3d lp = G3DTools.getPoint(lPos);
                storeProperty(lPos.getResource(), lp);
                MathTools.rotate(parentWRot, lp, lp);
                lp.add(parentWPos);
                IEntity nwp = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
                G3DTools.setTuple3(nwp, lp);
                storeProperty(nwp.getResource(), lp);
            }
            if (DEBUG) System.out.println();
            propagateTransformChange(node);
            
        }
//    }
    
    public static void resetTransformation(IEntity shape) {
		Graph graph = shape.getGraph();
		if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalPosition) == null) {

			// LocalPosition p = LocalPosition.createDefault(graph);
			Position p = Position.createDefault(graph);
			shape.addStatement(Resources.g3dResource.HasLocalPosition, p);
			// WorldPosition p2 = WorldPosition.createDefault(graph);
			Position p2 = Position.createDefault(graph);
			shape.addStatement(Resources.g3dResource.HasWorldPosition, p2);
			p.setX(new double[] { 0.0 });
			p.setY(new double[] { 0.0 });
			p.setZ(new double[] { 0.0 });

			p2.setX(new double[] { 0.0 });
			p2.setY(new double[] { 0.0 });
			p2.setZ(new double[] { 0.0 });

		} else {
			G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition), 0.0, 0.0, 0.0);
			G3DTools.setTuple3(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition), 0.0, 0.0, 0.0);
		}
		if (shape.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation) == null) {

			// LocalOrientation r = LocalOrientationFactory.create(graph);
			Orientation r = Orientation.createDefault(graph);
			shape.addStatement(Resources.g3dResource.HasLocalOrientation, r);
			// WorldOrientation r2 = WorldOrientationFactory.create(graph);
			Orientation r2 = Orientation.createDefault(graph);
			shape.addStatement(Resources.g3dResource.HasWorldOrientation, r2);
			r.setAngle(new double[] { 0.0 });
			r.setX(new double[] { 1.0 });
			r.setY(new double[] { 0.0 });
			r.setZ(new double[] { 0.0 });
			r2.setAngle(new double[] { 0.0 });
			r2.setX(new double[] { 1.0 });
			r2.setY(new double[] { 0.0 });
			r2.setZ(new double[] { 0.0 });

		} else {
			G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation),
					new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
			G3DTools.setOrientation(shape.getSingleRelatedObject(Resources.g3dResource.HasWorldOrientation),
					new AxisAngle4d(0.0, 1.0, 0.0, 0.0));
		}
	}
    
    /**
     * Updates transformation of one child node without changing its world transformation.
     * @param parent
     * @param node
     */
    public void propagateWorldTransformChange(IEntity parent, IEntity node) {
        //Collection<IEntity> children = parent.getRelatedObjects(childRelation);
    	IEntity parentWP = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
        IEntity parentWR = parent.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
        Quat4d parentWQuat = new Quat4d();
        if (parentWR != null) {
            parentWQuat.set(G3DTools.getOrientation(parentWR));
        }
        else
            parentWQuat.w = 1.0;
        Point3d parentWPos = new Point3d();
        if (parentWP != null) {
            parentWPos = G3DTools.getPoint(parentWP);
        }
        if (DEBUG){
        	if (parentWR != null) 
        	    System.out.println("propagate transform " + parent.getResource() + " " + parentWPos + " " + G3DTools.getOrientation(parentWR));
        	else
        		System.out.println("propagate transform " + parent.getResource() + " " + parentWPos);
        }
        //for (IEntity n : children) {
      //  	if (!n.equals(node))
      //  		continue;
            IEntity wPos = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldPosition);
            IEntity wRot = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
            
            if (DEBUG) System.out.print(node);
            if (wRot != null) {
            	AxisAngle4d aa = G3DTools.getOrientation(wRot);
            	storeProperty(wRot.getResource(), aa);
                AxisAngle4d la = getLocalFromWorld(node, aa);
                IEntity lRot = node.getSingleRelatedObject(Resources.g3dResource.HasLocalOrientation);
                G3DTools.setOrientation(lRot,la);
                storeProperty(lRot.getResource(), la);
            }
            if (wPos != null) {
                Point3d lp = G3DTools.getPoint(wPos);
                storeProperty(wPos.getResource(), lp);
                lp.sub(parentWPos);
                parentWQuat.inverse();     
                MathTools.rotate(parentWQuat, lp, lp);
                IEntity lPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
                G3DTools.setTuple3(lPos, lp);
                storeProperty(lPos.getResource(), lp);
            }
            if (DEBUG) System.out.println();
            propagateTransformChange(node);
            
 //       }
    }
        
    public boolean transformationUpdate(Graph graph, Resource resource) {
    	
    	//resources.startTransaction("transformationUpdate");
    	IEntity entity = EntityFactory.create(graph,resource);
    	return transformationUpdate(entity);
    }
    
    public boolean transformationUpdate(IEntity node) {
    	if (DEBUG) System.out.println("Node transformation update " + node.getResource());
    	IEntity worldPos = node.getSingleRelatedObject(Resources.g3dResource.HasWorldPosition);
    	IEntity localPos = node.getSingleRelatedObject(Resources.g3dResource.HasLocalPosition);
    	IEntity worldOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasWorldOrientation);
    	IEntity localOr = node.getAtMostOneRelatedObject(Resources.g3dResource.HasLocalOrientation);
    	
    	Tuple3d worldP =G3DTools.getPoint(worldPos);
    	Tuple3d localP = G3DTools.getPoint(localPos);

    	AxisAngle4d worldR = null;
    	AxisAngle4d localR = null;
    	if (worldOr != null) {
    		worldR = G3DTools.getOrientation(worldOr);
    		localR = G3DTools.getOrientation(localOr);
    	}
    	
    	boolean changed = false;
    	if (localP != null && worldP != null) {
    		Tuple3d cachedWorldP = (Tuple3d)getProperty(worldPos.getResource());
    		Tuple3d cachedLocalP = (Tuple3d)getProperty(localPos.getResource());
    		boolean changedLocalP = false;
    		boolean changedWorldP = false;
    		if (cachedLocalP == null)
    			changedLocalP = true;
    		else if (changed(cachedLocalP,localP))
    			changedLocalP = true;

    		if (cachedWorldP == null) {
    			changedWorldP = true;
    		} else if (changed(cachedWorldP,worldP)){
    			changedWorldP = true;
    		}
    		if (changedLocalP) {
    			storeProperty(localPos.getResource(), localP);
    			Tuple3d p = getWorldFromLocal(node, new Point3d(localP));
    			storeProperty(worldPos.getResource(), p);
    			G3DTools.setTuple3(worldPos, p);
    			if (DEBUG) System.out.println("Node changed local: wp " + worldP + " lp " + p + " old " + cachedLocalP);
    			changed = true;
    		} else if (changedWorldP) {
    			storeProperty(worldPos.getResource(), worldP);
    			Tuple3d p = getLocalFromWorld(node, new Point3d(worldP));
    			G3DTools.setTuple3(localPos, p);
    			storeProperty(localPos.getResource(), p);
    			if (DEBUG) System.out.println("Node changed world: wp " + worldP + " lp " + p + " old " + cachedWorldP);
    			changed = true;
    		}
    	}

    	if (localR != null || worldR != null) {
    		AxisAngle4d cachedWorldR = (AxisAngle4d)getProperty(worldOr.getResource());
    		AxisAngle4d cachedLocalR = (AxisAngle4d)getProperty(localOr.getResource());
    		boolean changedLocalR = false;
    		boolean changedWorldR = false;

    		if (cachedLocalR == null)
    			changedLocalR = true;
    		else if (changed(cachedLocalR,localR))
    			changedLocalR = true;

    		if (cachedWorldR == null) {
    			changedWorldR = true;
    		} else if (changed(cachedWorldR,worldR)){
    			changedWorldR = true;
    		}

    		if (changedLocalR) {
    			storeProperty(localOr.getResource(), localR);
    			AxisAngle4d p = getWorldFromLocal(node, new AxisAngle4d(localR));
    			G3DTools.setOrientation(worldOr, p);
    			storeProperty(worldOr.getResource(), p);
    			if (DEBUG) System.out.println("Node changed localR: wr " + p + " lr " + localR + " old " + cachedLocalR);
    			changed = true;
    		} else if (changedWorldR) {
    			storeProperty(worldOr.getResource(), worldR);
    			AxisAngle4d p = getLocalFromWorld(node, new AxisAngle4d(worldR));
    			G3DTools.setOrientation(localOr, p);
    			storeProperty(localOr.getResource(), p);
    			if (DEBUG) System.out.println("Node changed worldR: wr " + worldR + " lr " + p + " old " + cachedWorldR);
    			changed = true;
    		} 
    	}

    	if (changed)
    		propagateTransformChange(node);
    	else if (DEBUG)
    		System.out.println("No Node transformation change detected " + node.getResource());
    	return changed;
    }
    
    public static boolean changed(Tuple3d v1, Tuple3d v2) {
        Vector3d t = new Vector3d(v1);
        t.sub(v2);
        return t.lengthSquared() > 0.00001;
    }
    
    public static boolean changed (double d1 , double d2) {
    	return (Math.abs(d1 - d2) > 0.00001 );
    		
    }
    
    public static boolean changed(AxisAngle4d aa1, AxisAngle4d aa2) {
        if (Math.abs(aa1.angle - aa2.angle) > 0.00001)
            return true;
        Vector3d t1 = new Vector3d(aa1.x-aa2.x,aa1.y-aa2.y,aa1.z-aa2.z);
        if (t1.lengthSquared() > 0.00001) {
        	if (Math.abs(aa1.angle) < 0.0001)
        		return false;
        	return true;
        }
        return false;
    }

    
    private HashMap<Object, Object> properties = new HashMap<Object, Object>();
    
    public Object getProperty(Object key) {
        return properties.get(key);
    }
    
    public void storeProperty(Object key, Object value) {
        properties.put(key, value);
    }
    
    
    
    
}