package org.simantics.debug.graphical.layout;

public class ExtensionLayoutAlgorithm {
    final double OPTIMAL_DISTANCE = 150.0;   
    final double C = 0.1;
    final double K = C / OPTIMAL_DISTANCE;
    final double G = K;
    final double Q = C * OPTIMAL_DISTANCE * OPTIMAL_DISTANCE;
    final double TOLERANCE = 1e-3;
    final int MAX_ITERATIONS = 10000;
    final long TIMEOUT = 1000000000L; // 1 sec
    final double MAX_MOVE2 = 40000.0;
    
    int size;
    double[] posX;
    double[] posY;
    double[] forceX;
    double[] forceY;
    
    int repulsiveSize;
    double[] fixedRepulsiveX;
    double[] fixedRepulsiveY;
    double[][] neighbors;
    
    public ExtensionLayoutAlgorithm(double[][] neighbors, 
            double[] fixedRepulsiveX, double[] fixedRepulsiveY) {
        this.size = neighbors.length;
        this.neighbors = neighbors;
        this.repulsiveSize = fixedRepulsiveX.length;
        this.fixedRepulsiveX = fixedRepulsiveX;
        this.fixedRepulsiveY = fixedRepulsiveY;
        this.posX = new double[size];
        this.posY = new double[size];
        this.forceX = new double[size];
        this.forceY = new double[size];
    }
    
    public double[] getPosX() {
        return posX;
    }
    
    public double[] getPosY() {
        return posY;
    }
    
    private void computeForces() {
        for(int i=0;i<size;++i) {
            double x = posX[i];
            double y = posY[i];
            double fx = -G * x;
            double fy = -G * y;
            
            for(int j=0;j<size;++j)
                if(j != i) {
                    double dx = posX[j] - x;
                    double dy = posY[j] - y;
                    double l2 = dx*dx + dy*dy;
                    fx -= Q * dx / l2;
                    fy -= Q * dy / l2;
                }
            for(int j=0;j<repulsiveSize;++j)
                if(j != i) {
                    double dx = fixedRepulsiveX[j] - x;
                    double dy = fixedRepulsiveY[j] - y;
                    double l2 = dx*dx + dy*dy;
                    fx -= Q * dx / l2;
                    fy -= Q * dy / l2;
                }
            
            double[] ns = neighbors[i];
            for(int j=0;j<ns.length;j+=2) {
                double dx = ns[j] - x;
                double dy = ns[j+1] - y;       
                double l2 = dx*dx + dy*dy;
                double l = Math.sqrt(l2);
                fx += K * l * dx;
                fy += K * l * dy;
            }
            
            forceX[i] = fx;
            forceY[i] = fy;
        }
    }
    
    private boolean converged() {
        for(int i=0;i<size;++i) {
            double fx = forceX[i];
            double fy = forceY[i];
            double f2 = fx*fx + fy*fy;
            if(f2 > TOLERANCE)
                return false;
        }
        return true;
    }
    
    private void moveAll() {
        for(int i=0;i<size;++i) {
            double fx = forceX[i];
            double fy = forceY[i];
            double f2 = fx*fx + fy*fy;
            if(f2 > MAX_MOVE2) {
                double s = Math.sqrt(MAX_MOVE2 / f2);
                fx *= s;
                fy *= s;
            }
            posX[i] += fx;
            posY[i] += fy;
        }
        computeForces();
    }
    
    public void optimize() {
        computeForces();

        long beginTime = System.nanoTime();
        int iter;
        for(iter=0;iter<MAX_ITERATIONS;++iter) {
            if(converged() || ((iter&0xf)==0 && System.nanoTime()-beginTime > TIMEOUT))
                break;            
            moveAll();
        }
        System.out.println("Elapsed: " + (System.nanoTime()-beginTime)*1e-6 + " ms");
        System.out.println("Iterations: " + iter);
    }
}