package org.simantics.plant3d.scenegraph.controlpoint;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.Objects;

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.g3d.math.MathTools;
import org.simantics.g3d.property.annotations.GetPropertyValue;
import org.simantics.g3d.scenegraph.G3DNode;
import org.simantics.g3d.scenegraph.base.INode;
import org.simantics.plant3d.scenegraph.IP3DNode;
import org.simantics.plant3d.scenegraph.Nozzle;
import org.simantics.plant3d.scenegraph.P3DRootNode;
import org.simantics.plant3d.scenegraph.PipeRun;
import org.simantics.plant3d.scenegraph.PipelineComponent;

import vtk.vtkRenderer;


public class PipeControlPoint extends G3DNode implements IP3DNode {

	private static boolean DEBUG = false;

	public enum PointType{INLINE,TURN,END};
	public enum Direction{NEXT,PREVIOUS};
	public enum PositionType {SPLIT,NEXT,PREVIOUS,PORT}

	private PipelineComponent component;

	private PointType type;
	private boolean isFixed = true;        // In-line: fixed-length Turn: fixed-angle
	private boolean isMod = false;         // Can user modify fixed value manually 
	private boolean isRotate = false;      // rotates around path leg axis.
	private boolean isReverse = false;     // definition direction can be swapped
	private boolean isDeletable = true;    // can be removed by rules
	private boolean isSizeChange = false;  // changes size of the pipe. The next control point / component is on different PipeRun
	private boolean isSub = false;         // child point for offset / size change

	private boolean disposed = false;
	
	public PipeControlPoint(PipelineComponent component) {
		this.component = component;
		if (component.getPipeRun() != null)
			component.getPipeRun().addChild(this);

	}

	public PipeControlPoint(PipelineComponent component, PipeRun piperun) {
		this.component = component;
		piperun.addChild(this);
	}

	@Override
	public void update(vtkRenderer ren) {
		try {
			PipingRules.requestUpdate(this);
		} catch (Exception e) {
			e.printStackTrace();
		}

	}

	public PipeRun getPipeRun() {
		return (PipeRun)getParent();
	}

	public PipelineComponent getPipelineComponent() {
		return component;
	}

	public PointType getType() {
		return type;
	}

	public void setType(PointType type) {
		this.type = type;
	}

	@GetPropertyValue(name="Fixed",tabId="Debug",value="fixed")
	public boolean isFixed() {
		return isFixed;
	}

	public void setFixed(boolean fixed) {
		this.isFixed = fixed;
	}
	
	@GetPropertyValue(name="Mod",tabId="Debug",value="mod")
	public boolean isMod() {
        return isMod;
    }
    
    public void setMod(boolean isMod) {
        this.isMod = isMod;
    }

	@GetPropertyValue(name="Rotate",tabId="Debug",value="rotate")
	public boolean isRotate() {
		return isRotate;
	}

	public void setRotate(boolean rotate) {
		this.isRotate = rotate;
	}

	@GetPropertyValue(name="Reverse",tabId="Debug",value="reverse")
	public boolean isReverse() {
		return isReverse;
	}

	public void setReverse(boolean reverse) {
		this.isReverse = reverse;
	}

	public void setSub(boolean sub) {
		this.isSub = sub;
	}

	@GetPropertyValue(name="Deletable",tabId="Debug",value="deletable")
	public boolean isDeletable() {
		return isDeletable;
	}

	public void setDeletable(boolean deletable) {
		this.isDeletable = deletable;
	}

	public boolean isPathLegEnd() {
		return type != PointType.INLINE;
	}

	public boolean isEnd() {
		return type == PointType.END;
	}

	public boolean isTurn() {
		return type == PointType.TURN;
	}

	public boolean isInline() {
		return type == PointType.INLINE;
	}
	
	public boolean asPathLegEnd() {
	    // Ends and Turns are path leg ends by default, but also unconnected inline are path leg ends.
	    return isPathLegEnd() || getNext() == null || getPrevious() == null;
	}

	/**
	 * True for end components, if control point defines absolute position direction, which rules cannot modify. 
	 * This is typical for nozzles.
	 * @return
	 */
	public boolean isDirected() {
		return isFixed && isEnd();
	}

	/**
     * True for end components, if control is opposite to directed, and rules can modify position and orientation.
     * This is typical for caps, and other end components.
     * @return
     */
	public boolean isNonDirected() {
		return !isFixed && isEnd();
	}

	public boolean isVariableLength() {
		return !isFixed && isInline();
	}
	
	/**
	 * Fixed length in-line component is such that piping rules cannot modify the length.
	 * @return
	 */
	public boolean isFixedLength() {
        return isFixed && isInline();
    }

	public boolean isVariableAngle() {
		return !isFixed && isTurn();
	}
	
	/**
	 * Fixed angle turn component is such that piping rules cannot modify the angle.
	 * @return
	 */
	public boolean isFixedAngle() {
        return isFixed && isTurn();
    }
	
	/**
	 * Does the turn behave like fixed angle?
	 * For variable angle turns, the turn angle is defined by connected components, and without them, we must handle the component as fixed angle. 
	 * @return
	 */
	public boolean asFixedAngle() {
        return isTurn() && (isFixed || next == null || previous == null);
    }

	public boolean isBranchEnd() {
		return isDeletable && isEnd();
	}

	public boolean isOffset() {
		return offset != null;
	}

	public boolean isDualSub() {
		return parent != null && isSub;
	}

	public boolean isDualInline() {
		return children.size() == 1 && children.get(0).isDualSub();
	}

	public boolean isAxial() {
		return isInline() && !isDualInline();
	}

	public boolean isSizeChange() {
		return isSizeChange;
		//		if (children.size() == 0)
		//			return false;
		//		if (!isDualInline())
		//			return false;
		//		return getPipeRun() != children.get(0).getPipeRun();
	}

	public void setSizeChange(boolean isSizeChange) {
		this.isSizeChange = isSizeChange;
	}


	private PipeControlPoint next;
	private PipeControlPoint previous;

	public PipeControlPoint getNext() {
		return next;
	}

	public PipeControlPoint getPrevious() {
		return previous;
	}

	public void setNext(PipeControlPoint next) {
	    if (isSub) {
	        getParentPoint().setNext(next);
	        return;
	    }
	    if (next != null && next.isDualSub())
	        next = next.parent;
	    if (_setNext(next)) {
	        for (PipeControlPoint pcp : children) {
                if (pcp.isSub)
                    pcp._setNext(next);
            }
	        updateSubPoint();
	    }
	}
	
	public void setPrevious(PipeControlPoint prev) {
        if (isSub) {
            getParentPoint().setPrevious(prev);
            return;
        }
        if (prev != null && prev.isDualInline())
            prev = prev.children.get(0);
        if (_setPrevious(prev)) {
            for (PipeControlPoint pcp : children) {
                if (pcp.isSub)
                    pcp._setPrevious(prev);
            }
            updateSubPoint();
        }
    }
	
	protected boolean _setNext(PipeControlPoint next) {
		if (isEnd() && previous != null && next != null)
			throw new RuntimeException("End control points are allowed to have only one connection");
		if (next == this)
			throw new RuntimeException("Cannot connect to self");
		if (this.next == next)
			return false;
		if (DEBUG) System.out.println(this + " next " + next);
		if (next == null && isVariableAngle() && previous != null && !isRemoved()) {
		    convertVariableAngleToFixed(Direction.NEXT);
		}
		this.next = next;
		if (component != null) {
			if (parent == null || isSub)
				component.setNext(next != null ? next.component : null);
			else
				component.setBranch0(next != null ? next.component : null);
			
		}
		return true;
	}

	protected boolean _setPrevious(PipeControlPoint previous) {
		if (isEnd() && next != null && previous != null)
			throw new RuntimeException("End control points are allowed to have only one connection");
		if (previous == this)
			throw new RuntimeException("Cannot connect to self");
		if (this.previous == previous)
			return false;
		if (DEBUG) System.out.println(this + " previous " + previous);
		if (previous == null && isVariableAngle() && next != null && !isRemoved()) {
            convertVariableAngleToFixed(Direction.PREVIOUS);
        }
		this.previous = previous;
		if (component != null) {
			if (parent == null || isSub)
				component.setPrevious(previous != null ? previous.component : null);
			else
				component.setBranch0(previous != null ? previous.component : null);
			updateSubPoint();
		}
		return true;
	}
	
	private void convertVariableAngleToFixed(Direction direction) {
	    // We are removing reference, which transforms variable angle to fixed angle.
        // Since fixed angle is defined differently, we need to calculate fixed angle parameters based on current data
        // We need to calculate turnAngle and rotationAngle
	    Vector3d dirOut = getPathLegDirection(direction == Direction.NEXT ? Direction.NEXT : Direction.PREVIOUS);
        Vector3d dir = getPathLegDirection(direction == Direction.NEXT ? Direction.PREVIOUS : Direction.NEXT);
        if (dir == null || dirOut == null)
            return;
        dir.negate();
        double angle = dir.angle(dirOut);
        //super._setNext(null);
        if (direction == Direction.NEXT)
            next = null;
        else
            previous = null;
        setRotationAngle(0.0);
        setReversed(direction == Direction.NEXT ? false : true);
        Vector3d dirOutN = getPathLegDirection(direction == Direction.NEXT ? Direction.NEXT : Direction.PREVIOUS);
        dirOutN.normalize();
        AxisAngle4d aa = new AxisAngle4d();
        if (MathTools.createRotation(dirOutN, dirOut, dir, aa)) {
            setRotationAngle(aa.angle);
            setTurnAngle(angle);
            if (DEBUG) System.out.println("convertToFixed " + dir + " " + dirOut + " " +dirOutN + " " +angle + " "+ aa.angle);
        }
	}

	public PipeControlPoint parent;
	public List<PipeControlPoint> children = new ArrayList<PipeControlPoint>();

	public List<PipeControlPoint> getChildPoints() {
		return children;
	}

	public PipeControlPoint getParentPoint() {
		return parent;
	}


	private double length;
	private Double turnAngle;
	private Vector3d turnAxis;

	private Double offset;
	private Double rotationAngle;
	private Boolean reversed;

	@GetPropertyValue(name="Length",tabId="Debug",value="length")
	public double getLength() {
		return length;
	}

	public void setLength(double l) {
		if (this.length == l)
			return;
		if (Double.isInfinite(l) || Double.isNaN(l))
			return;
		if (Math.abs(this.length-l) < MathTools.NEAR_ZERO)
			return;
		this.length = l;
		firePropertyChanged("length");
		if (isDualInline())
		    getDualSub().setLength(l);
	}

	@GetPropertyValue(name="Turn Angle",tabId="Debug",value="turnAngle")
	public Double getTurnAngle() {
		return turnAngle;
	}

	@GetPropertyValue(name="Turn Axis",tabId="Debug",value="turnAxis")
	public Vector3d getTurnAxis() {
		return turnAxis;
	}

	@GetPropertyValue(name="Offset",tabId="Debug",value="offset")
	public Double getOffset() {
		return offset;
	}

	@GetPropertyValue(name="Rotation Angle",tabId="Debug",value="rotationAngle")
	public Double getRotationAngle() {
	    if (isRotate || asFixedAngle())
	        return rotationAngle;
	    return null;
	}

	@GetPropertyValue(name="Reversed",tabId="Debug",value="reversed")
	public Boolean getReversed() {
		return reversed;
	}

	public boolean _getReversed() {
		if (reversed == null)
			return false;
		return reversed;
	}

	public void setTurnAngle(Double turnAngle) {
		if (turnAngle == null || Double.isInfinite(turnAngle) || Double.isNaN(turnAngle)) {
			return;
		}
		if (this.turnAngle != null && Math.abs(this.turnAngle-turnAngle) < MathTools.NEAR_ZERO)
			return;
		if (Objects.equals(this.turnAngle, turnAngle))
			return;
		this.turnAngle = turnAngle;
		firePropertyChanged("turnAngle");
	}

	public void setTurnAxis(Vector3d turnAxis) {
		if (this.turnAxis != null && MathTools.equals(turnAxis, this.turnAxis))
			return;
		this.turnAxis = turnAxis;
		firePropertyChanged("turnAxis");
	}

	public void setOffset(Double offset) {
		if (Double.isInfinite(offset) || Double.isNaN(offset)) {
			return;
		}
		if (this.offset != null && Math.abs(this.offset-offset) < MathTools.NEAR_ZERO)
			return;
		if (Objects.equals(this.offset, offset))
			return;
		this.offset = offset;
		firePropertyChanged("offset");
	}

	public void setRotationAngle(Double rotationAngle) {
		if (Double.isInfinite(rotationAngle) || Double.isNaN(rotationAngle)) {
			return;
		}
		if (this.rotationAngle != null && Math.abs(this.rotationAngle-rotationAngle) < MathTools.NEAR_ZERO)
			return;
		if (Objects.equals(rotationAngle, rotationAngle))
			return;
		this.rotationAngle = rotationAngle;
		firePropertyChanged("rotationAngle");
	}

	public void setReversed(Boolean reversed) {
		if (this.reversed == reversed)
			return;
		this.reversed = reversed;
		firePropertyChanged("reversed");
	}

	public Vector3d getSizeChangeOffsetVector(Vector3d dir) {
		Quat4d q;
		if (rotationAngle == null)
			q = getControlPointOrientationQuat(dir, 0.0);
		else
			q = getControlPointOrientationQuat(dir, rotationAngle);
		Vector3d v = new Vector3d(0.0,offset,0.0);
		Vector3d offset = new Vector3d();
		MathTools.rotate(q, v, offset);
		return offset;
	}

	public Vector3d getSizeChangeOffsetVector() {
		Quat4d q;
		if (rotationAngle == null)
			q = getControlPointOrientationQuat(0.0);
		else
			q = getControlPointOrientationQuat(rotationAngle);
		Vector3d v = new Vector3d(0.0,offset,0.0);
		Vector3d offset = new Vector3d();
		MathTools.rotate(q, v, offset);
		return offset;
	}

	@GetPropertyValue(name="Next",tabId="Debug",value="next")
	private String getNextString() {
		if (next == null)
			return null;
		return next.toString();
	}

	@GetPropertyValue(name="Previous",tabId="Debug",value="previous")
	private String getPrevString() {
		if (previous == null)
			return null;
		return previous.toString();
	}

	@GetPropertyValue(name="Sub",tabId="Debug",value="sub")
	private String getSubString() {
		if (children.size() == 0)
			return "";
		return Arrays.toString(children.toArray());
	}

	@GetPropertyValue(name="Type",tabId="Debug",value="type")
	public String getTypeString() {
		return type.name();
	}

	public Vector3d getPathLegEndpointVector() {
		PipeControlPoint a = findPreviousEnd();
		PipeControlPoint b = findNextEnd();
		
		if (a == null || b == null) {
			return getPathLegDirection();
		}
		
		Vector3d p1 = a.getWorldPosition();
		Vector3d p2 = b.getWorldPosition();
		p2.sub(p1);
		double l = p2.length();
		if (l != 0.0) {
			p2.scale(1.0 / l);
			return p2;
		}
		else {
			return getPathLegDirection();
		}
	}

	public Vector3d getPathLegDirection() {
		if (turnAxis == null) {
			return getPathLegDirection(Direction.NEXT);
		} else {
			Vector3d dir = getPathLegDirection(Direction.PREVIOUS);
			if (dir != null) dir.negate();
			return dir;
		}
	}
	
	public Quat4d getControlPointOrientationQuat(double angle) {
		Vector3d dir = getPathLegDirection();
		if (turnAxis == null) {
			return getControlPointOrientationQuat(dir, angle);
		} else {
			return getControlPointOrientationQuat(dir, turnAxis, angle);
		}
	}
	
	public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle, boolean reversed) {
	    if (turnAxis == null) {
            if (dir.lengthSquared() > MathTools.NEAR_ZERO)
                dir.normalize();
            Quat4d q =  getControlPointOrientationQuat(dir, angle);
            if (reversed) {
                Quat4d q2 = new Quat4d();
                q2.set(new AxisAngle4d(MathTools.Y_AXIS, Math.PI));
                q.mulInverse(q2);
            }
            return q;
        } else {
            if (dir.lengthSquared() > MathTools.NEAR_ZERO)
                dir.normalize();
            return getControlPointOrientationQuat(dir, turnAxis, angle);
        }
	}

	public Quat4d getControlPointOrientationQuat(double angle, boolean reversed) {
		Vector3d dir = getPathLegDirection();
		return getControlPointOrientationQuat(dir, angle, reversed);
	}

	public Quat4d getControlPointOrientationQuat(Vector3d dir, double angle) {
		if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
			return MathTools.getIdentityQuat();

		final P3DRootNode root = getRoot();
		Vector3d up = root != null ? new Vector3d(root.getUpVector()) : new Vector3d(0.0, 1.0, 0.0);
		final Vector3d legDir = getPathLegEndpointVector();
		double a = up.angle(legDir);
		if (a < 0.1 || (Math.PI - a) < 0.1) {
			// Rotate components
			up.set(up.getY(), up.getZ(), up.getX());
		}
		
		// Project up vector into a normal of the leg direction before applying to 'dir'
		MathTools.mad(up, legDir, -legDir.dot(up)/legDir.lengthSquared());
		up.normalize();

		return getControlPointOrientationQuat(dir, up, angle);
	}

	public P3DRootNode getRoot() {
		INode n = getParent();
		while (n != null && !(n instanceof P3DRootNode))
			n = n.getParent();
		return (P3DRootNode) n;
	}

	public static Quat4d getControlPointOrientationQuat(Vector3d dir, Vector3d up,  double angle) {
		if (dir == null || dir.lengthSquared() < MathTools.NEAR_ZERO)
			return MathTools.getIdentityQuat();

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

		Quat4d q1 = new Quat4d();


		Vector3d right = new Vector3d();
		
		up = new Vector3d(up);
		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 void insert(PipeControlPoint previous, PipeControlPoint next) {
		// inserting an offsetpoint is error, 
		if (isDualSub())
			throw new RuntimeException("Dual sub points cannot be inserted.");
		// size change control point cannot be inserted this way, because it ends PipeRun
//		if (isSizeChange())
//			throw new RuntimeException("Size change points cannot be inserted.");
		PipeRun piperun = previous.getPipeRun();
		// and just to make sure that control point structure is not corrupted
		if (getPipeRun() != null) {
			if (piperun != getPipeRun() || piperun != next.getPipeRun())
				throw new RuntimeException("All controls points must be located on the same pipe run");
		} else {
			piperun.addChild(this);
		}

		// insert new BranchControlPoint between straight's control points
		PipeControlPoint previousNext = previous.getNext();
		PipeControlPoint previousPrevious = previous.getPrevious();

		PipeControlPoint offsetCP = null;
		if (isOffset()) {
			offsetCP = getDualSub();
		}
		if (previousNext != null && previousNext == next) {
			if (previous.isDualInline()) {
				throw new RuntimeException();
			}
			if (next.isDualSub()) {
				throw new RuntimeException();
			}
			previous.setNext(this);
			this.setPrevious(previous);
			if (previous.isDualSub()) {
				previous.getParentPoint().setNext(this);
			}
			this.setNext(next);

			if (offsetCP == null) {
				next.setPrevious(this);
			} else {
				next.setPrevious(offsetCP);
				offsetCP.setNext(next);
				offsetCP.setPrevious(previous);
			}

			if (next.isDualInline()) {
				next.getDualSub().setPrevious(this);
			}
		} else if (previousPrevious != null && previousPrevious == next) {
			// control point were given in reverse order 
			if (next.isDualInline())
				throw new RuntimeException();
			if (previous.isDualSub())
				throw new RuntimeException();

			this.setNext(previous);
			if (offsetCP == null) {
				previous.setNext(this);
			} else {
				previous.setPrevious(offsetCP);
				offsetCP.setNext(previous);
				offsetCP.setPrevious(next);
			}
			if (previous.isDualInline()) {
				previous.getDualSub().setPrevious(this);
			}
			this.setPrevious(next);
			next.setNext(this);
			if (next.isDualSub()) {
				next.getParentPoint().setNext(this);
			}

		} else {
			throw new RuntimeException();
		}	

		PipingRules.validate(piperun);
	}



	public void insert(PipeControlPoint pcp, Direction direction) {
		if (isDualSub())
			throw new RuntimeException();
		if (direction == Direction.NEXT) {
			// if direction is next, user must have given OffsetPoint
			if (pcp.isDualInline())
				throw new RuntimeException();
			// basic next/prev links
			pcp.setNext(this);
			this.setPrevious(pcp);
			// and last take care of sizechange / offset points
			if (pcp.isDualSub()) {
				pcp.getParentPoint().setNext(this);
			}
			if (isDualInline()) {
			    getDualSub().setPrevious(this);
			}
		} else {
			// if direction is previous, user must have given sizechange
			if (pcp.isDualSub())
				throw new RuntimeException();
			// previous direction is more complicated, since if newCP is SizeChangeControlPoint,
			// we must link pcp to newCP's OffsetPoint
			PipeControlPoint nocp = null;
			if (isDualInline()) {
				nocp = getDualSub();
				nocp.setNext(pcp);
			}
			if (nocp == null) {
				pcp.setPrevious(this);
			} else {
				pcp.setPrevious(nocp);
			}
			this.setNext(pcp);
			if (pcp.isDualInline()) {
				PipeControlPoint ocp = pcp.getDualSub();
				if (nocp == null)
					ocp.setPrevious(this);
				else
					ocp.setPrevious(nocp);
			}

		}
		PipingRules.validate(getPipeRun());
	}

	public Vector3d getDirectedControlPointDirection() {
		assert (isDirected());
		Vector3d dir = new Vector3d();
		MathTools.rotate(getWorldOrientation(), new Vector3d(1.0, 0.0, 0.0), dir);
		dir.normalize();
		return dir;
	}
	
	/**
	 * Returns direction vector. 
	 * 
	 * For directed control points, always returns outwards pointing vector.
	 * 
	 * @param direction
	 * @return normalized vector, or null
	 */
	public Vector3d getDirection(Direction direction) {
        if (isDirected())
            return getDirectedControlPointDirection();
        if (isTurn() && asFixedAngle()) {
            if (direction == Direction.NEXT) {
                if (previous != null) {
                    PipeControlPoint pcp = this;
                    Vector3d dir = new Vector3d();
                    dir.sub(pcp.getWorldPosition(),previous.getWorldPosition());
                    if (dir.lengthSquared() > MathTools.NEAR_ZERO)
                        dir.normalize();
                    else
                        return null;
                    Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
                    AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
                    Quat4d q2 = MathTools.getQuat(aa);
                    Vector3d v = new Vector3d(1.,0.,0.);
                    Vector3d offset = new Vector3d();
                    MathTools.rotate(q2, v, offset);
                    MathTools.rotate(q, offset, dir);
                    dir.normalize();
                    return dir;
                }
            } else {
                if (next != null) {
                    PipeControlPoint pcp = this;
                    Vector3d dir = new Vector3d();
                    dir.sub(next.getWorldPosition(),pcp.getWorldPosition());
                    if (dir.lengthSquared() > MathTools.NEAR_ZERO)
                        dir.normalize();
                    else
                        return null;
                    Quat4d q = getControlPointOrientationQuat(dir, pcp.getRotationAngle() != null ? pcp.getRotationAngle() : 0.0);
                    AxisAngle4d aa = new AxisAngle4d(MathTools.Y_AXIS,pcp.getTurnAngle() == null ? 0.0 : pcp.getTurnAngle());
                    Quat4d q2 = MathTools.getQuat(aa);
                    Vector3d v = new Vector3d(1.,0.,0.);
                    Vector3d offset = new Vector3d();
                    MathTools.rotate(q2, v, offset);
                    MathTools.rotate(q, offset, dir);
                    dir.normalize();
                    return dir;
                }
            }
        }
        return null;
    }

	/**
	 * Returns path leg direction of the control point.
	 * 
	 * This method differs from getDirection by also returning inward pointing vectors for directed control points.
	 * 
	 * @param direction
	 * @return
	 */
	public Vector3d getPathLegDirection(Direction direction) {
		if (direction == Direction.NEXT) {
			if (next != null) {
				PipeControlPoint pcp = this;
				if (pcp.isDualInline()) {
					pcp = pcp.getDualSub();
				}
				Vector3d v = new Vector3d();
				v.sub(next.getWorldPosition(),pcp.getWorldPosition());
				if (v.lengthSquared() > MathTools.NEAR_ZERO)
                    v.normalize();
                else
                    return null;
				return v;
			} else {
				if (previous == null) {
					if (!isDirected())
						throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + this);
					return getDirectedControlPointDirection();

				} else {
					if (isVariableAngle() && !asFixedAngle())
						throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + this);
					if (isInline()) {
						PipeControlPoint pcp = this;
						if (pcp.isDualSub()) {
							pcp = pcp.getParentPoint();
						}
						Vector3d v = new Vector3d();
						v.sub(pcp.getWorldPosition(),previous.getWorldPosition());
						if (v.lengthSquared() > MathTools.NEAR_ZERO)
	                        v.normalize();
						else
						    return null;
						return v;
					} else if (isDirected()) {
						return getDirectedControlPointDirection();
					} else if (isEnd()) {
						Vector3d v = new Vector3d();
						v.sub(getWorldPosition(),previous.getWorldPosition());
						if (v.lengthSquared() > MathTools.NEAR_ZERO)
                            v.normalize();
                        else
                            return null;
						return v;
					} else if (isTurn() && asFixedAngle() && !_getReversed()) {
						return getDirection(Direction.NEXT);
					}
					throw new RuntimeException("Missing implementation " + this);
				}
			}
		} else {
			if (previous != null) {
				PipeControlPoint pcp = this;
				if (isDualSub()) 
					pcp = getParentPoint();
				Vector3d v = new Vector3d();
				v.sub(previous.getWorldPosition(),pcp.getWorldPosition());
				if (v.lengthSquared() > MathTools.NEAR_ZERO)
                    v.normalize();
                else
                    return null;
				return v;
			} else {
				if (next == null)  {
					if (!isDirected())
						throw new RuntimeException("Cannot calculate path leg direction for unconnected control point " + this);
					Vector3d v = getDirectedControlPointDirection();
					v.negate();
					return v;
				} else {
					if (isVariableAngle() && !asFixedAngle())
						throw new RuntimeException("Cannot calculate path leg direction for unconnected variable angle control point " + this);
					if (isInline()) {
						PipeControlPoint pcp = this;
						if (pcp.isDualInline()) {
							pcp = pcp.getDualSub();
						}
						Vector3d v = new Vector3d();
						v.sub(pcp.getWorldPosition(),next.getWorldPosition());
						if (v.lengthSquared() > MathTools.NEAR_ZERO)
                            v.normalize();
                        else
                            return null;
						return v;
					} else if (isDirected()) {
						Vector3d v = getDirectedControlPointDirection();
						v.negate();
						return v;
					} else if (isEnd()) {
						Vector3d v = new Vector3d();
						v.sub(getWorldPosition(),next.getWorldPosition());
						if (v.lengthSquared() > MathTools.NEAR_ZERO)
                            v.normalize();
                        else
                            return null;
						return v;
					} else if (isTurn() && asFixedAngle() && _getReversed()) {
						return getDirection(Direction.PREVIOUS);
					}
					throw new RuntimeException("Missing implementation " + this);
				}
			}
		}
	}

	public void getInlineControlPointEnds(Tuple3d p1, Tuple3d p2) {
		assert (isInline());

		PipeControlPoint sub = isAxial() ? this : getDualSub();
		Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
		Vector3d dir = sub.getInlineDir();
		
		dir.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos2);
		p1.sub(dir);
		p2.add(dir);
	}

	public void getControlPointEnds(Tuple3d p1, Tuple3d p2) {
		PipeControlPoint sub = isAxial() || isDirected() || isTurn() ? this : getChildPoints().get(0);
		Vector3d pos = getWorldPosition(), pos2 = sub == this ? pos : sub.getWorldPosition();
		
		Vector3d dir1;
		Vector3d dir2;
		if (isInline()) {
			dir2 = getInlineDir();
			dir2.scale(length * 0.5);
			dir1 = new Vector3d(dir2);
			dir1.negate();
		} else {
			dir1 = getPathLegDirection(Direction.PREVIOUS);
			dir2 = sub.getPathLegDirection(Direction.NEXT);
			dir1.scale(length);
			dir2.scale(length);
		}
		p1.set(pos);
		p2.set(pos2);
		p1.add(dir1);
		p2.add(dir2);
	}

	/**
	 * Get both path leg directions, with (0,0,0) if no connection exists. The returned vectors are not normalized.
	 * 
	 * @param v1  Set to the direction towards the previous control point on output
	 * @param v2  Set to the direction towards the next control point on output
	 */
	public void getEndDirections(Tuple3d v1, Tuple3d v2) {
		PipeControlPoint sub = isAxial() ? this : getDualSub();
		
		Vector3d dir1 = getPathLegDirection(Direction.PREVIOUS);
		Vector3d dir2 = sub.getPathLegDirection(Direction.NEXT);
		
		if (dir1 != null)
			v1.set(dir1);
		else
			v1.set(0,0,0);
		
		if (dir2 != null)
			v2.set(dir2);
		else
			v2.set(0,0,0);
	}

	public void getInlineControlPointEnds(Tuple3d p1, Tuple3d p2, Vector3d dir) {
		assert (isInline());

		Vector3d pos = getWorldPosition();
		dir.set(getInlineDir());
		dir.normalize();
		dir.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos);
		p1.sub(dir);
		p2.add(dir);
	}

	public void getInlineControlPointEnds(Tuple3d center, Tuple3d p1, Tuple3d p2, Vector3d dir) {
		assert (isInline());

		Vector3d pos = getWorldPosition();
		center.set(pos);
		dir.set(getInlineDir());
		dir.normalize();
		dir.scale(length * 0.5);
		p1.set(pos);
		p2.set(pos);
		p1.sub(dir);
		p2.add(dir);
	}

	public Vector3d getInlineDir() {
		Vector3d dir = getPathLegDirection(Direction.NEXT);
		if (dir == null) {
			dir = getPathLegDirection(Direction.PREVIOUS);
			if (dir != null) {
				// Use reverse direction
				dir.scale(-1.0);
			} else {
				// Control point is not connected at all, use current orientation
				dir = new Vector3d(1,0,0);
				MathTools.rotate(getWorldOrientation(), dir, dir);
			}
		}
		return dir;
	}

	public double getInlineLength() {
		if (type == PointType.TURN)
			return length;
		else if (type == PointType.INLINE)
			return length * 0.5;
		return 0;
	}

	/**
	 * Return the position indicated by the argument. If the indicated direction is not connected, the
	 * control point's wolrd position is returned instead.
	 * 
	 * @param type  A selector for the position to be returned
	 * @return  The selected position
	 */
	public Vector3d getRealPosition(PositionType type) {
		Vector3d pos = getWorldPosition();
		switch (type) {
		case NEXT: {
			double length = getInlineLength();
			Vector3d dir;
			if (isInline()) {
				dir = getInlineDir();
			} else {
				dir = getPathLegDirection(Direction.NEXT);
			}
			dir.scale(length);
			pos.add(dir);
			break;
		}
		case PREVIOUS: {
			double length = getInlineLength();
			Vector3d dir;
			if (isInline()) {
				dir = getInlineDir();
				dir.negate();
			} else {
				dir = getPathLegDirection(Direction.PREVIOUS);
			}
			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 void getInlineMovement(Tuple3d start, Tuple3d end) {
		// FIXME : check type of neighbor components and allow movement on top of variable length components,
		//         find proper range for movement (pcp's position is not)
		PipeControlPoint p = previous.getPrevious();
		PipeControlPoint n = next.getNext();
		start.set(p.getWorldPosition());
		end.set(n.getWorldPosition());
	}

	public PipeControlPoint findNextEnd() {
		ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();
		return findNextEnd( t);
	}

	public PipeControlPoint findPreviousEnd() {
		ArrayList<PipeControlPoint> t = new ArrayList<PipeControlPoint>();
		return findPreviousEnd(t);
	}

	public PipeControlPoint findNextEnd(List<PipeControlPoint> nextList) {
		while (true) {
			PipeControlPoint pcp = null;
			PipeControlPoint p = null;
			if (nextList.size() == 0)
				p = this;

			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.isPathLegEnd()) {
				//if (DEBUG) System.out.println(" " + pcp.getResource());
				return pcp;
			} else {
				nextList.add(pcp);
				// if (DEBUG) System.out.print(" " + pcp.getResource());
			}
		}
	}

	public PipeControlPoint findPreviousEnd(List<PipeControlPoint> prevList) {
		while (true) {
			PipeControlPoint pcp = null;
			PipeControlPoint p = null;
			if (prevList.size() == 0)
				p = this;

			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.isPathLegEnd()) {
				//				if (DEBUG)	System.out.println(" " + pcp.getResource());
				return pcp;
			} else {
				prevList.add(pcp);
				//				if (DEBUG)System.out.print(" " + pcp.getResource());
			}
		}
	}
	
	public void _remove() {
	    _remove(true);
	}
	
	
	public PipeControlPoint getDualSub() {
	    if (isDualInline())
	        return getChildPoints().get(0);
	    else
	        throw new IllegalStateException("Current control point is not dual inline");
	}
	

	public void _remove(boolean renconnect) {
	    if (disposed)
	        return;
		
	    if (DEBUG) System.out.println(this + " Remove " + renconnect);

		if (getParentPoint() != null) {
		    getParentPoint()._remove(renconnect);
		    return;
		}
		PipeRun pipeRun = getPipeRun();
//		PipeRUn removal has been changed, so pipeRun may be null.
//		if (pipeRun == null)
//			return;

		PipeControlPoint additionalRemove = null;
		if (!PipingRules.isEnabled()) {
		    component = null;
			setPrevious(null);
			setNext(null);
		} else {

			PipeControlPoint currentPrev = previous;
			PipeControlPoint currentNext = next;
			if (currentNext == null && currentPrev == null) {
				removeComponent();
				if (pipeRun != null) {
				    pipeRun.remChild(this);
				    checkRemove(pipeRun);
				}
				return;
			}
			if (currentNext != null && currentPrev != null) {
				boolean link = renconnect;
				if (currentNext.isBranchEnd()) {
					link = false;
					currentNext.remove();
					currentNext = null;
					setNext(null);
				}
				if (currentPrev.isBranchEnd()) {
					link = false;
					currentPrev.remove();
					currentPrev = null;
					setPrevious(null);
				}
				if (link) {
				    if (currentPrev.isDirected() && currentNext.isDirected())
				        link = false;
				    else if (this.isDualSub()) {
				        throw new RuntimeException("_remove() is called for parent point, somehow got to child point. " + this);
				    }
				}
				if (currentNext == null) {
					// Nothing to do
				} else if (currentNext.isDualInline()) {
					PipeControlPoint sccp = currentNext;
					PipeControlPoint ocp = currentNext.getDualSub();
					if (ocp == null) {
						throw new RuntimeException("Removing PipeControlPoint " + this+ " structure damaged, no offset control point");
					}
					if (link) {
						sccp.setPrevious(currentPrev);
						//ocp.setPrevious(currentPrev);
						assert(ocp.getPrevious() == currentPrev);
					} else {
						sccp.setPrevious(null);
						//ocp.setPrevious(null);
						assert(ocp.getPrevious() == null);
					}
					setNext(null);
				} else if (currentNext.isDualSub()) {
					throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, next control point is offset control point");
				} else if (currentNext.previous == this) {
					if (link) {
						currentNext.setPrevious(currentPrev);
					} else {
						currentNext.setPrevious(null);
					}
					setNext(null);
				} else if (isDualInline()) {
				    if (currentNext.previous != getDualSub()) {
				        throw new RuntimeException("Removing PipeControlPoint "+ this+ " structure damaged");
				    }
				    if (link) {
                        currentNext.setPrevious(currentPrev);
                    } else {
                        currentNext.setPrevious(null);
                    }
                    setNext(null);
				} else {
					throw new RuntimeException("Removing PipeControlPoint "+ this+ " structure damaged");
				}
				if (currentPrev == null) {
					// Nothing to do
				} else if (currentPrev.isDualInline()) {
					throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, previous control point is size change control point");
				} else if (currentPrev.isDualSub()) {
					PipeControlPoint ocp = currentPrev;
					PipeControlPoint sccp = currentPrev.getParentPoint();
					if (sccp == null)
						throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, no size change control point");
					if (link) {
						//ocp.setNext(currentNext);
						sccp.setNext(currentNext);
						assert(ocp.getNext() == currentNext);
					} else {
						//ocp.setNext(null);
						sccp.setNext(null);
						assert(ocp.getNext() == null);
					}
					setPrevious(null);
				} else if (currentPrev.next == this) {
					if (link)
						currentPrev.setNext(currentNext);
					else
						currentPrev.setNext(null);

					setPrevious(null);
				} else {
					throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged");
				}
				if (link) {
					if (currentNext.isVariableLength() && currentPrev.isVariableLength()) {
						// we have to join them into single variable length component.
						additionalRemove = currentPrev;
						// combine lengths and set the location of remaining control point to the center.
						Point3d ps = new Point3d();
						Point3d pe = new Point3d();
						Point3d ns = new Point3d();
						Point3d ne = new Point3d();
						currentPrev.getInlineControlPointEnds(ps, pe);
						currentNext.getInlineControlPointEnds(ns, ne);
						double l = currentPrev.getLength() + currentNext.getLength();
						Vector3d cp = new Vector3d();
						cp.add(ps, ne);
						cp.scale(0.5);
						currentNext.setLength(l);
						currentNext.setWorldPosition(cp);
					}
				} else {
					// FIXME : pipe run must be split into two parts, since the control point structure is no more continuous. 
				}
			} else if (currentNext != null) {
				if (currentNext.isDualInline()) {
					PipeControlPoint sccp = currentNext;
					PipeControlPoint ocp = currentNext.getDualSub();
					if (ocp == null) {
						throw new RuntimeException("Removing PipeControlPoint " + this+ " structure damaged, no offset control point");
					}
					sccp.setPrevious(null);
					assert(ocp.getPrevious() == null);
					//ocp.setPrevious(null);
				} else if (currentNext.isDualSub()) {
					throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, next control point is offset control point");
				} else if (currentNext.previous == this) {
				    currentNext.setPrevious(null);
				}  else if (isDualInline()) {
                    if (currentNext.previous != getDualSub()) {
                        throw new RuntimeException("Removing PipeControlPoint "+ this+ " structure damaged");
                    }
                    currentNext.setPrevious(null);
				} else {
					throw new RuntimeException("Removing PipeControlPoint "+ this+ " structure damaged");
				}
				setNext(null);
			} else {  //(previous != null)
				if(currentPrev.isDualInline()) {
					throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, previous control point is size change control point");
				} else if (currentPrev.isDualSub()) {
					PipeControlPoint ocp = currentPrev;
					PipeControlPoint sccp = currentPrev.getParentPoint();
					if (sccp == null) {
						throw new RuntimeException("Removing PipeControlPoint " + this + " structure damaged, no size change control point");
					}
					sccp.setNext(null);
					assert(ocp.getNext() == null);
				} else if (currentPrev.next == this) {
				    currentPrev.setNext(null);
				} else {
					throw new RuntimeException("Removing PipeControlPoint "+ this+ " structure damaged");
				}
				setPrevious(null);
			}
			if (children.size() > 0 ) {
				removeSubPoints();
			} else if (parent!= null) {
				removeParentPoint();
			}

		}

		removeComponent();
		if (pipeRun != null) {
    		pipeRun.remChild(this);
    		checkRemove(pipeRun);
    		if (PipingRules.isEnabled() && pipeRun.getParent() != null && pipeRun.getControlPoints().size() > 0)
    			PipingRules.validate(pipeRun);
		}
		if (additionalRemove != null)
			additionalRemove.remove();
		disposed = true;
	}

	/**
	 * Removes control point and attempts to reconnect next/prev
	 * 
	 * If this point is size change (PipeRuns are different on both sides), then reconnection cannot be made.
	 */
	public void remove() {
		PipeControlPoint currentPrev = previous;
		PipeControlPoint currentNext = next;
		_remove();
		try {
			if (currentNext != null)
			    if (!currentNext.checkRemove())
			        PipingRules.requestUpdate(currentNext);
			if (currentPrev != null)
			    if (!currentPrev.checkRemove())
			        PipingRules.requestUpdate(currentPrev);
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	
	/**
	 * Removes control point without attempting to reconnect next/prev.
	 * This usually leads to creation of another PipeRun for the control points after this point. 
	 */
	public void removeAndSplit() {
        PipeControlPoint currentPrev = previous;
        PipeControlPoint currentNext = next;
        
        if (next != null && previous != null) {
            P3DRootNode root = (P3DRootNode)getPipelineComponent().getRootNode();
            PipeRun nextPipeRun = new PipeRun();
            nextPipeRun.setName(root.getUniqueName("PipeRun"));
            root.addChild(nextPipeRun);
            
            PipeRun previousRun = previous.getPipeRun();
            nextPipeRun.setPipeDiameter(previousRun.getPipeDiameter());
            nextPipeRun.setTurnRadiusArray(previousRun.getTurnRadiusArray());
            
            PipelineComponent n = next.getPipelineComponent();
            while (n != null) {
                if (n.getPipeRun() != previousRun)
                    break;
                if (! (n instanceof Nozzle)) {
                    n.deattach();
                    nextPipeRun.addChild(n);
                } else
                    n.setPipeRun(nextPipeRun);
                n = n.getNext();
            }
        }
        _remove(false);
        try {
            if (currentNext != null)
                if (!currentNext.checkRemove())
                    PipingRules.requestUpdate(currentNext);
            if (currentPrev != null)
                if (!currentPrev.checkRemove())
                    PipingRules.requestUpdate(currentPrev);
        } catch (Exception e) {
            e.printStackTrace();
        }
    }
	
	/**
	 * This is called when adjacent control point is removed.
	 * 
	 * This call should remove the give point, if the point cannot exist alone. 
	 * At the moment there is one such case: branch.
	 * 
	 * @return
	 */
	protected boolean checkRemove() {
	    if (getParentPoint() != null) {
	        return getParentPoint().checkRemove();
	    } else {
	        if (getPipelineComponent() == null)
	            return true; // already removed
    	    if (getPipelineComponent().getType().equals("Plant3D.URIs.Builtin_BranchSplitComponent")) {
    	        if (getChildPoints().get(0).getNext() == null && getChildPoints().get(0).getPrevious() == null) {
        	        remove();
        	        return true;
    	        }
    	    }
    	    return checkRemove(getPipeRun());
	    }
    }

	private boolean checkRemove(PipeRun pipeRun) {
	    if (pipeRun == null)
	        return false;
		Collection<PipeControlPoint> points = pipeRun.getControlPoints();
		if (points.size() == 0) {
			pipeRun.remove();
			return true;
		} else if (points.size() == 1) {
			PipeControlPoint pcp = points.iterator().next();
			if (pcp.isDeletable() && pcp.getNext() == null && pcp.getPrevious() == null) {
				pcp._remove(); // This call will recursively call also this method...
				return true;
			}
		} else if (points.size() == 2) {
		    
		}
		return false;
	}

	private void removeSubPoints() {
		for (PipeControlPoint p : children) {
			p.parent = null;
			p.component = null;
			//p._remove();
			PipeControlPoint currentNext = p.getNext();
			PipeControlPoint currentPrev = p.getPrevious();
			p._setNext(null);
			p._setPrevious(null);
			PipeRun run = p.getPipeRun();
			if (run != null) {
			    run.remChild(p);
			    checkRemove(run);
			}
			if (currentNext != null)
                if (!currentNext.checkRemove())
                    PipingRules.requestUpdate(currentNext);
            if (currentPrev != null)
                if (!currentPrev.checkRemove())
                    PipingRules.requestUpdate(currentPrev);
            
		}
		children.clear();
	}

	private void removeParentPoint() {
		throw new RuntimeException("Child points cannot be removed directly");
	}
	
	public boolean isRemoved() {
	    return component == null;
	}

	private void removeComponent() {
		if (component == null)
			return;
		PipelineComponent next = component.getNext();
		PipelineComponent prev = component.getPrevious();
		PipelineComponent br0 = component.getBranch0();
		component.setNext(null);
		component.setPrevious(null);
		component.setBranch0(null);
		if (next != null) {
			if (next.getNext() == component)
				next.setNext(null);
			else if (next.getPrevious() == component)
				next.setPrevious(null);
			else if (next.getBranch0() == component)
				next.setBranch0(null);
		}
		if (prev != null) {
			if (prev.getNext() == component)
				prev.setNext(null);
			else if (prev.getPrevious() == component)
				prev.setPrevious(null);
			else if (prev.getBranch0() == component)
				prev.setBranch0(null);
		}
		if (br0 != null) {
			if (br0.getNext() == component)
				br0.setNext(null);
			else if (br0.getPrevious() == component)
				br0.setPrevious(null);
			else if (br0.getBranch0() == component)
				br0.setBranch0(null);
		}
		PipelineComponent comp = component;
		component = null;

		comp.remove();
	}

	@Override
	public void setOrientation(Quat4d orientation) {
		if (MathTools.equals(orientation, getOrientation()))
			return;
		if (getPipelineComponent() != null && (getPipelineComponent() instanceof Nozzle))
		    System.out.println();
		super.setOrientation(orientation);
		if (getParentPoint() == null && component != null)
			component._setWorldOrientation(getWorldOrientation());
		updateSubPoint();
	}

	@Override
	public void setPosition(Vector3d position) {
		if (MathTools.equals(position, getPosition()))
			return;
		if (Double.isNaN(position.x) || Double.isNaN(position.y) || Double.isNaN(position.z))
			throw new IllegalArgumentException("NaN is not supported");
		super.setPosition(position);
		if (getParentPoint() == null && component != null)
			component._setWorldPosition(getWorldPosition());
		updateSubPoint();
	}
	
	private void updateSubPoint() {
		if (isOffset()) {
			if (next == null && previous == null) {
				for (PipeControlPoint sub : getChildPoints()) {
					sub.setWorldPosition(getWorldPosition());
					sub.setWorldOrientation(getWorldOrientation());
				}
				return;
			}
			for (PipeControlPoint sub : getChildPoints()) {
				Vector3d wp = getWorldPosition();
				wp.add(getSizeChangeOffsetVector());
				sub.setWorldPosition(wp);
				sub.setWorldOrientation(getWorldOrientation());
			}
		} else {
			for (PipeControlPoint sub : getChildPoints()) {
				sub.setWorldPosition(getWorldPosition());
				sub.setWorldOrientation(getWorldOrientation());
			}
		}
	}


	public void _setWorldPosition(Vector3d position) {
		Vector3d localPos = getLocalPosition(position);
		super.setPosition(localPos);
		updateSubPoint();
	}

	public void _setWorldOrientation(Quat4d orientation) {
		Quat4d localOr = getLocalOrientation(orientation);
		super.setOrientation(localOr);
		updateSubPoint();
	}

	@Override
	public String toString() {
		return getClass().getName() + "@" + Integer.toHexString(hashCode());
	}

}