package org.simantics.processeditor.common;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.Stack;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Matrix3d;
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.processeditor.ProcessResource;
import org.simantics.processeditor.actions.PositionType;
import org.simantics.processeditor.common.PipingTools2.Direction;
import org.simantics.processeditor.stubs.PipeControlPoint;
import org.simantics.processeditor.stubs.PipeRun;
import org.simantics.proconf.g3d.base.G3DTools;
import org.simantics.proconf.g3d.base.MathTools;
import org.simantics.proconf.g3d.base.TransformationTools;
import org.simantics.utils.ErrorLogger;
import org.simantics.utils.datastructures.Pair;


public class ControlPointTools {
	
	private static boolean DEBUG = false;
	
	private static TransformationTools tt;
	
	public static void initialize() {
		tt = new TransformationTools(ProcessResource.plant3Dresource.HasSubPoint,ProcessResource.plant3Dresource.SubPointOf) {
			@Override
			public IEntity getParent(IEntity node) {
				IEntity parent =  super.getParent(node);
				if (parent == null) {
					parent = node.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.ControlPointOf);
					if (parent != null)
						parent = parent.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasParent);
					else
						parent = node.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasParent);
						
				}
				return parent;
			}
		};
	}
	
	public static void deinitialize() {
		tt = null;
	}
	
	/**
	 * Adds new control point between given control points.
	 * New pcp must be already part of the same piperun as previous CP and nextCP
	 * 
	 * SizeChangeControlPoints cannot be inserted with this method, since it does link two different piperuns to each other
	 * 
	 * @param newCP
	 * @param previousCP
	 * @param nextCP
	 */
	public static void insertControlPoint(IEntity newCP, IEntity previousCP, IEntity nextCP) {
		// inserting an offsetpoint is error, 
		assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));
		// size change control point cannot be inserted this way, because it ends PipeRun
		assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.SizeChangeControlPoint));
		
		IEntity piperun = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun);
		// and just to make sure that control point structure is not corrupted
		assert(piperun.equals(nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun)));
		assert(piperun.equals(newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOfPipeRun)));
		
		// insert new BranchControlPoint between straight's control points
    	IEntity previousNext = previousCP.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasNext);
    	IEntity previousPrevious = previousCP.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasPrevious);
    	
    	IEntity offsetCP = null;
    	
    	if (newCP.isInstanceOf(ProcessResource.plant3Dresource.OffsettingPoint)) {
    		offsetCP = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
    	}
    	
		if (previousNext != null && previousNext.equals(nextCP)) {
			assert(!previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));
			assert(!nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));
			
			setStatement(previousCP, ProcessResource.plant3Dresource.HasNext, newCP);
            setStatement(newCP, ProcessResource.plant3Dresource.HasPrevious, previousCP);
            
            if (previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {        	
            	IEntity sccp = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);
            	setStatement(sccp, ProcessResource.plant3Dresource.HasNext, newCP);
            }
            
            setStatement(newCP, ProcessResource.plant3Dresource.HasNext, nextCP);
            
            if (offsetCP == null) {
            	setStatement(nextCP, ProcessResource.plant3Dresource.HasPrevious, newCP);
            } else {
            	setStatement(nextCP, ProcessResource.plant3Dresource.HasPrevious, offsetCP);
            	setStatement(offsetCP, ProcessResource.plant3Dresource.HasNext, nextCP);
            	setStatement(offsetCP, ProcessResource.plant3Dresource.HasPrevious, previousCP);
            }
            
            if (nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
            	IEntity ocp = nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
            	setStatement(ocp, ProcessResource.plant3Dresource.HasPrevious, newCP);
            }
            
         } else if (previousPrevious != null && previousPrevious.equals(nextCP)) {
        	 // control point were given in reverse order 
        	 assert(!nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));
 			 assert(!previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));
 			 
 			 setStatement(newCP, ProcessResource.plant3Dresource.HasNext, previousCP);
 			 if (offsetCP == null) {
             	setStatement(previousCP, ProcessResource.plant3Dresource.HasPrevious, newCP);
             } else {
             	setStatement(previousCP, ProcessResource.plant3Dresource.HasPrevious, offsetCP);
             	setStatement(offsetCP, ProcessResource.plant3Dresource.HasNext, previousCP);
             	setStatement(offsetCP, ProcessResource.plant3Dresource.HasPrevious, nextCP);
             }
 			 
        	 if (previousCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
        		 IEntity ocp = previousCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
        		 setStatement(ocp, ProcessResource.plant3Dresource.HasPrevious, newCP);
             }
        	 
        	 setStatement(newCP, ProcessResource.plant3Dresource.HasPrevious, nextCP);
        	 setStatement(nextCP, ProcessResource.plant3Dresource.HasNext, newCP);
             if (nextCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
            	 IEntity sccp = nextCP.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);
            	 setStatement(sccp, ProcessResource.plant3Dresource.HasNext, newCP);
             }
        } else {
            ErrorLogger.defaultLogError(
                    "Route pipe : could not find connection between straight pipe's control points", null);
        }

	}
	
	/**
	 * Adds new control point attaching it to given control point.
	 * If the new control point is SizeChangeControlPoint, it must have its offset point set.
	 * 
	 * @param newCP
	 * @param pcp
	 * @param direction
	 */
	public static void insertControlPoint(IEntity newCP, IEntity pcp, Direction direction) {
		assert(!newCP.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));
		if (direction == Direction.NEXT) {
			// if direction is next, user must have given OffsetPoint
			assert(!pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint));
			// basic next/prev links
			pcp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
			pcp.addStatement(ProcessResource.plant3Dresource.HasNext, newCP);
			newCP.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
			newCP.addStatement(ProcessResource.plant3Dresource.HasPrevious, pcp);
			// and last take care of sizechange / offset points
			if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
				IEntity sccp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);
				sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
				sccp.addStatement(ProcessResource.plant3Dresource.HasNext, newCP);
			}
			if (newCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
				IEntity ocp = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
				ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
				ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, pcp);
			}
		} else {
			// if direction is previous, user must have given sizechange
			assert(!pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint));
			// previous direction is more complicated, since if newCP is SizeChangeControlPoint,
			// we must link pcp to newCP's OffsetPoint
			IEntity nocp = null;
			if (newCP.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
				nocp = newCP.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
				nocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
				nocp.addStatement(ProcessResource.plant3Dresource.HasNext, pcp);
			}
			pcp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
			if (nocp == null)
				pcp.addStatement(ProcessResource.plant3Dresource.HasPrevious, newCP);
			else
				pcp.addStatement(ProcessResource.plant3Dresource.HasPrevious, nocp);
			
			newCP.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
			newCP.addStatement(ProcessResource.plant3Dresource.HasNext, pcp);
			if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
				IEntity ocp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
				ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
				if (nocp == null)
					ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, newCP);
				else
					ocp.addStatement(ProcessResource.plant3Dresource.HasPrevious, nocp);
			}
			
		}
	}
	
	/**
	 * Returns path legs direction
	 * @param pcp
	 * @param direction
	 * @return
	 */
	public static Vector3d getPathLegDirection(IEntity pcp,Direction direction) {
		IEntity previous = pcp.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasPrevious);
		IEntity next = pcp.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasNext);
		if (direction == Direction.NEXT) {
			if (next != null) {
				if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint))
					pcp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasSubPoint);
				Point3d p1 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
				Point3d p2 = G3DTools.getPoint(next.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
				Vector3d v = new Vector3d();
				v.sub(p2, p1);
				return v;
			} else {
				if (pcp.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint))
					throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + pcp.getResource());
				if (previous == null) {
					if (!pcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint))
						throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + pcp.getResource());
					else
						return getDirectedControlPointDirection(pcp);
				} else {
					if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {
						Point3d p1 = G3DTools.getPoint(previous.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
						Point3d p2 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
						Vector3d v = new Vector3d();
						v.sub(p2, p1);
						return v;
					}
					throw new RuntimeException("Missing implementation");
				}
			}
		} else {
			if (previous != null) {
				if (pcp.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint))
					pcp = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.SubPointOf);
				Point3d p1 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
				Point3d p2 = G3DTools.getPoint(previous.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
				Vector3d v = new Vector3d();
				v.sub(p2, p1);
				return v;
			} else {
				if (pcp.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint))
					throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + pcp.getResource());
				if (next == null) {
					if (!pcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint))
						throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + pcp.getResource());
					else {
						Vector3d v = getDirectedControlPointDirection(pcp);
						v.negate();
						return v;
					}
				} else {
					if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {
						Point3d p1 = G3DTools.getPoint(next.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
						Point3d p2 = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
						Vector3d v = new Vector3d();
						v.sub(p2, p1);
						return v;
					}
					throw new RuntimeException("Missing implementation");
				}
			}
		}
		
	}
	
	/**
	 * Return positions (world) of an InlineComponents ends
	 * @param pcp
	 * @param p1
	 * @param p2
	 */
	public static void getInlineControlPointEnds(IEntity pcp, Point3d p1, Point3d p2) {
		assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));
		
		Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
		Vector3d dir = ControlPointTools.getPathLegDirection(pcp, Direction.NEXT);
		dir.normalize();
		double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);
		dir.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos);
		p1.add(dir);
		p2.sub(dir);
	}
	
	/**
	 * Return positions (world) of an InlineComponents ends
	 * @param pcp
	 * @param p1
	 * @param p2
	 */
	public static void getInlineControlPointEnds(IEntity pcp, Point3d p1, Point3d p2, Vector3d dir) {
		assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));
		
		Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
		dir.set(ControlPointTools.getPathLegDirection(pcp, Direction.NEXT));
		double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);
		Vector3d d = new Vector3d(dir);
		d.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos);
		p1.add(d);
		p2.sub(d);
	}
	
	/**
	 * Return positions (world) of an InlineComponents ends
	 * @param pcp
	 * @param p1
	 * @param p2
	 */
	public static void getInlineControlPointEnds(IEntity pcp, Point3d center, Point3d p1, Point3d p2, Vector3d dir) {
		assert (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint));
		
		Point3d pos = G3DTools.getPoint(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition));
		center.set(pos);
		dir.set(ControlPointTools.getPathLegDirection(pcp, Direction.NEXT));
		double length = pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);
		Vector3d d = new Vector3d(dir);
		d.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos);
		p1.add(d);
		p2.sub(d);
	}
	
	public static double getInlineLength(IEntity pcp) {
    	double l2 = 0.0;
    	if (pcp.isInstanceOf(ProcessResource.plant3Dresource.TurnControlPoint)) {
    		l2 += pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);
    	} else if (pcp.isInstanceOf(ProcessResource.plant3Dresource.InlineControlPoint)) {
    		l2 += pcp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength) * 0.5;
    	}
    	return l2;
    }
	
	/**
	 * Returns position (world) of a single port
	 * @param pcp
	 * @param type
	 * @return
	 */
	public static Point3d getRealPosition(IEntity pcp, PositionType type) {
		PipeControlPoint p= new PipeControlPoint(pcp);
		Point3d pos = G3DTools.getPoint(p.getWorldPosition());
		switch (type) {
		case NEXT: {
			Vector3d dir = ControlPointTools.getPathLegDirection(pcp,Direction.NEXT);
			double length = getInlineLength(pcp);
			dir.normalize();
			dir.scale(length);
			pos.add(dir);
			break;
		}
		case PREVIOUS: {
			Vector3d dir = ControlPointTools.getPathLegDirection(pcp,Direction.PREVIOUS);
			double length = getInlineLength(pcp);
			dir.normalize();
			dir.scale(length);
			pos.add(dir);
			break;
		}
		case PORT:
			// IEntity portDir = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.HasDirection);
			// TODO : how we calculated needed space for a port; does it has an offset from control point's position or not?
			break;
		case SPLIT:
			// do nothing
			break;
			
		}
		return pos;
	}
	
	public static void getInlineMovement(PipeControlPoint pcp, Point3d start, Point3d end) {
		// FIXME : check type of neighbor components and allow movement on top of variable length components,
		//         ffind proper range for movement (pcp's position is not)
		PipeControlPoint prev = pcp.getPrevious().getPrevious();
		PipeControlPoint next = pcp.getNext().getNext();
		start.set(G3DTools.getPoint(prev.getWorldPosition()));
		end.set(G3DTools.getPoint(next.getWorldPosition()));
	}
	
	public static AxisAngle4d getControlPointLocalRotation(IEntity pcp, double angle) {
		Quat4d q1 = getControlPointLocalOrientationQuat(pcp, angle);
        AxisAngle4d aa= new AxisAngle4d();
		aa.set(q1);
		return aa;
	}
	
	public static AxisAngle4d getControlPointWorldRotation(IEntity pcp, double angle) {
		Quat4d q1 = getControlPointWorldOrientationQuat(pcp, angle);
        AxisAngle4d aa= new AxisAngle4d();
		aa.set(q1);
		return aa;
	}
	

    public static Quat4d getControlPointLocalOrientationQuat(IEntity pcp, double angle) {
    	return getControlPointLocalOrientationQuat(pcp, angle, false);
    }
    
    public static Quat4d getControlPointWorldOrientationQuat(IEntity pcp, double angle) {
    	return getControlPointWorldOrientationQuat(pcp, angle, false);
    }
    
    public static Quat4d getControlPointLocalOrientationQuat(IEntity cp, double angle, boolean offset) {
    	PipeControlPoint pcp = new PipeControlPoint(cp);
    	PipeControlPoint next;
    	// if pcp is size change control point with offset, next control point is not in line with previous and pcp control point (it's offsetted)
    	// else it's more numerically stable to use next control point
    	if (offset)
    		next = pcp;
    	else
    		next = pcp.getNext();
    	
    	PipeControlPoint prev = pcp.getPrevious();
        assert (next != null || prev != null);
        if (prev == null)
        	prev = pcp;
        else if (next == null)
        	next = pcp;
        // TODO : check correct type
        if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint) && pcp.getPrevious() == null) {
        	PipeControlPoint temp = next;
        	next = prev;
        	prev = temp;
        }
        Vector3d current = new Vector3d(G3DTools.getPoint(next.getLocalPosition()));
        current.sub(G3DTools.getPoint(prev.getLocalPosition()));
        current.normalize();
        return getControlPointOrientationQuat(current, angle);
    }
    
    public static Quat4d getControlPointWorldOrientationQuat(IEntity cp, double angle, boolean offset) {
    	PipeControlPoint pcp = new PipeControlPoint(cp);
    	PipeControlPoint next;
    	// if pcp is size change control point with offset, next control point is not in line with previous and pcp control point (it's offsetted)
    	// else it's more numerically stable to use next control point
    	if (offset)
    		next = pcp;
    	else
    		next = pcp.getNext();
    	
    	PipeControlPoint prev = pcp.getPrevious();
        assert (next != null || prev != null);
        if (prev == null)
        	prev = pcp;
        else if (next == null)
        	next = pcp;
        // TODO : check correct type
        if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint) && pcp.getPrevious() == null) {
        	PipeControlPoint temp = next;
        	next = prev;
        	prev = temp;
        }
        Vector3d current = new Vector3d(G3DTools.getPoint(next.getWorldPosition()));
        current.sub(G3DTools.getPoint(prev.getWorldPosition()));
        current.normalize();
        return getControlPointOrientationQuat(current, angle);
    }
    
    public static Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {

        
        final Vector3d front = new Vector3d(1.0,0.0,0.0);
        
        Quat4d q1 = new Quat4d();

		Vector3d up = new Vector3d(0.0, 1.0, 0.0);
		double a = up.angle(dir);
		if (a < 0.1 || (Math.PI - a) < 0.1) {
			up.set(1.0, 0.0, 0.0);
		}

		Vector3d right = new Vector3d();

		right.cross(dir, up);
		up.cross(right, dir);
		right.normalize();
		up.normalize();

		Matrix3d m = new Matrix3d();
		m.m00 = dir.x;
		m.m10 = dir.y;
		m.m20 = dir.z;
		m.m01 = up.x;
		m.m11 = up.y;
		m.m21 = up.z;
		m.m02 = right.x;
		m.m12 = right.y;
		m.m22 = right.z;

		//q1.set(m); MathTools contains more stable conversion
		MathTools.getQuat(m, q1);

		if (DEBUG) System.out.println("PipingTools.getPipeComponentOrientationQuat() " + dir+ " " + up + " " + right);

		Quat4d q2 = new Quat4d();
		q2.set(new AxisAngle4d(front, angle));
		q1.mul(q2);
		return q1;
    }
    
    public static PipeControlPoint findPreviousEnd(PipeControlPoint tcp) {
    	// TODO : optimize (we do not need the list here)
    	ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();
        return findPreviousEnd(tcp, t);
    }
    
    public static PipeControlPoint findNextEnd(PipeControlPoint tcp) {
        // TODO : optimize (we do not need the list here)
    	ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();
        return findNextEnd(tcp, t);
    }
    
    /**
     * Returns pipe leg's end using "nextControlPoint" relation and collects control point in the given list. 
     * @param tcp
     * @param nextList
     * @return
     */
    public static PipeControlPoint findNextEnd(PipeControlPoint tcp, ArrayList<PipeControlPoint> nextList) {
    	if (DEBUG) System.out.print("PipingTools.findNextEnd " + tcp.getResource());
        while (true) {
            PipeControlPoint pcp = null;
            PipeControlPoint p = null;
            if (nextList.size() == 0)
                p = tcp;
                
            else
                p = nextList.get(nextList.size() - 1);

            pcp = p.getNext();
            if (pcp == null) {
            	pcp = p;
            	if (nextList.size() > 0)
            		nextList.remove(nextList.size() - 1);
            	if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");
            	return pcp;
            	//break;
            }
            if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {
            	if (DEBUG) System.out.println(" " + pcp.getResource());
                return pcp;
            } else {
                nextList.add(pcp);
                if (DEBUG) System.out.print(" " + pcp.getResource());
            }
        }
    }
    
   
    
    /**
     * Returns pipe leg's end using "previousControlPoint" relation and collects control point in the given list.
     * @param tcp
     * @param prevList
     * @return
     */
    public static PipeControlPoint findPreviousEnd(PipeControlPoint tcp, ArrayList<PipeControlPoint> prevList) {
    	if (DEBUG) System.out.print("PipingTools.findPreviousEnd " + tcp.getResource());
        while (true) {
            PipeControlPoint pcp = null;
            PipeControlPoint p = null;
            if (prevList.size() == 0)
                p = tcp;
                
            else
                p = prevList.get(prevList.size() - 1);

            pcp = p.getPrevious();
            if (pcp == null) {
            	pcp = p;
            	if (prevList.size() > 0)
            		prevList.remove(prevList.size() - 1);
            	if (DEBUG) System.out.println(" " + pcp.getResource() + " not full");
            	return pcp;
            }
            if (pcp.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {
            	if (DEBUG) System.out.println(" " + pcp.getResource());
                return pcp;
            } else {
            	prevList.add(pcp);
                if (DEBUG) System.out.print(" " + pcp.getResource());
            }
        }
    }
    
    public static PipeRun getPipeRun(PipeControlPoint pcp) {
    	return pcp.getControlPointOfPipeRun();
    }
    
    @Deprecated
    public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp) {
    	Quat4d q = getControlPointWorldOrientationQuat(sccp, sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle),true);
    	return getSizeChangeOffsetVector(sccp,q);
    }
    
    public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp, Vector3d dir) {
    	Quat4d q = getControlPointOrientationQuat(dir, sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle));
    	return getSizeChangeOffsetVector(sccp,q);
    }
    
    public static Vector3d getSizeChangeOffsetVector(PipeControlPoint sccp, Quat4d q) {
    	Vector3d v = new Vector3d(0.0,0.0,sccp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasOffset));
    	Vector3d offset = new Vector3d();
    	MathTools.rotate(q, v, offset);
    	return offset;
    }
    
    public static Vector3d getDirectedControlPointDirection(IEntity dcp) {
    	assert(dcp.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint));
    	AxisAngle4d worldR = G3DTools.getOrientation(dcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldOrientation));
    	Quat4d q = new Quat4d();
 		q.set(worldR);
 		Vector3d dir = new Vector3d();
 		MathTools.rotate(q, new Vector3d(1.0, 0.0, 0.0), dir);
 		dir.normalize();
 		return dir;
    }
    
    public static Vector3d getNozzleDirection(IEntity rotation) {
    	AxisAngle4d worldR = G3DTools.getOrientation(rotation);
        Quat4d q = new Quat4d();
		q.set(worldR);
		Vector3d dir = new Vector3d();
		MathTools.rotate(q, new Vector3d(1.0, 0.0, 0.0), dir);
		return dir;
    }
    
    public static Vector3d getNozzleDirection(PipeControlPoint dcp) {
    	return getNozzleDirection(dcp.getWorldOrientation());
    }

    public static void removeControlPoint(PipeControlPoint removed) {
    	if (DEBUG) System.out.println("PipingTools.removeControlPoint() controlpoint " + removed.getResource());//FIXME : offset + size change control points !
    	
    	// this code is not valid anymore.
    	
    	// different removing cases:
    	//
    	// 1. Point is SizeChangeControlPoint  (this is currently ok)
    	//    * remove offset point and component 
    	//    * do NOT link components together
    	//
    	// 2. Point is VariableLength
    	//    * check if its a branch (TODO : ontology support?)
    	//      * if so, also branch point in the other piperun may have to be removed
    	//        (we cannot move other components next to branch)
    	//      * if not, components next to removed one are moved next to each other
    	// 3. Other
    	//    * check if there is VariableLength on both sides,
    	//		* if so, those must be unified to a single Variable Length component.
    	//      * if not, components must be moved next to each other
    	//
    	// a) If removed Control Point is next to Nozzle and after the point is removed nozzle is not connected to anything
    	//    * nozzle's link to piperun's specs must be removed
    	//    
    	
    	
    	if (removed.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)||
    		removed.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
    		// sccp & ocp connect two pipeRuns to each other; when thoes are remove, pipeRuns are not connected to each other anymore.
    		removeDualPoint(removed);
    		return;
    	} else {
    		PipeControlPoint prev = removed.getPrevious();
            PipeControlPoint next = removed.getNext();
    		PipeRun pipeRun = getPipeRun(removed);
    		if (pipeRun == null)
    			return;
    		if (next == null && prev == null) {
				if (removed.isInstanceOf(ProcessResource.plant3Dresource.NozzleControlPoint)) {
					// Nozzle's control point does not need to be removed, only unlinked
					// TODO : what about component ports?
					PipingTools2.unlinkNozzleAndPiperun(removed.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOf), ControlPointTools.getPipeRun(removed));
					return;
				}
			} else {

				if (next != null && prev != null) {
					boolean link = true;
					if (next.isInstanceOf(ProcessResource.plant3Dresource.BranchEndControlPoint)){
						link = false;
						next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
						removeControlPoint(next);
					}
					if (prev.isInstanceOf(ProcessResource.plant3Dresource.BranchEndControlPoint)) {
						link = false;
						prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
						removeControlPoint(prev);
					}
					if (link && prev.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)&&
								next.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)) {
						link = false;
					}
					if (next.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
						PipeControlPoint sccp = next;
						PipeControlPoint ocp = sccp.getSubPoint().iterator().next();
						if (ocp == null) {
							ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource()+ " structure damaged, no offset control point",null);
							return;
						}
						if (link) {
							sccp.setPrevious(prev);
							ocp.setPrevious(prev);
						} else {
							sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
							ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
							
						}
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
					} else if (next.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource()+ " structure damaged, next control point is offset control point",null);
						return;
					} else if (next.getPrevious().getResource().equals(removed.getResource())) {
						if (link) {
							next.setPrevious(prev);
						} else {
							next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
						}
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
					} else {
						ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource()+ " structure damaged", null);
						return;
					}
					if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, previous control point is size change control point", null);
						return;
					} else if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
						PipeControlPoint ocp = prev;
						PipeControlPoint sccp = ocp.getSubPointOf();
						if (sccp == null) {
							ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, no size change control point",null);
							return;
						}
						if (link) {
							ocp.setNext(next);
							sccp.setNext(next);
						} else {
							ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
							sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
						}
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
					} else if (prev.getNext().getResource().equals(removed.getResource())) {
						if (link)
							prev.setNext(next);
						else
							prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
						removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
					} else {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged", null);
						return;
					}
					if (link) {
						if (next.isInstanceOf(ProcessResource.plant3Dresource.VariableLengthControlPoint) &&
							prev.isInstanceOf(ProcessResource.plant3Dresource.VariableLengthControlPoint)) {
							// we have to join them into single variable length component.
							removeControlPoint(prev);
						}
					}

					
				} else if (next != null) {
					if (next.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
						PipeControlPoint sccp = next;
						PipeControlPoint ocp = sccp.getSubPoint().iterator().next();
						if (ocp == null) {
							ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource()+ " structure damaged, no offset control point",null);
							return;
						}
						next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
						ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
					} else if (next.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, next control point is offset control point", null);
						return;
					} else if (next.getPrevious().getResource().equals(removed.getResource())) {
						next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
					} else {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged", null);
						return;
					}
					removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
				} else { //(prev != null)
					if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
						ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, previous control point is size change control point", null);
						return;
					} else if (prev.isInstanceOf(ProcessResource.plant3Dresource.DualSubControlPoint)) {
						PipeControlPoint ocp = prev;
						PipeControlPoint sccp = ocp.getSubPointOf();
						if (sccp == null) {
							ErrorLogger.defaultLogError("Removing PipeControlPoint " + removed.getResource() + " structure damaged, no size change control point", null);
							return;
						}
						prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
						sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
					} else if (prev.getNext().getResource().equals(removed.getResource())) {
						prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
					} else {
						ErrorLogger.defaultLogError("Removing PipeControlPoint "+ removed.getResource() + " structure damaged", null);
						return;
					}
					removed.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
				}

			}
			if (removed.getSubPoint().size() > 0 ) {
				removeSubPoints(removed);
			} else if (removed.getSubPointOf() != null) {
				removeParentPoint(removed);
			}
				
			removeComponents(removed);
			
			pipeRun.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, removed);
			if (pipeRun.getChild().size() == 0) {
				PipingTools2.removePipeRun(pipeRun);
			} 
			else if (pipeRun.getControlPoints().size() == 1) {
            	removeControlPoint(pipeRun.getControlPoints().iterator().next());
            }
		}
        
    }
    
    private static void removeDualPoint(PipeControlPoint removed) {
    	PipeControlPoint prev = removed.getPrevious();
        PipeControlPoint next = removed.getNext();
    	if (prev != null) {
    		prev.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
		}
		if (next != null)
			next.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
		PipeControlPoint ocp;
		PipeControlPoint sccp;
		// get sccp / ocp pair
		if (removed.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
			sccp = removed;
    		ocp = sccp.getSubPoint().iterator().next();
    		
		} else {
			ocp = removed;
    		sccp = ocp.getSubPointOf();
		}
		PipeRun p1 = getPipeRun(ocp);
		PipeRun p2 = getPipeRun(sccp);
		
		//  removes all components connected to control point
        
        removeComponents(ocp);
        
        removeComponents(sccp); 
        
        // remove control points from pipeRuns
        p1.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, ocp);
        p2.removeStatement(ProcessResource.plant3Dresource.HasControlPoints, sccp);

        // remove links to other control points
        ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);
        sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasNext);            
        ocp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);
        sccp.removeRelatedStatements(ProcessResource.plant3Dresource.HasPrevious);            
        
        // if pipeRuns contains no other components(control points), they can be removed too.
        if (p1.getControlPoints().size() == 0) {
        	PipingTools2.removePipeRun(p1);
        } else if (p1.getControlPoints().size() == 1) {
        	removeControlPoint(p1.getControlPoints().iterator().next());
        }
        if (p2.getControlPoints().size() == 0) {
        	PipingTools2.removePipeRun(p2);
        } else if (p2.getControlPoints().size() == 1) {
        	removeControlPoint(p2.getControlPoints().iterator().next());
        }
    }
    
    /**
     * Removes sub points of a point
     * @param removed
     * @throws TransactionException
     */
    private static void removeSubPoints(PipeControlPoint removed) {
    	// if control point is branch control point, all branch of points must be removed too
		Collection<PipeControlPoint> points = removed.getSubPoint();
		
		for (PipeControlPoint p : points) {
			removed.removeStatement(ProcessResource.plant3Dresource.HasSubPoint, p);
			removeControlPoint(p);
		}
    }
    
    /**
     * Removed point is a subpoint of something, 
     * @param removed
     */
    private static void removeParentPoint(PipeControlPoint removed)  {
    	throw new RuntimeException("Subpoints cannot be removed");
    	
    	// if control point is branch it has to be removed from branch control point
//		BranchEndControlPoint ecp = BranchEndControlPointFactory.create(removed);
//		BranchControlPoint bcp = null;
//		if (ecp.getBranchOfPointSet().size() == 1) {
//			bcp = ecp.getBranchOfPointSet().iterator().next();
//		}
//		if (bcp != null) {
//			bcp.getBranchPointSet().remove(ecp);
//			if (bcp.getBranchPointSet().size() == 0) {
//				// branch control point is not used and can be removed
//				removeControlPoint(bcp);
//			}
//		}
    }
 
    
    private static void removeComponents(PipeControlPoint pcp) {
    	IEntity component = pcp.getControlPointOf();
    	if (component != null) {
    		PipeRun p1 = getPipeRun(pcp);
    		p1.removeStatement(ProcessResource.g3dResource.HasChild, component);
    		component.removeRelatedStatements(ProcessResource.plant3Dresource.HasControlPoint);
    	}
    }
    
    private static void setStatement(IEntity subject, Resource relation, IEntity object) {
    	subject.removeRelatedStatements(relation);
    	subject.addStatement(relation, object);
    }
    
    
    public static void setWorldPosition(IEntity pcp, Tuple3d position) {
    	IEntity wp = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);
    	G3DTools.setTuple3(wp, position);
    	tt.propagateWorldTransformChange(tt.getParent(pcp), pcp);
    	
    }
    
    public static void setLocalPosition(IEntity pcp, Tuple3d position) {
    	G3DTools.setTuple3(pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition), position);
    	tt.propagateLocalTransformChange(tt.getParent(pcp), pcp);
    }
    
    public static void setWorldOrientation(IEntity node, AxisAngle4d orientation) {
		G3DTools.setOrientation(node.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldOrientation), orientation);
		tt.propagateWorldTransformChange(tt.getParent(node), node);
		
	}
	
	public static void setLocalOrientation(IEntity node, AxisAngle4d orientation) {
		G3DTools.setOrientation(node.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalOrientation), orientation);
		tt.propagateLocalTransformChange(tt.getParent(node), node);
	}
    
    private static boolean updatePosition(IEntity pcp) {
    	return tt.transformationUpdate(pcp);
    	
    	
    	// TODO : orientation is also needed, current code handles only position
    	// TODO : reuse the code in G3DTools!
    	/*
    	IEntity worldPosition = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);
    	IEntity localPosition = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition);
    	
    	
    	Tuple3d worldP = G3DTools.getPoint(worldPosition);
        Tuple3d localP = G3DTools.getPoint(localPosition);

        if (localP == null || worldP == null)
            return false;
        if (!isValid(worldP) && !isValid(localP))
        	return false;

        Tuple3d cachedWorldP = (Tuple3d) getProperty(worldPosition.getResource());
        Tuple3d cachedLocalP = (Tuple3d) getProperty(localPosition.getResource());

        if (DEBUG) System.out.println("PipeControlPoint changed " + worldP + " " + cachedWorldP + " " + localP + " " + cachedLocalP);
        boolean changed = false;

        IEntity parent = pcp.getSingleRelatedObject(ProcessResource.plant3Dresource.ControlPointOf);
        
        if (parent == null) {
        	if (DEBUG) System.out.println("PipeControlPoint changed, no parent node");
            return false;
        }

        if (cachedLocalP == null) {
            storeProperty(localPosition.getResource(), localP);
            Tuple3d p = G3DTools.getWorldFromLocal(parent, new Point3d(localP)); 
            storeProperty(worldPosition.getResource(), p);
            G3DTools.setTuple3(worldPosition, p);
            if (DEBUG) System.out.println("PipeControlPoint changed local " + worldP + " " + p);
            changed = true;
        } else {
            if (TransformationTools.changed(cachedLocalP, localP)) {
                storeProperty(localPosition.getResource(), localP);
                Tuple3d p = G3DTools.getWorldFromLocal(parent, new Point3d(localP));
                storeProperty(worldPosition.getResource(), p);
                G3DTools.setTuple3(worldPosition, p);
                if (DEBUG) System.out.println("PipeControlPoint changed local " + worldP + " " + localP);
                changed = true;
            }
            if (cachedWorldP == null) {
                storeProperty(worldPosition.getResource(), worldP);
                Tuple3d p = G3DTools.getLocalFromWorld(parent, new Point3d(worldP));
                G3DTools.setTuple3(localPosition, p);
                storeProperty(localPosition.getResource(), p);
                if (DEBUG) System.out.println("PipeControlPoint changed world " + worldP + " " + p);
                changed = true;
            } else {
                if (TransformationTools.changed(cachedWorldP, worldP)) {
                    storeProperty(worldPosition.getResource(), worldP);
                    Tuple3d p = G3DTools.getLocalFromWorld(parent, new Point3d(worldP));
                    G3DTools.setTuple3(localPosition, p);
                    storeProperty(localPosition.getResource(), p);
                    if (DEBUG) System.out.println("PipeControlPoint changed world " + worldP + " " + p);
                    changed = true;
                }
            }
        }
        return changed;
        //*/
    }
    
    static boolean isControlPointChanged(PipeControlPoint node) {
    	long id = node.getResource().getResourceId();
    	boolean changed = updatePosition(node);
        //if (!changed) {
        	if (node.isInstanceOf(ProcessResource.plant3Dresource.PathLegEndControlPoint)) {
        		if (node.isInstanceOf(ProcessResource.plant3Dresource.TurnControlPoint)) {
        			Pair<Long,Long> connected = (Pair<Long,Long>)getProperty(node.getResource().getResourceId());
                    PipeControlPoint next = node.getNext();
                    PipeControlPoint prev = node.getPrevious();
                    if ((next != null && prev != null) && (
                        connected == null || 
                       (connected.first == null && prev != null) ||
                       (connected.second == null && next != null) ||
                        !connected.first.equals(prev.getResource().getResourceId()) ||
                        !connected.second.equals(next.getResource().getResourceId()))) {
                    	storeProperty(id, new Pair<Long,Long>(prev.getResource().getResourceId(),next.getResource().getResourceId()));
                        changed = true;  
                    }
                    if (node.isInstanceOf(ProcessResource.plant3Dresource.VariableAngleTurnControlPoint)) {
                    	double r = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasTurnRadius);
                    	Double d = (Double)getProperty(id + ":turnradius");
                    	if (d == null || TransformationTools.changed(r, d)) {
                    		storeProperty(id + ":turnradius", r);
                    		changed = true;
                    	}
                    }
        		}
        		else if (node.isInstanceOf(ProcessResource.plant3Dresource.DirectedControlPoint)) {
        			Vector3d dir = ControlPointTools.getDirectedControlPointDirection(node);
        			Vector3d old = (Vector3d)getProperty(id + ":direction");
        			if (old == null || TransformationTools.changed(dir, old)) {
        				storeProperty(id + ":direction", dir);
        				changed = true;
        			}
        		}
        	} else { // InlineControlPoint
        		if (node.isInstanceOf(ProcessResource.plant3Dresource.FixedLengthControlPoint)) {
        			double length = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasLength);
        			Double d = (Double)getProperty(id + ":length");
        			if (d == null)
        				changed = true;
        			else
        				changed = changed || TransformationTools.changed(length, d);
        			
        			if (changed) {
        				storeProperty(id + ":length", length);
        				return true;
        			}
        		} else 
        		if (node.isInstanceOf(ProcessResource.plant3Dresource.DualInlineControlPoint)) {
        			
        			double angle = node.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasRotationAngle);
        			Double d = (Double)getProperty(id + ":rotationangle");
        			if (d == null)
        				changed = true;
        			else
        				changed = changed || TransformationTools.changed(angle, d);
        			if (changed) {
        				storeProperty(id + ":rotationangle", angle);
        				return true;
        			}
        			Collection<PipeControlPoint> subpoints = node.getSubPoint();
        			if (subpoints.size() != 1)
        				throw new RuntimeException("Current implementation assumes that size change components are dual conmnected");
        			PipeControlPoint ocp = subpoints.iterator().next();
        			if (node.isInstanceOf(ProcessResource.plant3Dresource.OffsettingPoint)) {
        				double offset = ocp.getSingleRelatedScalarDouble(ProcessResource.plant3Dresource.HasOffset);
        				d = (Double)getProperty(id +":offset");
        				if (d == null)
        					changed = true;
        				else
        					changed = TransformationTools.changed(offset, d);
        				if (changed) {
        					storeProperty(id+":offset", offset);
        					return true;
        				}
        			}
//        		} else if (node instanceof OffsetControlPoint) {
//        			OffsetControlPoint ocp = (OffsetControlPoint)node;
//        			//ocp.
//        		} else if (node instanceof BranchControlPoint) {
//        			BranchControlPoint bcp = (BranchControlPoint)node;
//        			int size = bcp.getBranchPointSet().size();
//        			Integer i = (Integer)getProperty(bcp.getResource().getId());
//        			if (i == null)
//        				changed =true;
//        			else
//        				changed = changed || i != size;
//        			if (changed) {
//        				storeProperty(bcp.getResource().getId(), size);
//        				return true;
//        			}
        		}
        	}
        //}
        
        return changed;
    }
    
    private static boolean isValid(Tuple3d v) {
        if (Double.isInfinite(v.x) || 
                 Double.isNaN(v.x) ||
            Double.isInfinite(v.y) || 
                 Double.isNaN(v.y) ||
            Double.isInfinite(v.z) || 
                 Double.isNaN(v.z))
            return false;
        return true;
    }
    
    private static HashMap<Object, Object> properties = new HashMap<Object, Object>();
    
    public static Object getProperty(Object key) {
        return properties.get(key);
    }
    
    public static void storeProperty(Object key, Object value) {
        properties.put(key, value);
    }
    
    /**
     * Loads positions of controlpoint to rule cache
     * 
     * TODO : this caches only transformation information : other info must be cached too
     * 
     * @param root resource of the modeled plant
     */
    public static void reloadCache(Graph graph, Resource root) {
        
        Stack<IEntity> stack = new Stack<IEntity>();
        stack.add(EntityFactory.create(graph,root));
        while (!stack.isEmpty()) {
        	IEntity current = stack.pop();
        	IEntity pcp = current.getAtMostOneRelatedObject(ProcessResource.plant3Dresource.HasControlPoint);
        	if (pcp == null) {
        		stack.addAll(current.getRelatedObjects(ProcessResource.g3dResource.HasChild));
        	} else {
        		if (DEBUG) System.out.println("Cached pcp " + pcp.getResource());
        		IEntity localPos = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasLocalPosition);
        		IEntity worldPos = pcp.getSingleRelatedObject(ProcessResource.g3dResource.HasWorldPosition);
        		
                tt.storeProperty(localPos.getResource(),G3DTools.getPoint(localPos));
                tt.storeProperty(worldPos.getResource(),G3DTools.getPoint(worldPos));
                
                IEntity localOr = pcp.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasLocalOrientation);
        		IEntity worldOr = pcp.getAtMostOneRelatedObject(ProcessResource.g3dResource.HasWorldOrientation);
        		
        		if (worldOr != null) {
        			tt.storeProperty(localOr.getResource(),G3DTools.getOrientation(localOr));
                    tt.storeProperty(worldOr.getResource(),G3DTools.getOrientation(worldOr));
        		}
        		
                stack.addAll(pcp.getRelatedObjects(ProcessResource.plant3Dresource.HasSubPoint));
        	}
        }

    }
}