//
//  FastMech.java
//  FastMech
//
// 2003 Apr 28 Started.  Relaxation working just after midnight.
//      Apr 29 Put 30 cell spastic/graceful example on web site.
//      Apr 30 Linear solver going with circular force fields.
//      May 2 - early AM of May 3 - linear, then eliptical
//             spring fields.  Solves 70 cells < 15 sec.
//             150 cells pretty much in < 3 minutes.
//             But that's with cells that rotate 80
//             degrees & get real big.
//

import java.applet.Applet;
import java.util.*;
import java.awt.*;
import java.lang.Math;

public class FastMech2 extends Applet {

    // x's and y's in the same array.  Numbering from zero.
    // x's first (evens), then y's (odds).
    // Relative to the index i of the beginning of a cell's
    // numbers, the corners of a cell get these coordinates:
    // ( xy[i+2], xy[i+3] ) --- ( xy[i+6], xy[i+7] )
    //          |                        |
    //          |                        |
    //          |                        |
    //          |                        |
    // ( xy[ i ], xy[i+1] ) --- ( xy[i+4], xy[i+5] )

    // These are all double because float requires casting!
    double xy[];   // x and y components of position
    double min_x = 0.0;
    double max_x = 0.0;
    double min_y = 0.0;
    double max_y = 0.0;
    double mxy[];  // x and y components of momentum
    double old_mxy[];  // used by local_drag()
    double fxy[];  // x and y components of force
    double A[][];  // square matrix set up so that A * xy = fxy
    int n_cells;
    double xf, xc, yf, yc;  // scaling constants for drawing
    Random random = new Random();
    int time_step = 0;
    int delay = 3;
    double k = .0005;
    
    public void init_cells( int n_cells )  {
        xy = new double [ 4 + 4 * n_cells ];
        mxy = new double [ xy.length ];
        old_mxy = new double [ xy.length ];
        fxy = new double [ xy.length ];
        A = new double [ xy.length ] [ xy.length ];
        mxy = new double [ 4 + 4 * n_cells ];
        this.n_cells = n_cells;
        
        xy[ 0 ] = 0.0;  xy[ 1 ] = 0.0;
        xy[ 2 ] = 0.0;  xy[ 3 ] = 1.0;
        for( int i = 0;  i < n_cells;  i++ )  {
            xy[ i*4 + 4 ] = ( i + 1 ) * 1.0;  xy[ i*4 + 5 ] = 0.0;
            xy[ i*4 + 6 ] = ( i + 1 ) * 1.0;  xy[ i*4 + 7 ] = 1.0;
            }
        }
        

    public void draw_wall( Graphics g, int i, int j )  {
        g.drawLine( (int) ( xy[ i ]*xf+xc ),  (int) ( xy[i+1]*yf+yc ),
                    (int) ( xy[ j ]*xf+xc ),  (int) ( xy[j+1]*yf+yc )  );
        }
    
	public void paint( Graphics g ) {
        double max;

        min_x += ( max_x - min_x ) / 10.0;
        max_x -= ( max_x - min_x ) / 9.0;
        min_y += ( max_y - min_y ) / 10.0;
        max_y -= ( max_y - min_y ) / 9.0;
        for( int i = 0;  i < xy.length;  i += 2 )  {
            if( xy[ i ] < min_x )  min_x = xy[ i ];
            if( xy[ i ] > max_x )  max_x = xy[ i ];
            if( xy[i+1] < min_y )  min_y = xy[i+1];
            if( xy[i+1] > max_y )  max_y = xy[i+1];
            }
        if( max_x - min_x > max_y - min_y )  max = max_x - min_x;
        else                                 max = max_y - min_y;
        // Assuming the window is 400 x 400...
        xf = 320.0 / max;  yf = -xf;  // Java AWT's y is upside-down;
        xc = 200.5 - xf * .5 * ( min_x + max_x );
        yc = 200.5 - yf * .5 * ( min_y + max_y );
        
        g.setColor( Color.gray );
        for( double d = 2.0;  d < max;  d *= 2.0 )  {
            int top = (int) ( 0.5 * yf + yc + d * yf );
            int left = (int) ( 200.5 - d * xf );
            int size = (int) ( 2.0 * d * xf );
            g.drawRect( left, top, size, size );
            }
            
        
        g.setColor( Color.black );
        draw_wall( g, 0, 2 );
        for( int i = 0;  i <= xy.length - 8;  i += 4 )  {
            draw_wall( g,      i,  i + 4 );
            draw_wall( g,  i + 4,  i + 6 );
            draw_wall( g,  i + 6,  i + 2 );
            }
            
        if( ( ++time_step ) == -1)  {
            recalc_relaxation( );
            }
        if( ( time_step ) == delay )  {
            recalc_solve();
            time_step = 0;
            k = k * .99;
            if( delay > 10 ) delay -= 2;
            if( delay > 1 ) delay--;
            }
        repaint();
		}
        
        
    // Calculate linear approximations to the two-way 
    // force effects of a spring
    // as a function of the end coordinates.
    void calc_spring( double A[][], double fxy[], double xy[], 
                    int i, int j, double preferred_len )  {
        double xl = xy[ j ] - xy[ i ];
        double yl = xy[j+1] - xy[i+1];
        double len = Math.sqrt( xl * xl + yl * yl );
        double c = xl / len;
        double s = yl / len;
        // What would xy[ j ] - xy[ i ], etc., like to be?
        double pxl = xl * preferred_len / len;
        double pyl = yl * preferred_len / len;
        // These springs will have a constant stiffness
        // regardless of preferred_len, because my head hurts.
        double stiff = 1.0;  // in the direction of the spring
        // difference between preferred distances and current
        double dx = xl - pxl;
        double dy = yl - pyl;
        double dl = len - preferred_len;
        
        // Instantaneous (current) forces (also force on the
        // i as opposed to the j point).  These are the same
        // whether the contours of the force field are 
        // circular, flat or eliptical.
        double fxi = dx * stiff;
        double fyi = dy * stiff;
        // System.out.println( "Forces between " +i +" & " +j +" should be ( " +fxi +", " +fyi +" )" );
        
        // Derivatives of force w.r.t. changes in dx, dy.
        // These are also the coeffs in the linear
        // approximations.
        double fxdx = c * c * stiff;
        double fxdy = s * c * stiff;
        double fydx = s * c * stiff;
        double fydy = s * s * stiff;
        
        // Add in some mixture of circular rather than linear
        // force contours, creating elipses.  k = ratio of
        // sides of the elipses.  k = 0 means lines.
        // The first derivative of these forces w.r.t.
        // sideways motion of the ends should be zero; making
        // k > 0 changes that, but it seems to prevent the
        // whole picture from rotating (also seems to make the
        // matrix non-singular--probably the same thing).
        // double k = 0.01;
        fxdx = k * stiff + ( 1.0 - k ) * fxdx;
        fxdy =             ( 1.0 - k ) * fxdy;
        fydx =             ( 1.0 - k ) * fydx;
        fydy = k * stiff + ( 1.0 - k ) * fydy;
        
        // Add coefficients into A matrix:
        A[i  ][i  ] += -fxdx;
        A[i  ][j  ] += fxdx;
        A[i  ][i+1] += -fxdy;
        A[i  ][j+1] += fxdy;
        
        A[i+1][i  ] += -fydx;
        A[i+1][j  ] += fydx;
        A[i+1][i+1] += -fydy; 
        A[i+1][j+1] += fydy;
        // Forces for the j point are opposite:
        A[j  ][j  ] += -fxdx;
        A[j  ][i  ] += fxdx;
        A[j  ][j+1] += -fxdy;
        A[j  ][i+1] += fxdy;
        
        A[j+1][j  ] += -fydx;
        A[j+1][i  ] += fydx;
        A[j+1][j+1] += -fydy;
        A[j+1][i+1] += fydy;
        // Add constants to fxy vector such that the
        // instantaneous forces come out right.
        fxy[i  ] += fxi - fxdx * xl - fxdy * yl;
        fxy[i+1] += fyi - fydx * xl - fydy * yl;
        // Forces for j point are opposite:
        fxy[j  ] += -fxi + fxdx * xl + fxdy * yl;
        fxy[j+1] += -fyi + fydx * xl + fydy * yl;
        }
        

    // Calculate the positions -> forces matrix
    // based on some arbitrary physics for the cells.
    // Each row of A, plus one entry in fxy, gets an equation
    // about the corresponding variable.
    void calc_matrix( double A[][], double fxy[], double xy[] )  {
        for( int i = 0;  i < fxy.length;  i++ )  {
            fxy[ i ] = 0.0;
            for( int j = 0;  j < fxy.length;  j++ )  {
                A[ i ][ j ] = 0.0;
                }
            }
        double s3 = Math.sqrt( 3.0 );
        // Each cell has a spring on each side, and 
        // criss-cross truss springs so it doesn't flatten.
        calc_spring( A, fxy, xy, 0, 2, 1.0 );
        for( int i = 0;  i <= xy.length - 8;  i += 4 )  {
            // i, i+2 spring is already calculated.
            calc_spring( A, fxy, xy, i,   i+4, 2.0 );
            calc_spring( A, fxy, xy, i,   i+6, s3 );
            calc_spring( A, fxy, xy, i+2, i+4, s3 );
            calc_spring( A, fxy, xy, i+2, i+6, 1.0 );
            calc_spring( A, fxy, xy, i+4, i+6, 1.0 );
            }
        }
        

    // Matrix multiply using conventional names for the variables:
    // b coming in is the constants, going out is the results.
    // b += A * x
    void matrix_multiply_add( double A[][], double x[], double b[] )  {
        for( int i = 0;  i < x.length;  i++ )  {
            double row[] = A[i];
            for( int j = 0;  j < x.length; )  {
                b[ i ] += row[j] * x[j];
                while( ++j < x.length  &&  row[j] == 0.0 );
                }
            }
        }
        
        
    // x1 = x1f * x1 + x2f * x2
    void scaled_vector_add( double x1f, double x1[], double x2f, double x2[] )  	{
        for( int i = 0;  i < x1.length;  i++ )  {
            x1[ i ] = x1f * x1[ i ]  +  x2f * x2[ i ];
            }
        }
        

    /*
    void random_forces( double fxy[] )  {
        for( int i = 0;  i < mxy.length;  i++ )  {
            fxy[ i ] = ( random.nextInt( 32768 ) - 16383.5 ) / 100000.0;
            }
        }
    */
        

    // Calculate two-way drag effect between points i & j. 
    void pair_drag( double mxy[], double f, int i, int j ) {
        for( int k = 0;  k < 2;  k++ )  {
            double diffxy = ( old_mxy[j+k] - old_mxy[i+k] ) * f;
            mxy[i+k] += diffxy;
            mxy[j+k] -= diffxy;
            }
        }
        
        
    void local_drag( double mxy[], double f )  {
        System.arraycopy( mxy, 0, old_mxy, 0, mxy.length );
        pair_drag( mxy, f, 0, 2 );
        for( int i = 0;  i <= mxy.length - 8;  i += 4 )  {
            // i is the bottom-left corner.
            // pair_drag( mxy, f, i, i + 2 ) already done.
            pair_drag( mxy, f, i,      i + 4 );
            pair_drag( mxy, f, i + 2,  i + 6 );
            pair_drag( mxy, f, i + 4,  i + 6 );
            }
        }
        

    void print_equations( double A[][], double fxy[] )  {    
        print_equations( A, fxy, fxy.length );
        }
        
        
    String format_number( double x )  {
        String result;
        
        if( x == 0.0 )  return( " 0.0" );
        if( x < 0.0 )  {
            result = "-";
            x = -x;
        } else {
            result = " ";
        }
        if( x < .1  ||  x > 1000 )  return( result + x );
        return( result + ((int) (x * 10.0 + .5 )) * .1 );
        }
        
        
    void print_equations( double A[][], double fxy[], int size )  {        
        for( int i = 0;  i < size;  i++ )  {
        	String str = "";
        	for( int j = 0;  j < size;  j++ )  {
                str = str + format_number( A[i][j] ) + " ";
                }
            System.out.println( str + "= " + format_number( fxy[ i ] ) );
            }
        }
        
        
    void print_values( double x[] )  {
        String str = "";
        for( int j = 0;  j < x.length;  j++ )  {
            str = str + x[j] + " ";
            }
        System.out.println( str );
        }
        
        
	public void init( )  {
        init_cells( 300 );
		}
        
        
    public void show_forces( )  {
        for( int i = 0;  i < fxy.length;  i += 2 )  {
            System.out.println( "Forces on " + i + " are ( " + fxy[i] + ", " + fxy[i+1] + " )" );
            }
        }


    void recalc_relaxation( )  {
        calc_matrix( A, fxy, xy );
        // print_equations( A, fxy );
        matrix_multiply_add( A, xy, fxy );  // Calculate forces.
        // show_forces();
        // for( double f = .01;  f < .1;  f += .005 )  {
        //    local_drag( mxy, f );
        //     }
        local_drag( mxy, .1 );
        // random_forces( fxy );
        scaled_vector_add( 1.0, mxy, 1.0, fxy );
        scaled_vector_add( 1.0, xy, 1.0, mxy );
        }


    public double average_every_other( double v[], int k )  {
        double total = 0;
        for( int i = 0;  i < v.length;  i += 2 )  {
            total += v[ i + k ];
            }
        return( total / ( v.length / 2 ) );
        }
        

    // row i = fi*(row i) + fj*(row j)
    public void scaled_row_add( double A[][], double b[], 
                                double fi, int i, double fj, int j )  {
        scaled_vector_add( fi, A[ i ], fj, A[ j ] );
        b[ i ] = fi * b[ i ] + fj * b[ j ];
        }
        
        
    public void zero_variable( double A[][], double x[], double b[], int i )  {
        for( int j = 0;  j < b.length;  j++ )  {
            A[ j ][ i ] = 0.0;
            }
        // Is it okay to just add one row to another?
        // int j = ( i == 0?  1  :  i - 1 );
        // scaled_row_add( A, b, 1.0, j, 1.0, i );
        // Zero row i:
        scaled_row_add( A, b, 0.0, i, 0.0, i );
        // Set the result variable:
        x[ i ] = 0.0;
        }
        
        
    // Naive substitution solver with no pivoting.
    // Find x such that A * x + b = 0.  Modifies A and b.
    void solve( double A[][], double x[], double b[], int size )  {
        // Substitute rows into lower rows:
        for( int i = 0;  i < size - 1;  i++ )  {
            double f = -1.0 / A[i][i];
            for( int j = i + 1;  j < size;  j++ )  {
                if( A[ j ][ i ] != 0.0 )  {
                    scaled_row_add( A, b, 1.0, j, f * A[ j ][ i ], i );
                    // A[j][i] = A[j][i] - (A[j][i]/A[i][i]) * A[i][i],
                    // so it should be pretty close to zero,
                    A[ j ][ i ] = 0.0;  // but make sure.
                    }
                }
            }
        // Now the matrix is "upper triangular," yay!
        // Each row expresses a var in terms of lower vars,
        // and the bottom row is trivial.  So go bottom-up:
        for( int i = size - 1;  i >= 0;  i-- )  {
            double t = b[ i ];
            double row[] = A[ i ];
            for( int j = i + 1;  j < size;  )  {
                t += row[ j ] * x[ j ];
                while( ++j < size  &&  row[ j ] == 0.0 ) ;
                }
            x[ i ] = -t / row[ i ];
            }
        }


    public void recalc_solve( )  {
        double center_x = average_every_other( xy, 0 );
        double center_y = average_every_other( xy, 1 );
        calc_matrix( A, fxy, xy );
        // System.out.println( "Values:" );
        // print_values( xy );
        // System.out.println( "Before:" );
        // print_equations( A, fxy );
        // A and fxy now contain n equations in n unknowns, but
        // these equations only say things about relative 
        // positions of points and not the absolute position
        // of the whole set.  So there's redundancy, the matrix is
        // singular, and the equations have no unique solution.
        // What we do is set one x and one y to zero, zero the
        // corresponding columns, and absorb their rows into the
        // others, then solve the smaller set.  I'm going to get 
        // rid of the last two variables, columns and rows because 
        // that makes the least mess.
        zero_variable( A, xy, fxy, fxy.length - 1 );
        zero_variable( A, xy, fxy, fxy.length - 2 );
        // System.out.println( "After:" );
        // print_equations( A, fxy, fxy.length - 2 );
        solve( A, xy, fxy, fxy.length - 2 );
        // System.out.println( "After solving:" );
        // print_equations( A, fxy, fxy.length - 2 );
        // System.out.println( "Values:" );
        // print_values( xy );
        // Now readjust points so "center of gravity is conserved."
        center_x -= average_every_other( xy, 0 );
        center_y -= average_every_other( xy, 1 );
        for( int i = 0;  i < xy.length;  i += 2 )  {
            xy[ i     ] += center_x;
            xy[ i + 1 ] += center_y;
            }
        // System.out.println( "Adjusted:" );
        // print_values( xy );
        }
		
		
	public void destroy( )  {
		}
		   
	}