#!/usr/bin/perl

use strict;
use Math::MatrixReal;
use Math::Amoeba qw(MinimiseND);
use PDL;
use PDL::Opt::Simplex;

#my $Method = "Amoeba::Simplex";
my $Method = "ModifiedNewton";
#my $Method = "PDL::Simplex";

my $EPS = 1.0e-7;
my $nMaxIter = 100;
my @guess = (1, 1);
my @scale = (1, 1);

print "use $Method\n";

if($Method =~ /Amoeba::Simplex/i) {
	my ($OptVars,$MinVal) = MinimiseND(\@guess, \@scale, \&MinimizationFunc, $EPS, $nMaxIter);
	print "(",join(',',@{$OptVars}),")=$MinVal\n";
}
elsif($Method =~ /ModifiedNewton/i) {
	my $MinVal = &ModifiedNewton(\@guess, \&MinimizationFunc, $EPS, $nMaxIter);
	print "(",join(',',@guess),")=$MinVal\n";
}
elsif($Method =~ /PDL::Simplex/i) {
	my $init = pdl( [@guess] );
	my $initsize = @guess;
	my ($optimum, $ssize) = simplex($init, $initsize, $EPS, $nMaxIter,
                 sub {&MinimizationFunc(&PDLtoList($_[0]))},
                 sub {&PDLDisplaySimplex($_[0])}
                 );
	print "optimum=$optimum}\n";
	print "ssize=$ssize\n";
}

exit;

#==============================================
# 最小化関数のサブルーチン
# MinimizationFunc    : 引数をリストでとる
# PDLDisplaySimplex: PDL::Opt::Simplex用のDisplay関数
#==============================================
sub MinimizationFunc {
	my ($a, $b) = @_;
	my $S = ($a-7)**2+($b+3)**2;
print "a,b=$a\t$b\t($S)\n";
	return $S;
}

sub PDLDisplaySimplex {
	my ($piddle, $v) = @_;
print "Display: piddle=$piddle\n";
}

#==============================================
# 共通静的ルーチン
#==============================================
sub PDLtoList {
	my ($piddle) = @_;
	my @a;
	my ($nDim) = dims($piddle);
	for(my $i = 0 ; $i < $nDim ; $i++) {
		my $v = $piddle->slice("($i)");
#		last if(!defined $v);
		$a[$i] = $v;
	}
	return @a;
}

#==============================================
# 修正Newton法(+CG)の自作ルーチン
# Differentiate1, 2も呼び出す
#==============================================
sub ModifiedNewton
{
	my ($pVariables, $pFunc, $EPS, $nMaxIter) = @_;
	$nMaxIter = 10 if(!defined $nMaxIter);
	$EPS = 1.0e-6 if(!defined $EPS);

	my $n = @$pVariables;
	for(my $iter = 0 ; $iter < $nMaxIter ; $iter++) {
		my $S = &$pFunc(@$pVariables);
print "Iter[$iter]: S=$S\n";

#一次微分の計算
		my @v;
		my $Si = new Math::MatrixReal($n, 1);
		for(my $i = 0 ; $i < $n ; $i++) {
			my $val = Differentiate1($pFunc, $pVariables, $i);
#print "dFdx[$i]=$val\n";
			$Si->assign($i+1, 1, $val);
		}
#二次微分の計算
		my $Sij = new Math::MatrixReal($n, $n);
		for(my $i = 0 ; $i < $n ; $i++) {
			for(my $j = $i ; $j < $n ; $j++) {
				my $val = Differentiate2($pFunc, $pVariables, $i, $j);
#print "dF2dx2[$i][$j]=$val\n";
				$Sij->assign($i+1, $j+1, $val);
				$Sij->assign($j+1, $i+1, $val) if($i != $j);
			}
		}
#対角成分の数値を格納
		my @kj;
		for(my $j = 0 ; $j < $n ; $j++) {
			$kj[$j] = 1.0 / $Sij->element($j+1, $j+1);
		}

#Dumping factorを変えながら最小点を探す
		my $MinS = $S;
		my $PrevS = 1.0e100;
		my $MinDump = -1;
		my @MinV;
		for(my $id = 0 ; $id <= 10 ; $id++) {
			my $Dump = 0.01 * (2**abs($id));
#Sijに各行方程式の係数を規格化した結果を格納する
			my $Sij2 = new Math::MatrixReal($n, $n);
			for(my $i = 0 ; $i < $n ; $i++) {
				for(my $j = 0 ; $j < $n ; $j++) {
					$Sij2->assign($i+1, $j+1, $kj[$j] * $Sij->element($i+1, $j+1));
				}
#対角成分にDumping factorを加える
				$Sij2->assign($i+1, $i+1, $Sij2->element($i+1, $i+1) + $Dump);
			}
#print "Sij:\n$Sij";
			my $RSij2 = $Sij2->inverse();
#print "Inverse Sij:\n$RSij";
#変数勾配の計算
			my $V = -$RSij2 * $Si;
			$V = -$V if($id < 0);
#print "V:\n$V";
#変数勾配の方向に、変位を変えながら関数値が最小になる変数を探す
			my @NewV;
			for(my $i = 0 ; $i < $n ; $i++) {
				$NewV[$i] = $pVariables->[$i] + $V->element($i+1, 1) * $kj[$i];
#print "  v[$i](d=$Dump): $pVariables->[$i] => $NewV[$i]\n";
			}
			my $NewS = &$pFunc(@NewV);
#print "  NewS(d=$Dump): $NewS\n";
			if($NewS < $MinS) {
				$MinS = $NewS;
				$MinDump = $Dump;
				for(my $i = 0 ; $i < $n ; $i++) {
					$MinV[$i] = $NewV[$i];
				}
			}
			last if($NewS > $PrevS);
			$PrevS = $NewS;
		}

		if(defined $MinV[0]) {
			my $MaxD = 0;
			for(my $i = 0 ; $i < $n ; $i++) {
#収束判定条件のチェック
				my $k = abs($MinV[$i]);
				if($k < abs($pVariables->[$i])) {
					$k = abs($pVariables->[$i]);
				}
				$k = 1.0 if($k == 0.0);
				my $d = ($MinV[$i] - $pVariables->[$i]) / $k;
				if($MaxD < abs($d)) {
					$MaxD = abs($d);
				}

print "  v[$i](d=$MinDump): $pVariables->[$i] => ";
				$pVariables->[$i] = $MinV[$i];
print "$MinV[$i]\n";
			}
#収束判定
			if($MaxD < $EPS) {
				print "  Convergence reached.: Stop Optimization\n";
				return $MinS;
			}
		}
		else {
#最小点がみつからなかったら止める
			print "  Warning: Better solution could not be found.\n";
			print "    Stop Optimization\n";
			last;
		}
	}

	return -1;
}

sub Differentiate1
{
	my ($pFunc, $pVariables, $idx) = @_;
	my $dv = abs($pVariables->[$idx]) * 0.001;
	$dv = 0.001 if($dv < 0.001);

	my @v;
	for(my $i = 0 ; $i < @$pVariables ; $i++) {
		$v[$i] = $pVariables->[$i];
	}
	$v[$idx] = $pVariables->[$idx] + $dv;

	my $S0 = &$pFunc(@$pVariables);
	my $S1 = &$pFunc(@v);
	return ($S1 - $S0) / $dv;
}

sub Differentiate2
{
	my ($pFunc, $pVariables, $idx, $jdx) = @_;
	my $dv = abs($pVariables->[$jdx]) * 0.001;
	$dv = 0.001 if($dv < 0.001);

	my @v;
	for(my $i = 0 ; $i < @$pVariables ; $i++) {
		$v[$i] = $pVariables->[$i];
	}
	$v[$jdx] = $pVariables->[$jdx] + $dv;

	my $S0 = Differentiate1($pFunc, $pVariables, $idx);
	my $S1 = Differentiate1($pFunc, \@v, $idx);

	return ($S1 - $S0) / $dv;
}