/*******************************************************************************
 * Copyright (c) 2007 VTT Technical Research Centre of Finland and others.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
package org.simantics.proconf.g3d.base;

import java.util.Collection;
import java.util.Set;
import java.util.Stack;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.AxisAngle4f;
import javax.vecmath.Point3d;
import javax.vecmath.Point3f;
import javax.vecmath.Quat4d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Tuple3f;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import org.simantics.db.Builtins;
import org.simantics.db.Graph;
import org.simantics.db.Resource;
import org.simantics.layer0.utils.Statement;
import org.simantics.layer0.utils.IEntity;
import org.simantics.layer0.utils.EntityFactory;
import org.simantics.layer0.utils.viewpoints.State;
import org.simantics.layer0.utils.viewpoints.TraversalDecision;
import org.simantics.layer0.utils.viewpoints.TraversalResult;
import org.simantics.layer0.utils.viewpoints.TraversalRule;
import org.simantics.layer0.utils.viewpoints.TraversalUtils;
import org.simantics.proconf.g3d.Resources;
import org.simantics.proconf.g3d.stubs.G3DNode;
import org.simantics.proconf.g3d.stubs.Orientation;
import org.simantics.proconf.g3d.stubs.Position;

/**
 * Basic ThreeDimensionalModelingOntology tools
 * 
 * @author Marko Luukkainen
 *
 */
public class G3DTools {
	
	private static boolean DEBUG = false;
	
	private static TransformationTools tt;

	public static void initialize() {
		tt = new TransformationTools(Resources.g3dResource.HasChild, Resources.g3dResource.HasParent);
	}
	
	public static void deinitialize() {
		tt = null;
	}
	
	public static Point3d getPoint(IEntity p) {
        return new Point3d(p.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
        				   p.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
        				   p.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }
    
    public static Point3f getPointFloat(IEntity p) {
        return new Point3f((float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
        				   (float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
        				   (float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }
    
    
    public static Vector3d getVector(IEntity p) {
        return new Vector3d(p.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
				   			p.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
				   			p.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }
    
    public static Vector3f getVectorFloat(IEntity p) {
        return new Vector3f((float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
				   			(float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
				   			(float)p.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }

    
    public static void setTuple3(IEntity d, Tuple3d translation) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, translation.x);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, translation.y);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, translation.z);    
    }
    
    public static void setTuple3(IEntity d, Tuple3f translation) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, translation.x);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, translation.y);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, translation.z);    
    }

    public static void setTuple3(IEntity d, double x, double y, double z) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, x);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, y);
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, z);    
    }
    
    public static void addTuple3(IEntity d, Tuple3d translation) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, translation.x + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasX));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, translation.y + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasY));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, translation.z + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }
    
    public static void addTuple3(IEntity d, Tuple3f translation) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, translation.x + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasX));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, translation.y + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasY));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, translation.z + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));
    }

    public static void addTuple3(IEntity d, double x, double y, double z) {
    	d.setRelatedScalarDouble(Resources.g3dResource.HasX, x + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasX));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasY, y + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasY));
    	d.setRelatedScalarDouble(Resources.g3dResource.HasZ, z + d.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ));    
    }
    
    public static AxisAngle4d getOrientation(IEntity r) {
        return new AxisAngle4d(r.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
        					   r.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
        					   r.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ),
        					   r.getSingleRelatedScalarDouble(Resources.g3dResource.HasAngle));
    }
    
    public static AxisAngle4f getOrientationFloat(IEntity r) {
        return new AxisAngle4f((float)r.getSingleRelatedScalarDouble(Resources.g3dResource.HasX),
                			   (float)r.getSingleRelatedScalarDouble(Resources.g3dResource.HasY),
                			   (float)r.getSingleRelatedScalarDouble(Resources.g3dResource.HasZ),
                			   (float)r.getSingleRelatedScalarDouble(Resources.g3dResource.HasAngle));
    }

    public static void setOrientation(IEntity r, AxisAngle4d aa) {
    	r.setRelatedScalarDouble(Resources.g3dResource.HasX, aa.x);
    	r.setRelatedScalarDouble(Resources.g3dResource.HasY, aa.y);
    	r.setRelatedScalarDouble(Resources.g3dResource.HasZ, aa.z);
    	r.setRelatedScalarDouble(Resources.g3dResource.HasAngle, aa.angle);
    }
    
    
    public static void multiplyOrientation(IEntity r, AxisAngle4d rot) {
		AxisAngle4d current = getOrientation(r);
		Quat4d q1 = new Quat4d();
		q1.set(current);
		Quat4d q2 = new Quat4d();
		// q2.set(rot);
		q2.set(rot);
		q2.mul(q1);
		rot.set(q2);
		setOrientation(r, rot);
	}
    
    
    public static AxisAngle4d getWorldFromLocal(IEntity node, AxisAngle4d localRot) {
    	return tt.getWorldFromLocal(node, localRot);
    }
    
    public static Point3d getWorldFromLocal(IEntity node, Point3d localRot) {
    	return tt.getWorldFromLocal(node, localRot);
    }
    
    public static AxisAngle4d getLocalFromWorld(IEntity node, AxisAngle4d localRot) {
    	return tt.getLocalFromWorld(node, localRot);
    }
    
    public static Point3d getLocalFromWorld(IEntity node, Point3d localRot) {
    	return tt.getLocalFromWorld(node, localRot);
    }
    
    public static void propagateLocalTransformChange(IEntity node, IEntity child) {
    	tt.propagateLocalTransformChange(node, child);
    }
    
    public static void propagateWorldTransformChange(IEntity node, IEntity child) {
    	tt.propagateWorldTransformChange(node, child);
    }
    
    public static void transformationUpdate(Graph graph, Resource resource) {
    	tt.transformationUpdate(graph, resource);
    }
    
    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));
		}
	}
    
    
    public static G3DNode getModelFromResource(Graph graph,Resource resource) {
    	G3DNode node = new G3DNode(graph, resource);
    	while (true) {
    		G3DNode parent = node.getParent();
    		if (parent == null)
    			break;
    		node = parent;
    	}
        return node;
    }
    
    /**
     * Checks if instance has a certain property instance
     * @param instance
     * @param propertyInstance
     * @return
     */
    public static boolean hasProperty(Graph graph,Resource instance, Resource propertyInstance) {
        Builtins builtins = graph.getBuiltins();
    	Stack<IEntity> props = new Stack<IEntity>();
        IEntity IEntity = EntityFactory.create(graph, instance);
        Collection<IEntity> res = IEntity.getRelatedObjects(builtins.HasProperty);
        for (IEntity t : res)
        	props.add(t);
        
        while (!props.isEmpty()) {
            IEntity property = props.pop();
            if (property.getResource().equals(propertyInstance)) {
                return true;
            }
            res = property.getRelatedObjects(builtins.HasProperty);
            for (IEntity r : res) {
                props.add(r);
            }
        }
        return false;
    }
    
    /**
     * Checks if one of shapes subshapes has a certain property
     * @param instance shape instance
     * @param propertyInstance instance of a property
     * @return
     * 
     */
    public static boolean hasSubProperty(Graph graph,Resource instance, Resource propertyInstance) {
    	 Builtins builtins = graph.getBuiltins();
    	 Stack<IEntity> instances = new Stack<IEntity>();

         //Resource res[] = instance.getRelatedResources(Builtins.HasProperty);
    	 IEntity entity = EntityFactory.create(graph, instance);
    	 Collection<IEntity> res;
//         res = entity.getRelatedObjects(Resources.g3dResource.HasChild);
//         for (IEntity t : res) {
//        	 Collection<IEntity> sub = t.getRelatedObjects(Resources.g3dResource.HasGeometryDefinition);
//        	 if (sub.size() > 0)
//             	instances.addAll(sub);
//         }
    	 {
    		 Collection<IEntity> sub = entity.getRelatedObjects(Resources.g3dResource.HasGeometryDefinition);
    		 if (sub.size() > 0)
              	instances.addAll(sub);
    	 }
         while (!instances.isEmpty()) {
            IEntity i = instances.pop();
            Stack<IEntity> props = new Stack<IEntity>();
            res = i.getRelatedObjects(builtins.HasProperty);
            for (IEntity r : res) {
                props.add(r);
            }
            while (!props.isEmpty()) {
                IEntity property = props.pop();
                if (property.equals(propertyInstance)) {
                    return true;
                }
                res = property.getRelatedObjects(builtins.HasProperty);
                for (IEntity r : res) {
                    props.add(r);
                }
            }
            res = i.getRelatedObjects(Resources.g3dResource.HasGeometryDefinition);
            for (IEntity r : res) {
            	// TODO : unnecessary check
            	Collection<IEntity> sub = r.getRelatedObjects(Resources.g3dResource.GeometryDefinitionOf);
           	 	if (sub.size() > 0)
                	instances.add(r);
            }
            
        }
         return false;
     }
    
    
    
    
    
    
    /**
     * Loads positions of control point to rule cache
     * 
     * @param root resource of the modeled plant
     */
    public static void reloadCache(Graph graph, Resource root) {
//        TraverseHandler handler = new TraverseHandler() {
//            public boolean addToResult(Resource r) {
//                if (r.isInstanceOf(GlobalIdMap.get(ThreeDimensionalModelingOntologyMapping.GRAPHICS_NODE)))
//                    return true;
//                return false;
//            }
//            
//            public TraverseRelation[] traverseFromResource(Resource resource) {
//                return new TraverseRelation[] { new TraverseRelation(
//                        ThreeDimensionalModelingOntologyMapping.HAS_SUBNODES,
//                        TraverseDirection.OUTBOUND)};
//            }
//            
//            
//        };
    	
    	IEntity IEntity = EntityFactory.create(graph, root);
    	State s = new State() {};
    	TraversalResult res = TraversalUtils.traverse(new TraversalRule (){
    		
    		
    		@Override
    		public boolean areAllStatesRelevant() {
    			return true;
    		}
    		
    		@Override
    		public TraversalDecision makeTraversalDecision(State state,
    				Statement statement) {
    			if (statement.getPredicate().isInstanceOf(Resources.g3dResource.HasChild))
    				return TraversalDecision.continueTraversal(state);
    			else
    				return TraversalDecision.stopTraversal;
    		}
    		
    		@Override
    		public Collection<State> relevantStates() {
    			return null;
    		}
    		}, IEntity,s
    		);
    	
    
    	Set<Resource> cps = res.get(s);
        
        //Collection<Resource> cps = TraverseUtils.traverseGraph(root, handler);
        for (Resource r : cps) {
            G3DNode cp = new G3DNode(graph,r);
            if (cp.getLocalPosition() != null)
                tt.storeProperty(cp.getLocalPosition().getResource(),G3DTools.getPoint(cp.getLocalPosition()));
            if (cp.getWorldPosition() != null)
                tt.storeProperty(cp.getWorldPosition().getResource(),G3DTools.getPoint(cp.getWorldPosition()));
            if (cp.getLocalOrientation() != null)
                tt.storeProperty(cp.getLocalOrientation().getResource(),G3DTools.getOrientation(cp.getLocalOrientation()));
            if (cp.getWorldOrientation() != null)
                tt.storeProperty(cp.getWorldOrientation().getResource(),G3DTools.getOrientation(cp.getWorldOrientation()));
            
        }
    }

}