#!/usr/bin/perl 

use Math::MatrixReal;       # available on CPAN

# solve() implements a multidimensional Newton's method for computing
# the solution to a set of N arbitrary equations in N unknowns.
sub solve {
    my ($funcs, $point, $iterations, $epsilon_var, $epsilon_value) = @_;
    my ($i, $j, $k, @values, @delta, $error_var, $error_value);

    # Make sure we have N functions in N unknowns.
    return unless @$funcs == @$point;

    for ($i = 0; $i < $iterations; $i++) {
        for ($j = 0; $j < @$funcs; $j++) {
            $values[$j] = &{$funcs->[$j]}( @$point );
        }
        @jacobian = jacobian( $funcs, $point );

        for ($j = 0, $error_value = 0; $j < @$funcs; $j++) {
            $error_value += abs( $values[$j] );
        }
        return $point if $error_value <= $epsilon_value;

        for ($j = 0; $j < @$funcs; $j++) { $delta[$j] = -$values[$j] }

        # Treat our Jacobian matrix as a set of linear equations
        # and solve using LR decomposition.
        my $matrix = new Math::MatrixReal(scalar @$funcs, scalar @$point);
        for ($j = 0; $j < @$funcs; $j++) {
            for ($k = 0; $k < @$point; $k++) {
                assign $matrix ( $j+1, $k+1, $jacobian[$j][$k] );
            }
        }
        my $vector = new Math::MatrixReal(scalar @delta, 1);
        for ($j = 0; $j < @delta; $j++) {
             assign $vector( $j+1, 1, $delta[$j] )
        }
        my $LR = decompose_LR $matrix;
        my ($dimension, $solution, $base) = $LR->solve_LR( $vector );

        for ($j = 0; $j < @$funcs; $j++) {
            $delta[$j] = $solution->element($j+1, 1);
        }

        for ($j = 0, $error_value = 0; $j < @$funcs; $j++) {
            $error_value += abs( $delta[$j] );
            $point->[$j] += $delta[$j];
        }
        return $point if $error_value <= $epsilon_var;
    }
    return $point;
}

sub sphere {
    my ($x, $y, $z) = @_;
    $x**2 + $y**2 + $z**2 - 64;
}

sub saddle {
    my ($x, $y, $z) = @_;
    -($x**2) + $y**2 - $z;
}

sub plane {
    my ($x, $y, $z) = @_;
    $x + $y + $z - 8;
}

$solution = solve( [\&sphere, \&saddle, \&plane], [3,3,3], 300, 0.01,
                  0.01 );
print "Solution: @$solution\n";


# jacobian($func_array, $point) calculates the Jacobian matrix at
# $point for the array of functions referred to by $func_array.
# $point is a reference to an array of coordinates.
#
sub jacobian {
    my ($func_array, $point) = @_;
    my ($delta, $i, $j, $k, $coord, @values, @func, @jacobian);
    my $epsilon = 1e-8;

    # Feed the point into each function.
    #
    for ($i = 0; $i < @$func_array; $i++) {
        $values[$i] = &{$func_array->[$i]}( @$point );
    }

    for ($i = 0; $i < @$point; $i++) {
        $coord = $point->[$i];
        $delta = $epsilon * abs($coord) || $epsilon;
        $point->[$i] = $delta + $coord;
        $delta = $point->[$i] - $coord;
        for ($k = 0; $k < @$func_array; $k++) {
            $func[$k] = &{$func_array->[$k]}( @$point );
        }
        $point->[$i] = $coord;
        for ($j = 0; $j < @$func_array; $j++) {
            $jacobian[$j][$i] = ($func[$j] - $values[$j]) / $delta;
        }
    }
    return @jacobian;
}