#!/usr/bin/perl 

use Math::MatrixReal;       # available on CPAN

# Procedura rozwiaz() implementuje metode Newtona do rozwiazywania
# ukladu N dowolnych funkcji z N niewiadomymi.
sub rozwiaz {
    my ($funkcje, $punkt, $iteracje, $epsilon_zmienna, $epsilon_wartosc) = @_;
    my ($i, $j, $k, @wartosci, @delta, $blad_zmienna, $blad_wartosc);

    # Sprawdzenie obecnosci N funkcji z N niewiadomymi.
    return unless @$funkcje == @$punkt;

    for ($i = 0; $i < $iteracje; $i++) {
        for ($j = 0; $j < @$funkcje; $j++) {
            $wartosci[$j] = &{$funkcje->[$j]}( @$punkt );
        }
        @jakobian = jakobian( $funkcje, $punkt );

        for ($j = 0, $blad_wartosc = 0; $j < @$funkcje; $j++) {
            $blad_wartosc += abs( $wartosci[$j] );
        }
        return $punkt if $blad_wartosc <= $epsilon_wartosc;

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

        # Potraktowanie macierzy jakobianu jako ukladu rownan liniowych
        # i rozwiazanie ich przy uzyciu rozkladu LR.
        my $macierz = new Math::MatrixReal(scalar @$funkcje, scalar @$punkt);
        for ($j = 0; $j < @$funkcje; $j++) {
            for ($k = 0; $k < @$punkt; $k++) {
                assign $macierz ( $j+1, $k+1, $jakobian[$j][$k] );
            }
        }
        my $wektor = new Math::MatrixReal(scalar @delta, 1);
        for ($j = 0; $j < @delta; $j++) {
             assign $wektor( $j+1, 1, $delta[$j] )
        }
        my $LR = decompose_LR $macierz;
        my ($wymiar, $rozwiazanie, $bazowa_macierz) = $LR->solve_LR( $wektor );

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

        for ($j = 0, $blad_wartosc = 0; $j < @$funkcje; $j++) {
            $blad_wartosc += abs( $delta[$j] );
            $punkt->[$j] += $delta[$j];
        }
        return $punkt if $blad_wartosc <= $epsilon_zmienna;
    }
    return $punkt;
}

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

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

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

$rozwiazanie = rozwiaz( [\&kula, \&funkcja, \&plaszczyzna], [3,3,3], 300, 0.01, 0.01 );

print "Rozwiazanie: @$rozwiazanie\n";


# jakobian($tablica_funkcji, $punkt) 
# Oblicza macierz jakobinu w punkcje $punkt dla tablicy funkcji
# wskazywanej przez $tablica_funkcji.
# $punkt to odwolanie do tablicy wspolrzednych.
#
sub jakobian {
    my ($tablica_funkcji, $punkt) = @_;
    my ($delta, $i, $j, $k, $wspolrzedne, @wartosci, @funkcja, @jakobian);
    my $epsilon = 1e-8;

    # Przekazanie punktu do kazdej funkcji.
    #
    for ($i = 0; $i < @$tablica_funkcji; $i++) {
        $wartosci[$i] = &{$tablica_funkcji->[$i]}( @$punkt );
    }

    for ($i = 0; $i < @$punkt; $i++) {
        $wspolrzedne = $punkt->[$i];
        $delta = $epsilon * abs($wspolrzedne) || $epsilon;
        $punkt->[$i] = $delta + $wspolrzedne;
        $delta = $punkt->[$i] - $wspolrzedne;
        for ($k = 0; $k < @$tablica_funkcji; $k++) {
            $funkcja[$k] = &{$tablica_funkcji->[$k]}( @$punkt );
        }
        $punkt->[$i] = $wspolrzedne;
        for ($j = 0; $j < @$tablica_funkcji; $j++) {
            $jakobian[$j][$i] = ($funkcja[$j] - $wartosci[$j]) / $delta;
        }
    }
    return @jakobian;
}
