package org.simantics.debug.graphical.layout;

public class LayoutAlgorithm {

    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 double MAX_MOVE2 = 40000.0;
    
    int size;
    double[] posX;
    double[] posY;
    double[] forceX;
    double[] forceY;
    
    int[][] neighbors;
    
    public LayoutAlgorithm(int[][] neighbors) {
        this.size = neighbors.length;
        this.neighbors = neighbors;
        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;
                    /*double l = Math.sqrt(l2);
                    double l3 = l*l2;*/
                    fx -= Q * dx / l2;
                    fy -= Q * dy / l2;
                }
            
            for(int j : neighbors[i]) {
                double dx = posX[j] - x;
                double dy = posY[j] - 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();
        
        for(int iter=0;iter<MAX_ITERATIONS;++iter) {
            if(converged())
                break;
            moveAll();
        }
    }
}