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

import java.util.ArrayList;
import java.util.List;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import org.simantics.db.Graph;
import org.simantics.db.GraphRequestWithResult;
import org.simantics.db.Resource;
import org.simantics.db.adaption.AdaptionException;
import org.simantics.proconf.g3d.Resources;
import org.simantics.utils.ui.ErrorLogger;

import com.jme.renderer.ColorRGBA;
import com.jme.scene.Geometry;
import com.jme.scene.Line;
import com.jme.scene.state.MaterialState;
import com.jme.util.geom.BufferUtils;

public class ConstraintDetector {
    
    private static final int X = 0;
    private static final int Y = 1;
    private static final int Z = 2;
    
    
    private ThreeDimensionalEditorBase editor;
    //private G3DNode constraintReference = null;
    private Resource constraintReference = null;
    private ArrayList<Point3d> constraintPoints = new ArrayList<Point3d>();
    private ArrayList<Vector3d> constraintDirections = new ArrayList<Vector3d>();
    private MaterialState ms;
    
    private ColorRGBA xColor = new ColorRGBA(1.f,0.f,0.f,1.f);
    private ColorRGBA yColor = new ColorRGBA(0.f,1.f,0.f,1.f);
    private ColorRGBA zColor = new ColorRGBA(0.f,0.f,1.f,1.f);
    
    
    public ConstraintDetector(ThreeDimensionalEditorBase editor) {
        this.editor = editor;
        ms = editor.getRenderingComponent().getDisplaySystem().getRenderer().createMaterialState();
        ms.setEmissive(new ColorRGBA(1.f,1.f,1.f,1.f));
        ms.setColorMaterial(MaterialState.CM_EMISSIVE);
    }
    

    public void clearConstraints() {
        //System.out.println("ConstraintDetector.clearConstraints()");
        constraintPoints.clear();
        constraintDirections.clear();
    }
    
    private void updateConstraints() {
        clearConstraints();
        if (constraintReference == null)
            return;
        GraphRequestWithResult<Constraint>  request = new GraphRequestWithResult<Constraint>() {
        	@Override
        	public Constraint performWithResult(Graph g) throws Exception {
        		try {
        			return g.adapt(constraintReference, Resources.g3dResource.HasConstraints);
        		} catch (AdaptionException e) {
        			ErrorLogger.defaultLogWarning("Cannot add constraint", e);
        			return null;
        		}
        	}
        };
        editor.getSession().syncRead(request);
        Constraint c = request.getResult();
        if (c == null)
        	return;
        constraintPoints.addAll(c.points);
        constraintDirections.addAll(c.dirs);
    }
    
    
    public ArrayList<Point3d> getConstraintPoints() {
        return constraintPoints;
    }
    
    public ArrayList<Vector3d> getConstraintDirections() {
        return constraintDirections;
    }

    public void updateConstraintReference() {
   
            Resource interactive = null;
            if (editor.getSelectionAdapter().getHighlightSelection().size() > 0) {
                interactive = editor.getSelectionAdapter().getInteractiveSelectedObjects().iterator().next().getResource();
                if (constraintReference == null) {
                    constraintReference = interactive;
                    updateConstraints();
                } else if (!constraintReference.getResource().equals(interactive.getResource())) {
                    constraintReference = interactive;
                    updateConstraints();
                }
            } else {
                constraintReference = null;
                updateConstraints();
            }

    }
    
    public void addContraintPoint(Point3d p) {
        //System.out.println("ConstraintDetector.addConstraintPoint() " + p);
        constraintPoints.add(p);
    }
    
    public void addContraintDirection(Vector3d v) {
        //System.out.println("ConstraintDetector.addConstraintDirection() " + v);
        constraintDirections.add(v);
    }
    
    private double snapAngle = 0.1;
    private String snapString = "";

    private ArrayList<Geometry> constraintHighlights = new ArrayList<Geometry>();
    
    public Point3d getSnappedPoint(Vector3d pickPoint, Vector3d pickDir, Vector3d requestedPoint) {
        
        
        Vector3d snappedPoint = new Vector3d();
        Vector3d t = new Vector3d();
        Point3d currentPoint = null;
        // TODO : snap to closest angle
        for (Vector3d constraintDir : constraintDirections) {
            
            MathTools.intersectStraightStraight(pickPoint,pickDir, requestedPoint, constraintDir, t, snappedPoint);
            t.sub(snappedPoint);
            if (t.lengthSquared() < snapAngle) {
                
                snapString += "Angle snap ";
                currentPoint = new Point3d(snappedPoint);
                break;
            }
        }
        if (currentPoint != null) {
            Vector3d dir = new Vector3d(currentPoint);
            dir.sub(requestedPoint);
            Point3d p = getPointSnap(requestedPoint, dir);
            if (p != null)
                currentPoint = p;
        } else {
        	List<Double> distances = new ArrayList<Double>();
        	List<Point3d> snapPoints = new ArrayList<Point3d>();
        	List<String> snapStrings = new ArrayList<String>();
        	List<ColorRGBA> snapColors = new ArrayList<ColorRGBA>();
            for (Point3d constraintPoint : constraintPoints) {
            	distances.clear();
            	snapPoints.clear();
            	snapStrings.clear();
                MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(1.0, 0.0, 0.0),
                        pickPoint, pickDir, snappedPoint, t);
                t.sub(snappedPoint);
                double distance = t.lengthSquared();
                if (distance < snapAngle) {
                	distances.add(distance);
                	snapPoints.add(new Point3d(snappedPoint));
                	snapStrings.add("Point x-snap ");
                	snapColors.add(xColor);
                }
                MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 1.0, 0.0),
                        pickPoint, pickDir, snappedPoint, t);
                t.sub(snappedPoint);
                distance = t.lengthSquared();
                if (distance < snapAngle) {
                	distances.add(distance);
                	snapPoints.add(new Point3d(snappedPoint));
                	snapStrings.add("Point y-snap ");
                	snapColors.add(yColor);
                }
                MathTools.intersectStraightStraight(new Vector3d(constraintPoint), new Vector3d(0.0, 0.0, 1.0),
                        pickPoint, pickDir, snappedPoint, t);
                t.sub(snappedPoint);
                distance = t.lengthSquared();
                if (distance < snapAngle) {
                	distances.add(distance);
                	snapPoints.add(new Point3d(snappedPoint));
                	snapStrings.add("Point z-snap ");
                	snapColors.add(zColor);
                    
                }
                if (distances.size() > 0) {
                    if (distances.size() > 1) {
                        // more than one axes snape
                    	Vector3d ref = MathTools.closestPointOnStraight(constraintPoint, new Point3d(pickPoint), pickDir);
                    	ref.sub(constraintPoint);
                    	distance = ref.lengthSquared();
                    	if (distance < snapAngle) {
                    		// we are close enought to point, so we'll just snap there
                    		currentPoint = new Point3d(constraintPoint);
                    		snapString += "Point snap ";
                    	} else {
                    		// select the closest of axes snap to
                    		int min = 0;
                    		for (int i = 1; i < distances.size(); i++) {
                    			if (distances.get(i) < distances.get(min))
                    				min = i;
                    		}
                    		currentPoint = snapPoints.get(min);
                    		addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(min));
                    		snapString += snapStrings.get(min);
                    	}
                    } else {
                    	// only one of the axes snaps
                    	currentPoint = snapPoints.get(0);
                    	addConstrainLineHighlight(currentPoint, constraintPoint,snapColors.get(0));
                    	snapString += snapStrings.get(0);
                    }
                    break;
                }
            }
        }
        return currentPoint;

    }
    
    public void clearConstraintHighlights() {
        snapString = "";

        for (Geometry s : constraintHighlights)
            s.removeFromParent();
        
        constraintHighlights.clear();
    }
    
    private void addConstrainLineHighlight(Point3d p1, Point3d p2, ColorRGBA color) {

    	float coord[] = new float[6];
    	ColorRGBA colors[] = new ColorRGBA[2];
    	colors[0] = color;
    	colors[1] = color;
    	coord[0] = (float)p1.x;
    	coord[1] = (float)p1.y;
    	coord[2] = (float)p1.z;
    	coord[3] = (float)p2.x;
    	coord[4] = (float)p2.y;
    	coord[5] = (float)p2.z;
    	Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);
    	editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);
    	shape.setRenderState(ms);
    	constraintHighlights.add(shape);
    }
    
    private void addConstrainPlaneHighlight(Point3d p1, Point3d p2, int axis) {

    	float coord[] = new float[9];
    	ColorRGBA colors[] = new ColorRGBA[3];
    	coord[0] = (float)p1.x;
    	coord[1] = (float)p1.y;
    	coord[2] = (float)p1.z;
    	switch (axis) {
    	case X:
    		coord[3] = (float)p1.x;
        	coord[4] = (float)p1.y;
        	coord[5] = (float)p2.z;
        	colors[0] = colors[1] = colors[2] = xColor;
    		break;
    	case Y:
    		coord[3] = (float)p1.x;
        	coord[4] = (float)p1.y;
        	coord[5] = (float)p2.z;
        	colors[0] = colors[1] = colors[2] = yColor;
    		break;
    	case Z:
    		coord[3] = (float)p1.x;
        	coord[4] = (float)p2.y;
        	coord[5] = (float)p2.z;
        	colors[0] = colors[1] = colors[2] = zColor;
    		break;
    	
    	}
    	coord[6] = (float)p2.x;
    	coord[7] = (float)p2.y;
    	coord[8] = (float)p2.z;
    	Line shape = new Line("",BufferUtils.createFloatBuffer(coord),null,BufferUtils.createFloatBuffer(colors),null);
    	shape.setMode(Line.CONNECTED);
    	editor.getRenderingComponent().getNoShadowRoot().attachChild(shape);
    	shape.setRenderState(ms);
        constraintHighlights.add(shape);
    }
    
    /**
     * Snaps position to axis-aligned planes defined by constraint points
     * Form of position is p+v, meaning that the position that is snapped is requestedPoint + requestedDir
     * @param requestedPoint one part of the position to be snapped
     * @param requestedDir second part of the position to be snapped and direction that the position is allowed to move
     * @return
     */
    public Point3d getPointSnap(Vector3d requestedPoint, Vector3d requestedDir) {
        
        Vector3d snappedPoint = new Vector3d();
        Point3d currentPoint = null;
        double u[] = new double[1];
        List<Point3d> p1s = new ArrayList<Point3d>();
        List<Point3d> p2s = new ArrayList<Point3d>();
        List<Integer> axes = new ArrayList<Integer>();
        
        for (Point3d constraintPoint : constraintPoints) {
            boolean snap = false;
            
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(X), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                //snapString += "Point/Plane x-snap ";
                snap = true;
                //addConstrainPlaneHighlight(constraintPoint, currentPoint,X);
                p1s.add(constraintPoint);
                p2s.add(currentPoint);
                axes.add(X);
            }
            
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Y), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                //snapString += "Point/Plane y-snap ";
                snap = true;
                //addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);
                p1s.add(constraintPoint);
                p2s.add(currentPoint);
                axes.add(Y);
            }
            
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), getAxialVector(Z), snappedPoint,u) && Math.abs(1.0 - u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                //snapString += "Point/Plane z-snap ";
                snap = true;
                //addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);
                p1s.add(constraintPoint);
                p2s.add(currentPoint);
                axes.add(Z);
            }
            if (snap)
                break;
        }
        if (p1s.size() == 0)
        	return null;
        if (p1s.size() == 1) {
        	snapString += "Point/Plane ";
        	switch (axes.get(0)) {
        	case X:
        		snapString += "x";
        		break;
        	case Y:
        		snapString += "y";
        		break;
        	case Z:
        		snapString += "z";
        		break;
        	}
        	snapString += "-snap ";
        	addConstrainPlaneHighlight(p1s.get(0), p2s.get(0),axes.get(0));
        	return currentPoint;
        } else if (p1s.size() == 3){
        	// all axial planes are intersecting, snapping point must be the constraint point
        	// all constraint points are the same, so just pick the first in the list
        	snapString += "Point/Point ";
        	return p1s.get(0);
        } else {
        	Vector3d dir = new Vector3d();
        	dir.cross(getAxialVector(axes.get(0)), getAxialVector(axes.get(1)));
        	currentPoint = new Point3d(MathTools.closestPointOnStraight(currentPoint, p1s.get(0), dir));
        	addConstrainLineHighlight(p1s.get(0), currentPoint, xColor);
        	snapString += "Point/Line ";
        	return currentPoint;
        }
        
    }
    
    private Vector3d getAxialVector(int axis) {
    	switch (axis) {
    	case X:
    		return new Vector3d(1.0,0.0,0.0);
    	case Y:
    		return new Vector3d(0.0,1.0,0.0);
    	case Z:
    		return new Vector3d(0.0,0.0,1.0);
    	}
    	throw new RuntimeException("Unknown axis " + axis); 	
    }
    
    /**
     * Snaps the position to axis-aligned planes defined by constraint points
     * @param requestedPoint point that is snapped
     * @param requestedDir direction that point is allowed to move
     * @return
     */
    
    public Point3d getPointSnap2(Vector3d requestedPoint, Vector3d requestedDir) {
        
        Vector3d snappedPoint = new Vector3d();
        Point3d currentPoint = null;
        double u[] = new double[1];
        //System.out.println(requestedPoint + "  " + requestedDir);
        for (Point3d constraintPoint : constraintPoints) {
            boolean snap = false;
            //System.out.print(constraintPoint + " ");
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(1.0,0.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                snapString += "Point/Plane x-snap ";
                snap = true;
                addConstrainPlaneHighlight(constraintPoint, currentPoint,X);
                //System.out.print(" x " + u[0]);
            }
            
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,1.0,0.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                snapString += "Point/Plane y-snap ";
                snap = true;
                addConstrainPlaneHighlight(constraintPoint, currentPoint,Y);
                //System.out.print(" y " + u[0]);
            }
           
            
            if (MathTools.intersectStraightPlane(requestedPoint, requestedDir, new Vector3d(constraintPoint), new Vector3d(0.0,0.0,1.0), snappedPoint,u) && Math.abs(u[0]) < snapAngle) {
                currentPoint = new Point3d(snappedPoint);
                snapString += "Point/Plane z-snap ";
                snap = true;
                addConstrainPlaneHighlight(constraintPoint, currentPoint,Z);
                //System.out.print(" z " + u[0]);
            }
            //System.out.println();
            if (snap)
                break;
        }
        return currentPoint;
    }
    
    public String getSnapString() {
        return snapString;
    }
}