#===============================================
# Optimize
#===============================================
package Optimize;
use Exporter;
@ISA = qw(Exporter);

#公開したいサブルーチン
#@EXPORT = qw(aa );

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

#============================================================
#　変数取得関数
#============================================================
sub nS2Calculation      { return shift->{nS2Calculation}; }
sub SetnS2Calculation   { my($this,$n)=@_; return $this->{nS2Calculation} = $n; }
sub IncrementnS2        { my($this)=@_; return $this->{nS2Calculation}++; }
sub Variable            { my ($this,$i)=@_; return $this->{pVariables}->[$i]; }
sub pVariables          { my ($this)=@_; return $this->{pVariables}; }
sub pParameterNameArray { my ($this) = @_; return $this->{pParameterNameArray}; }

#============================================================
#　コンストラクタ、デストラクタ
#============================================================
sub new
{
	my ($module) = @_;
	my $this = {};
	bless $this;
	$this->Initialize();
	$this->SetDumpingFactor(0.0, 1.0, 10);
	$this->SetDiffV(0.05);

	return $this;
}

sub DESTROY
{
	my $this = shift;
}

#============================================================
#　静的関数
#============================================================
#==============================================
# 共通静的ルーチン
#==============================================
#最適化に含む変数、全変数、全変数のうち最適化するかどうかのIdから、
#関数値を計算するための変数リストを作ってリストを返す
sub RecoverParams
{
	my ($pOptVars, $pAllParams, $pOptId) = @_;
#print "OptVars=", @$pOptVars, "\n";
	my @a;
	my $c = 0;
	for(my $i = 0 ; $i < @$pAllParams ; $i++) {
		if($pOptId->[$i]) {
			$a[$i] = $pOptVars->[$c];
			$c++;
		}
		else {
			$a[$i] = $pAllParams->[$i];
		}
#$a[$i]がpiddleだった場合、最初の要素を抽出する
#		if($a[$i] =~ /^\[(\S+)/) {
#			$a[$i] = $1;
#		}
#print "$i: $a[$i], dim=", dims($a[$i]), "\n";
	}
#print "a=", join(',',@a), "\n";
	return \@a;
}

sub PDLtoList {
	my ($piddle) = @_;
print "PDLtoList: $piddle\n";
	my @a;
	my ($nDim) = dims($piddle);
	for(my $i = 0 ; $i < $nDim ; $i++) {
		my $v = $piddle->slice("($i)");
#print "v=$v\n";
#		last if(!defined $v);
#		$a[$i] = $v;
		$a[$i] = $v->slice("(0)");
	}
#print "a=", @a, "\n";
	return @a;
}

sub PDLtoListRef {
	my ($piddle) = @_;
#print "PDLtoListRef: $piddle\n";
	my @a;
	my ($nDim) = dims($piddle);
	for(my $i = 0 ; $i < $nDim ; $i++) {
		my $v = $piddle->slice("($i)");
#print "v[$i] = $v\n";
#		last if(!defined $v);
		$a[$i] = $v;
	}
	return \@a;
}

#============================================================
#　メンバー関数
#============================================================

#==============================================
# 1変数の二分法
#==============================================
sub BisectionMethod
{
	my ($this, @a) = @_;
	$this->BinarySearch(@a);
}

sub BinarySearch
{
	my ($this, $xmin, $xmax, $pCalS, $nMaxIter, $xEPS, $SEPS, $iPrintLevel) = @_;
#print "Optimize: $this, $xmin, $xmax, $pCalS, $nMaxIter, $xEPS, $SEPS, $iPrintLevel\n";
	$iPrintLevel = 0 if(!defined $iPrintLevel);

	my $Smin = &$pCalS($xmin, $iPrintLevel);
	my $Smax = &$pCalS($xmax, $iPrintLevel);
	if($Smin * $Smax > 0.0) {
print "S($Smin)\@$xmin and S($Smax)\@$xmax must be negative and positive.\n" if($iPrintLevel >= 1);
		return ([0.0], 0.0);
	}

	for(my $i = 0 ; $i < $nMaxIter ; $i++) {
		my $dx = abs($xmax - $xmin);
print "Iter[$i]: dx=$dx ($xmax - $xmin)\n" if($iPrintLevel >= 2);
		return ([($xmin+$xmax)/2.0], ($Smin+$Smax)/2.0, $i) if($dx < $xEPS);

		my $xhalf  = ($xmin + $xmax) / 2.0;
		my $Shalf = &$pCalS($xhalf, $iPrintLevel);
print "Iter[$i]: E: $xmin - $xhalf - $xmax: $Smin - $Shalf - $Smax\n" if($iPrintLevel >= 2);
		return ([$xhalf], $Shalf, $i) if(abs($Shalf) < $SEPS);

		if($Smin * $Shalf < 0.0) {
			$xmax = $xhalf;
			$Smax = $Shalf; #&$pCalS($xmax, $iPrintLevel);
		}
		else {
			$xmin = $xhalf;
			$Smin = $Shalf; #&$pCalS($xmin, $iPrintLevel);
		}
	}
	return ([($xmin+$xmax)/2.0], ($Smin+$Smax)/2.0);
}

sub SolveByModifiedNewton1D
{
	my ($this, $x, $diffv, $Dump, $pCalS, $nMaxIter, $xEPS, $SEPS, $iPrintLevel) = @_;
	$diffv       = 0.01   if(!defined $diffv);
	$Dump        = 0.0    if(!defined $Dump);
	$nMaxIter    = 10     if(!defined $nMaxIter);
	$xEPS        = 1.0e-6 if(!defined $xEPS);
	$SEPS        = 1.0e-6 if(!defined $SEPS);
	$iPrintLevel = 0      if(!defined $iPrintLevel);

	my $Snew;
	for(my $iter = 0 ; $iter < $nMaxIter ; $iter++) {
		my $x0   = $x;
		my $S    = &$pCalS($x);
		my $v0   = $x;
		my $S1   = &$pCalS($x+$diffv);
		my $dSdx = ($S1 - $S) / $diffv;
#print "S=$S (x=$x)  $S1(", $x+$diffv, ")  dSdx=$dSdx\n";
		my $dx = -$S / ($Dump + 1.0) / $dSdx;
		$x += $dx;
		$Snew = &$pCalS($x);
print "iter[$iter]: x=$x0 => $x: S=$S => $Snew\n" if($iPrintLevel >= 2);
		if(abs($dx) < $xEPS or abs($Snew) < $SEPS) {
			return ([$x], $Snew, $iter);
		}
	}

	return ([$x], $Snew);
}

#==============================================
# 補助サブルーチン
#==============================================
sub PrintParameters
{
	my ($this, $IsDetail, $pVars, $MinVal, $PrintAll) = @_;
	$pVars    = $this->{pVariableArray} if(!defined $pVars);
	$MinVal   = $this->{MinVal}         if(!defined $MinVal);
	$MinVal   = "'Not returned'"        if(!defined $MinVal);
	$PrintAll = 1                       if(!defined $PrintAll);

	my $pName     = $this->{pParameterNameArray};
	my $pId       = $this->{pParameterOptIdArray};
	my $pMinValue = $this->{pParameterMinValueArray};
	my $pMaxValue = $this->{pParameterMaxValueArray};
	$pVars =$this->{pVariables} if(!defined $pVars);

	if($IsDetail) {
		print("  Return value: $MinVal\n");
		for(my $i = 0 ; $i < @$pVars ; $i++) {
			next if(!$PrintAll and !$pId->[$i]);
			my $pVar = $pVars->[$i];
			$pVar = $$pVar if(ref $pVar =~ /ARRAY\(/);
			printf("    %2d: %20s[%d]: %12.6g (%s - %s)\n",
					$i, $pName->[$i], $pId->[$i], $pVar, $pMinValue->[$i], $pMaxValue->[$i]);
		}
		return;
	}

	if($MinVal =~ /^[+\-\d\.eE]+$/) {
		printf("f=%5.3g: ", $MinVal);
	}
	else {
		print("f=$MinVal: ");
	}
	for(my $i = 0 ; $i < @$pVars ; $i++) {
		next if(!$PrintAll and !$pId->[$i]);
		my $v = $pVars->[$i];
		if($i == 0) {
			printf("%6.4g", $v);
		}
		else {
			printf(",%6.4g", $v);
		}
		if(($i+1) % 9 == 0) {
			print("\n         ");
		}
	}
	print("\n");
}

sub Initialize
{
	my ($this) = @_;
	$this->SetnS2Calculation(0);
	$this->{pVariables}                  = ();
	$this->{pParameterNameArray}         = ();
	$this->{pVariableArray}              = ();
	$this->{pCoeffNormalize}             = ();
	$this->{pParameterInitialValueArray} = ();
	$this->{pParameterOptIdArray}        = ();
	$this->{pParameterScaleArray}        = ();
}

sub RecoverParameters
{
	my ($this, $pVars) = @_;
	my $pRecoverFunction = $this->{pParameterRecoverFunctionArray};
	return if(!defined $pRecoverFunction);

	if(!defined $pVars) {
		$pVars = $this->{pVariableArray};
		for(my $i = 0 ; $i < @$pVars ; $i++) {
			my $f    = $pRecoverFunction->[$i];
			my $pval = $pVars->[$i];
#print "Recover to [$$pval]\n";
#			&$f($$pval);
			&$f($pval);
		}
	}
	else {
		for(my $i = 0 ; $i < @$pVars ; $i++) {
			my $f   = $pRecoverFunction->[$i];
			my $val = $pVars->[$i];
#print "Recover to [$val]\n";
			&$f($val);
		}
	}
}

sub CalPenalty
{
	my ($this, $Penalty, $pVars, $IsPrint) = @_;
	my $pMinValue = $this->{pParameterMinValueArray};
	my $pMaxValue = $this->{pParameterMaxValueArray};
	my $pName     = $this->{pParameterNameArray};
	my $p = 0.0;
	for(my $i = 0 ; $i < @$pVars ; $i++) {
#print "pv[$i] = $pVars->[$i]\n";
		if($pMinValue->[$i] ne '' and $pVars->[$i] < $pMinValue->[$i]) {
print "   Optimize::CalPenalty:: Value[$pName->[$i]]=$pVars->[$i] (< $pMinValue->[$i])\n" if($IsPrint);
			$p += $Penalty;
		}
		if($pMaxValue->[$i] ne '' and $pVars->[$i] > $pMaxValue->[$i]) {
print "   Optimize::CalPenalty:: Value[$pName->[$i]]=$pVars->[$i] (> $pMaxValue->[$i])\n" if($IsPrint);
			$p += $Penalty;
		}
	}
	return $p;
}

sub AddParameters2
{
	my ($this, @params) = @_;
	my (@Name, @pVariable, @Guess, @CoeffNormalize, @OptId, @Scale, @MinValue, @MaxValue, @RecoverFunction);
	my $pn = $this->{pParameterNameArray};
	my $c = 0;
	if($pn) {
		$c               = @$pn;
		@Name            = @{$this->{pParameterNameArray}};
		@pVariable       = @{$this->{pVariableArray}};
		@Guess           = @{$this->{pParameterInitialValueArray}};
		@OptId           = @{$this->{pParameterOptIdArray}};
		@Scale           = @{$this->{pParameterScaleArray}};
		@MinValue        = @{$this->{pParameterMinValueArray}};
		@MaxValue        = @{$this->{pParameterMaxValueArray}};
		@RecoverFunction = @{$this->{pParameterRecoverFunctionArray}};
	}
	for(my $i = 0 ; $i < @params ; $i += 7) {
		my $name   = $params[$i];
		my $pVar   = $params[$i+1];
		my $id     = $params[$i+2];
		my $scale  = $params[$i+3];
		my $minVal = $params[$i+4];
		my $maxVal = $params[$i+5];
		my $rf     = $params[$i+6];
#print "name=$name,$pVar,$id,$scale, $rf\n";

		$Name[$c]            = $name;
		$pVariable[$c]       = $pVar;
		$Guess[$c]           = $$pVar;
		$OptId[$c]           = $id;
		$Scale[$c]           = $scale;
		$MinValue[$c]        = $minVal;
		$MaxValue[$c]        = $maxVal;
		$RecoverFunction[$c] = $rf;
#print "c=$c: id=$id ($name) $scale\n";
		$c++;
	}
	$this->{pParameterNameArray}            = \@Name;
	$this->{pVariableArray}                 = \@pVariable;
	$this->{pParameterInitialValueArray}    = \@Guess;
	$this->{pParameterOptIdArray}           = \@OptId;
	$this->{pParameterScaleArray}           = \@Scale;
	$this->{pParameterMinValueArray}        = \@MinValue;
	$this->{pParameterMaxValueArray}        = \@MaxValue;
	$this->{pParameterRecoverFunctionArray} = \@RecoverFunction;
	return (\@Name, \@pVariable, \@Guess, \@OptId, \@Scale);
}

sub AddParameters
{
	my ($this, @params) = @_;
	return $this->AddParameters2(@params) if($params[6] =~ /CODE\(0x/);

	my (@Name, @pVariable, @Guess, @CoeffNormalize, @OptId, @Scale);
	my $pn = $this->{pParameterNameArray};
	my $c = 0;
	if($pn) {
		$c               = @$pn;
		@Name            = @{$this->{pParameterNameArray}};
		@pVariable       = @{$this->{pVariableArray}};
		@Guess           = @{$this->{pParameterInitialValueArray}};
		@OptId           = @{$this->{pParameterOptIdArray}};
		@Scale           = @{$this->{pParameterScaleArray}};
	}
	for(my $i = 0 ; $i < @params ; $i += 4) {
		my $name  = $params[$i];
		my $pVar  = $params[$i+1];
		my $id    = $params[$i+2];
		my $scale = $params[$i+3];
		
#print "name=$name,$pVar,$id,$scale\n";
		$Name[$c]      = $name;
		$pVariable[$c] = $pVar;
		$Guess[$c]     = $$pVar;
		$OptId[$c]     = $id;
		$Scale[$c]     = $scale;
#print "c=$c: id=$id ($name) $scale\n";
		$c++;
	}
	$this->{pParameterNameArray}         = \@Name;
	$this->{pVariableArray}              = \@pVariable;
	$this->{pParameterInitialValueArray} = \@Guess;
	$this->{pParameterOptIdArray}        = \@OptId;
	$this->{pParameterScaleArray}        = \@Scale;
	return (\@Name, \@pVariable, \@Guess, \@OptId, \@Scale);
}

sub UpdateParameters
{
	my ($this) = @_;
	my $pointerVariables = $this->{pVariableArray};
	for(my $i = 0 ; $i < @$pointerVariables ; $i++) {
		${$pointerVariables->[$i]} = $this->{pVariables}->[$i];
	}
}

sub XVal
{
	my ($this, $y, $EPS, $nMaxIter, $xmin, $xmax, $xinit, $dx, $Debug) = @_;
	$EPS = 1.0e-6 if(!defined $EPS);
	$nMaxIter = 100 if(!defined $nMaxIter);
#	($xmin, $xmax) = $this->GetXMinMax() if(!defined $xmin or !defined $xmax);
	$dx = ($xmax - $xmin) * 0.01 if(!defined $dx);
	$xinit = 0.5 * ($xmin + $xmax) if(!defined $xinit);

#	return $this->XValByBinaryMethod($y, $EPS, $nMaxIter, $xmin, $xmax, $xinit, $dx, $Debug);
	return $this->XValByNewtonMethod($y, $EPS, $nMaxIter, $xmin, $xmax, $xinit, $dx, $Debug);
}

sub XValByNewtonMethod
{
	my ($this, $y, $EPS, $nMaxIter, $xmin, $xmax, $xinit, $dx, $Debug) = @_;
	my $x = $xinit;
	for(my $i = 0 ; $i <= $nMaxIter ; $i++) {
		if($i >= $nMaxIter) {
print "Error in Optimize::XValByNewtonMethod: Not converged in $nMaxIter iteration to EPS=$EPS.\n";
			return undef;
		}
		if((defined $xmin and $x < $xmin) or (defined $xmax and $xmax < $x)) {
print "Error in Optimize::XValByNewtonMethod: X [$x] gets out from the data range [$xmin - $xmax].\n";
			return undef;
		}
		my $y0  = $this->YCal($x);
		my $error =abs($y0 - $y);
		if($error < $EPS) {
print "Convergence reached at iter=$i within EPS=$error [$EPS].\n" if($Debug);
			last;
		}
		my $y1  = $this->YCal($x + $dx);
		my $dydx = ($y1 - $y0) / $dx;
		$x -= ($y0 - $y) / $dydx;
	}
	return $x;
}

sub YVal
{
	my ($this, $x) = @_;
	my $pFunc = $this->{YCalFunc};
	return &$pFunc($this, $x);
}

sub YCal
{
	my ($this, $x) = @_;
	my $pFunc = $this->{YCalFunc};
	return &$pFunc($this, $x);
}

sub CalS2
{
	my ($this) = @_;
	my $pX = $this->{pX};
	my $pY = $this->{pY};
	my $S2 = 0.0;
	for(my $i = 0 ; $i < @$pX ; $i++) {
		my $x    = $pX->[$i];
		my $yobs = $pY->[$i];
		my $ycal = $this->YCal($x);
		$S2 += ($ycal - $yobs)**2;
	}
	return $S2;
}

#==============================================
# 多項式線形最小二乗法の自作ルーチン
#==============================================
sub mlsqYCal
{
	my ($this, $x, $pVars) = @_;
	$pVars = $this->{pVariables} if(!defined $pVars);
	my $ycal = 0.0;
	my $xn = 1.0;
	for(my $i = 0 ; $i < @$pVars ; $i++) {
		$ycal += $pVars->[$i] * $xn;
		$xn *= $x;
	}
	return $ycal;
}

sub mlsqFunc
{
	my ($idx, $x) = @_;
	return 1.0 if($idx == 0);
	return $x**$idx;
}

sub mlsq
{
	my ($this, $pX, $pY, $m, $iPrintLevel) = @_;
	$iPrintLevel = 0 if(!defined $iPrintLevel);

	$this->{pX}       = $pX;
	$this->{pY}       = $pY;
	$this->{pFunc}    = \&mlsqFunc;
	$this->{YCalFunc} = \&mlsqYCal;
	my $n = @$pX;
	my $Si  = new Math::MatrixReal($m+1, 1);
	my $Sij = new Math::MatrixReal($m+1, $m+1);
	for(my $l = 0 ; $l <= $m ; $l++) {
		my $v = 0.0;
		for(my $i = 0 ; $i < $n ; $i++) {
			$v += $pY->[$i] * $pX->[$i]**$l;
		}
		$Si->assign($l+1, 1, $v);
	}
	for(my $j = 0 ; $j <= $m ; $j++) {
		for(my $l = $j ; $l <= $m ; $l++) {
			my $v = 0.0;
			for(my $i = 0 ; $i < $n ; $i++) {
				$v += $pX->[$i]**($j+$l);
			}
			$Sij->assign($j+1, $l+1, $v);
			$Sij->assign($l+1, $j+1, $v);
		}
	}
print "Si=\n",  $Si  if($iPrintLevel);
print "Sij=\n", $Sij if($iPrintLevel >= 2);
#対角成分の数値を格納。Sijに各行方程式の係数を規格化した結果を格納する
	my @kj;
	for(my $j = 0 ; $j < $m+1 ; $j++) {
		$kj[$j] = 1.0 / $Sij->element($j+1, $j+1);
	}
	my $Sij2 = new Math::MatrixReal($m+1, $m+1);
	for(my $i = 0 ; $i < $m+1 ; $i++) {
		for(my $j = 0 ; $j < $m+1 ; $j++) {
			$Sij2->assign($i+1, $j+1, $kj[$j] * $Sij->element($i+1, $j+1));
		}
	}
print "Sij2=\n", $Sij2 if($iPrintLevel);
#最適化した変数を計算する
	my $LR = $Sij2->decompose_LR();
	my ($dim, $solution, $base_matrix) = $LR->solve_LR($Si);
	my @c;
	for(my $i = 0 ; $i <= $m ; $i++) {
		$c[$i] = $solution->element($i+1, 1) * $kj[$i];
	}
	$this->{pVariables} = \@c;

	return \@c;
}

sub mlsqYCal2
{
	my ($this, $x, $pVars) = @_;
#print "this=$this x=$x pVars=$pVars\n";
	$pVars = $this->{pVariables} if(!defined $pVars);
	my $ycal = 0.0;
	for(my $i = 0 ; $i < @$pVars ; $i++) {
		my $n = $this->{pm}->[$i];
		$ycal += $pVars->[$i] * $x**$n;
	}
	return $ycal;
}

sub mlsqFunc2
{
	my ($this, $idx, $x) = @_;
#print "($this, $idx, $x)\n";
	my $n = $this->{pm}->[$idx];
	return 1.0 if($n == 0);
	return $x**$n;
}

sub mlsq2
{
	my ($this, $pX, $pY, $pm, $iPrintLevel) = @_;
	$iPrintLevel = 0 if(!defined $iPrintLevel);

	$this->{pX}       = $pX;
	$this->{pY}       = $pY;
	$this->{pm}       = $pm;
	$this->{pFunc}    = sub { $this->mlsqFunc2(@_); };
	$this->{YCalFunc} = sub { mlsqYCal2(@_); };

	my $Opt = new Optimize;
	my ($pCi, $S2) = $Opt->Optimize(
					"lsqLCFunctions", $pX, $pY, scalar @$pm, 
					sub { $this->mlsqFunc2(@_); },
					$iPrintLevel
					);
	$this->{pVariables} = $pCi;
	$this->{MinVal}     = $S2;

	return ($pCi, $S2);
}

#==============================================
# 関数の線形和：最小二乗法の自作ルーチン
#==============================================
sub LCFunctionsYCal
{
	my ($this, $x, $pVars) = @_;
	$pVars = $this->{pVariables} if(!defined $pVars);
	my $pFunc = $this->{pFunc};
	my $ycal = 0.0;
	my $xn = 1.0;
	for(my $i = 0 ; $i < @$pVars ; $i++) {
		$ycal += $pVars->[$i] * &$pFunc($i, $x);
	}
	return $ycal;
}

sub lsqLCFuncions
{
	my ($this, $pX, $pY, $nParams, $pFunc, $iPrintLevel) = @_;
#print "this=$this  pX=$pX  pY=$pY  nParams=$nParams  pFunc=$pFunc  iPrintLevel=$iPrintLevel\n";
	$this->{pX}       = $pX;
	$this->{pY}       = $pY;
	$this->{pFunc}    = $pFunc;
	$this->{YCalFunc} = \&LCFunctionsYCal;
	my $nData = @$pX;
	my $Si  = new Math::MatrixReal($nParams, 1);
	my $Sij = new Math::MatrixReal($nParams, $nParams);
	for(my $l = 0 ; $l < $nParams ; $l++) {
		my $v = 0.0;
		for(my $i = 0 ; $i < $nData ; $i++) {
			$v += $pY->[$i] * &$pFunc($l, $pX->[$i]);
		}
		$Si->assign($l+1, 1, $v);
	}
	for(my $j = 0 ; $j < $nParams ; $j++) {
		for(my $l = $j ; $l < $nParams ; $l++) {
			my $v = 0.0;
			for(my $i = 0 ; $i < $nData ; $i++) {
				$v += &$pFunc($j, $pX->[$i]) *  &$pFunc($l, $pX->[$i]);
			}
			$Sij->assign($j+1, $l+1, $v);
			$Sij->assign($l+1, $j+1, $v);
		}
	}
print "Si=\n",  $Si  if($iPrintLevel);
print "Sij=\n", $Sij if($iPrintLevel >= 2);
#対角成分の数値を格納。Sijに各行方程式の係数を規格化した結果を格納する
	my @kj;
	for(my $j = 0 ; $j < $nParams ; $j++) {
		$kj[$j] = 1.0 / $Sij->element($j+1, $j+1);
	}
	my $Sij2 = new Math::MatrixReal($nParams, $nParams);
	for(my $i = 0 ; $i < $nParams ; $i++) {
		for(my $j = 0 ; $j < $nParams ; $j++) {
			$Sij2->assign($i+1, $j+1, $kj[$j] * $Sij->element($i+1, $j+1));
		}
	}
print "Sij2=\n", $Sij2 if($iPrintLevel);
#最適化した変数を計算する
	my $LR = $Sij2->decompose_LR();
	my ($dim, $solution, $base_matrix) = $LR->solve_LR($Si);
	my @c;
	for(my $i = 0 ; $i < $nParams ; $i++) {
		$c[$i] = $solution->element($i+1, 1) * $kj[$i];
	}
	$this->{pVariables} = \@c;

	return \@c;
}

#==============================================
# 線形最小二乗法の自作ルーチン
# Differentiate1, 2も呼び出す
#==============================================
sub LinearOptimization
{
	my ($pVariables, $pFunc, $iPrintLevel, $pBuildDifferentialMatrixesFunc) = @_;
	$pBuildDifferentialMatrixesFunc = \&BuildDifferentialMatrixes 
			if(!defined $pBuildDifferentialMatrixesFunc);

	my $n = @$pVariables;
	my $S = &$pFunc(@$pVariables);
#一,二次微分の計算
	my ($Si, $Sij) = &$pBuildDifferentialMatrixesFunc($pFunc, $pVariables);
#対角成分の数値を格納。Sijに各行方程式の係数を規格化した結果を格納する
	my @kj;
	for(my $j = 0 ; $j < $n ; $j++) {
		$kj[$j] = 1.0 / $Sij->element($j+1, $j+1);
	}
	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));
		}
	}
#最適化した変数を計算する
if(0) {
	my $RSij2 = $Sij2->inverse();
	my $V = -$RSij2 * $Si;
	for(my $i = 0 ; $i < $n ; $i++) {
		$pVariables->[$i] = $pVariables->[$i] + $V->element($i+1, 1) * $kj[$i];
	}
}
if(1) {
	my $LR = $Sij2->decompose_LR();
	my ($dim, $solution, $base_matrix) = $LR->solve_LR($Si);
	for(my $i = 0 ; $i < $n ; $i++) {
		$pVariables->[$i] = $pVariables->[$i] - $solution->element($i+1, 1) * $kj[$i];
	}
}

	my $NewS = &$pFunc(@$pVariables);
#print "S: $S => $NewS\n";
	return $NewS;
}

#==============================================
# 修正Newton法(+CG)の自作ルーチン
# Differentiate1, 2も呼び出す
#==============================================
sub SetDiffV
{
	my ($this, $diffv) = @_;
	$diffv = 0.05 if(!defined $diffv);
	return $this->{diffv} = $diffv;
}

sub SetDumpingFactor
{
	my ($this, $Dump0, $Dump1, $nDump) = @_;

	if(!defined $Dump1) {
		$Dump1 = $Dump0;
		$nDump = 1;
	}
	if($Dump0 > $Dump1) {
		($Dump0, $Dump1) = ($Dump1, $Dump0);
	}

	my $DumpStep;
	if($nDump <= 1) {
		$nDump = 1;
		$DumpStep = $Dump1 - $Dump0;
	}
	else {
		$DumpStep = ($Dump1 - $Dump0) / ($nDump - 1);
	}

	$this->{DumpingFactorStart} = $Dump0;
	$this->{DumpingFactorEnd}   = $Dump1;
	$this->{DumpingFactorStep}  = $DumpStep;
	$this->{nDumpingFactor}     = $nDump;

	return ($Dump0, $Dump1, $DumpStep, $nDump);
}

sub ModifiedNewton1D
{
	my ($this, $pVariables, $pFunc, $EPS, $nMaxIter, $iPrintLevel, $pBuildDifferentialMatrixesFunc) = @_;
	my $diffv = (defined $this->{diffv})? $this->{diffv} : 0.05;
	$iPrintLevel = 0 if(!defined $iPrintLevel);
	$pBuildDifferentialMatrixesFunc = \&BuildDifferentialMatrixes if(!defined $pBuildDifferentialMatrixesFunc);
#$iPrintLevel = 1;
	$nMaxIter = 10 if(!defined $nMaxIter);
	$EPS      = 1.0e-6 if(!defined $EPS);
	my $Dump0 = $this->{DumpingFactorStart};

print "diffv=$diffv\n" if($iPrintLevel > 0);

	for(my $iter = 0 ; $iter < $nMaxIter ; $iter++) {
		my $v0 = $pVariables->[0];
		my $S  = &$pFunc(@$pVariables);
#一,二次微分の計算
		my ($Si, $Sij) = &$pBuildDifferentialMatrixesFunc($pFunc, $pVariables, $diffv);
		my $S1 = $Si->element(1, 1);
		my $S2 = $Sij->element(1, 1);

		my $dx = -$S1 / ($Dump0 + 1.0) / $S2;
		$pVariables->[0] = $v0 + $dx;
		my $MinS = &$pFunc(@$pVariables);
print "iter[$iter]: x = $pVariables->[0] S=$S => $MinS (dx=$dx  EPS=$EPS)\n" if($iPrintLevel > 0);
		if(abs($dx) < $EPS) {
			return $MinS;
		}
	}

	return -1;
}

sub ModifiedNewton
{
	my ($this, $pVariables, $pFunc, $EPS, $nMaxIter, $iPrintLevel, $pBuildDifferentialMatrixesFunc) = @_;
	my $diffv = (defined $this->{diffv})? $this->{diffv} : 0.05;
	$iPrintLevel = 0 if(!defined $iPrintLevel);
	$pBuildDifferentialMatrixesFunc = \&BuildDifferentialMatrixes if(!defined $pBuildDifferentialMatrixesFunc);
#$iPrintLevel = 1;
	$nMaxIter = 10 if(!defined $nMaxIter);
	$EPS = 1.0e-6 if(!defined $EPS);

print "diffv=$diffv\n" if($iPrintLevel > 0);

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

#一,二次微分の計算
		my ($Si, $Sij) = &$pBuildDifferentialMatrixesFunc($pFunc, $pVariables, $diffv);
#print "iter=$iter: Si=$Si  Sij=$Sij\n";

#for(my $j = 1 ; $j < $n+1 ; $j++) {
#	print "Si[$j]=", $Si->element($j,1), "\n";
#	for(my $k = 1 ; $k < $n+1 ; $k++) {
#		print "  Sij[$j][$k]=", $Sij->element($j, $k), "\n";
#	}
#}
#対角成分の数値を格納
		my @kj;
		for(my $j = 0 ; $j < $n ; $j++) {
			$kj[$j] = 1.0 / $Sij->element($j+1, $j+1);
		}

#Dumping factorを変えながら最小点を探す
		my $dxsign   = 1.0;
		my $MinS     = $S;
		my $PrevS    = 1.0e100;
		my $MinDump  = -1;
		my $Dump0    = $this->{DumpingFactorStart};
		my $DumpStep = $this->{DumpingFactorStep};
		my $nDump    = $this->{nDumpingFactor};
		my @MinV;
		for(my $id = 0 ; $id < $nDump ; $id++) {
#			my $Dump = 0.01 * (2**abs($id));
			my $Dump = $Dump0 + $id * $DumpStep;
#print "id=$id Dump=$Dump dxsign=$dxsign\n";
#Sijに各行方程式の係数を規格化した結果を格納する
			my $Si2  = new Math::MatrixReal($n, 1);
			my $Sij2 = new Math::MatrixReal($n, $n);
			for(my $i = 0 ; $i < $n ; $i++) {
#				$Si2->assign($i+1, 1, $kj[$i] * $Si->element($i+1, 1));
				for(my $j = 0 ; $j < $n ; $j++) {
#					$Sij2->assign($i+1, $j+1, $kj[$j] * $Sij->element($i+1, $j+1));
					$Sij2->assign($i+1, $j+1, $Sij->element($i+1, $j+1));
				}
#対角成分にDumping factorを加える
#				$Sij2->assign($i+1, $i+1, $Sij2->element($i+1, $i+1) + $Dump);
				$Sij2->assign($i+1, $i+1, $Sij2->element($i+1, $i+1) * (1.0 + $Dump));
			}
#print "Sij:\n$Sij";
			my $RSij2 = $Sij2->inverse();
#print "Inverse Sij:\n$RSij";

#変数勾配の計算
#			my $V = -$RSij2 * $Si2;
			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] * $dxsign;
#print "  v[$i](d=$Dump dxsign=$dxsign): $pVariables->[$i] => $NewV[$i]\n";
			}
#for(my $i = 0 ; $i < $n ; $i++) {
#	print "**NewV[$i]=$NewV[$i]\n";
#}
			my $NewS = &$pFunc(@NewV);
print "  NewS1 (d=$Dump dxsign=$dxsign): $NewS\n" if($iPrintLevel > 0);
			if($NewS < $MinS) {
				$MinS    = $NewS;
				$MinDump = $Dump;
				@MinV = @NewV;
#				for(my $i = 0 ; $i < $n ; $i++) {
#					$MinV[$i] = $NewV[$i];
#				}
			}
			else {
print "    Warning: S increased. dxsign is changed from $dxsign to ", -$dxsign, ".\n" if($iPrintLevel > -1);
				$dxsign *= -1.0;
				for(my $i = 0 ; $i < $n ; $i++) {
					$NewV[$i] = $pVariables->[$i] + $V->element($i+1, 1) * $kj[$i] * $dxsign;
#print "  v[$i](d=$Dump dxsign=$dxsign): $pVariables->[$i] => $NewV[$i]\n";
				}
if($iPrintLevel > -1) {
for(my $i = 0 ; $i < $n ; $i++) {
	print "**NewV[$i]=$NewV[$i]\n";
}
}
				my $NewS = &$pFunc(@NewV);
print "  NewS2 (d=$Dump dxsign=$dxsign): $NewS\n" if($iPrintLevel > 0);
				if($NewS < $MinS) {
					$MinS    = $NewS;
					$MinDump = $Dump;
					@MinV = @NewV;
#					for(my $i = 0 ; $i < $n ; $i++) {
#						$MinV[$i] = $NewV[$i];
#					}
				}
			}
			last if($NewS > $PrevS);
			$PrevS = $NewS;
		}
print "  MinS=$MinS\n" if($iPrintLevel > 0);
		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] => " if($iPrintLevel > 0);
				$pVariables->[$i] = $MinV[$i];
print "$MinV[$i]\n" if($iPrintLevel > 0);
			}
#収束判定
#print "   MaxD=$MaxD (EPS=$EPS,diffv=$diffv)\n";
			if($MaxD < $EPS) {
				print "  Convergence reached.: Stop Optimization\n" if($iPrintLevel > 0);
				return $MinS;
			}
		}
		else {
#最小点がみつからなかったら止める
			print "  Warning: Better solution could not be found.\n" if($iPrintLevel > 0);
			print "    Stop Optimization\n" if($iPrintLevel > 0);
			return $MinS;
		}
	}

	return -1;
}

sub BuildDifferentialMatrixes
{
	my ($pFunc, $pVariables, $diffv) = @_;
	my $Si  = &BuildDifferentialVector($pFunc, $pVariables, $diffv);
	my $Sij = &BuildDifferentialMatrix($pFunc, $pVariables, $diffv);
#print "Si=$Si\n";
#print "Si=", $Si->element(1,1), "   Sij=", $Sij->element(1,1), "\n";
	return ($Si, $Sij);
}

sub BuildDifferentialVector
{
	my ($pFunc, $pVariables, $diffv) = @_;
	my $n = @$pVariables;
	my $Si = new Math::MatrixReal($n, 1);
#print "Si=$Si\n";
#print "n=$n\n";
	for(my $i = 0 ; $i < $n ; $i++) {
		my $val = &Differentiate1($pFunc, $pVariables, $i, $diffv);
#print "dFdx[$i]=$val\n";
		$Si->assign($i+1, 1, $val);
	}
	return $Si;
}

sub BuildDifferentialMatrix
{
	my ($pFunc, $pVariables, $diffv) = @_;
	my $n = @$pVariables;
	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, $diffv);
#print "dF2dx2[$i][$j]=$val\n";
			$Sij->assign($i+1, $j+1, $val);
			$Sij->assign($j+1, $i+1, $val) if($i != $j);
		}
	}
	return $Sij;
}

sub Differentiate1
{
	my ($pFunc, $pVariables, $idx, $dv) = @_;
	if(!defined $dv or $dv == 0.0) {
		$dv = abs($pVariables->[$idx]) * 0.0005;
		$dv = 0.0005 if($dv < 0.0005);
	}

	my @v = @$pVariables;
	$v[$idx] = $pVariables->[$idx] + $dv;
	my $Sp1  = &$pFunc(@v);
	$v[$idx] = $pVariables->[$idx] - $dv;
	my $Sm1  = &$pFunc(@v);
	my $dSdv = ($Sp1 - $Sm1) / $dv / 2.0;

	return $dSdv;
}

sub Differentiate2
{
	my ($pFunc, $pVariables, $idx, $jdx, $dv) = @_;
	if(!defined $dv or $dv == 0.0) {
		$dv = abs($pVariables->[$idx]) * 0.0005;
		$dv = 0.0005 if($dv < 0.0005);
	}

	my @v = @$pVariables;
	$v[$jdx]     = $pVariables->[$jdx] + $dv;
	my $dSdxip1j = &Differentiate1($pFunc, \@v, $idx, $dv);
	$v[$jdx]     = $pVariables->[$jdx] - $dv;
	my $dSdximj  = &Differentiate1($pFunc, \@v, $idx, $dv);

	return ($dSdxip1j - $dSdximj) / $dv / 2.0;
}

#============================================================
#　関数
#============================================================
sub Optimize
{
	my $this   = shift;
	my $Method = shift;
	$this->{Method} = $Method;
	if($Method =~ /^mlsq$/i) {
		my $pCoeff = $this->mlsq(@_);
		my $S2     = $this->CalS2();
		return ($pCoeff, $S2);
	}
	elsif($Method =~ /^mlsq2$/i) {
		my ($pCoeff, $S2) = $this->mlsq2(@_);
		return ($pCoeff, $S2);
	}
	elsif($Method =~ /LCFunc/i) {
		my $pCoeff = $this->lsqLCFuncions(@_);
		my $S2     = $this->CalS2();
		return ($pCoeff, $S2);
	}
	my ($pInitial, $pOptId, $pScale, 
		$EPS, $nMaxIter, $iPrintLevel,
		$pMinimizationFunc, $pPDLDisplayFunc,
		$pBuildDifferentialMatrixesFunc
		) = @_;
#print "  EPS=$EPS\n";
	$pInitial = $this->{pParameterInitialValueArray} if(!$pInitial);
	$pOptId   = $this->{pParameterOptIdArray}        if(!$pOptId);
	$pScale   = $this->{pParameterScaleArray}        if(!$pScale);
#print "p=[$pInitial][$pOptId][$pScale]\n";
	$this->{iPrintLevel} = $iPrintLevel;
	my $pGuess          = [];
	my $pScaleExtracted = [];
	my $c = 0;
	for(my $i = 0 ; $i < @$pInitial ; $i++) {
		next if(!$pOptId->[$i]);
#print "Optimize for $i ($c)\n";		
		$pGuess->[$c]          = $pInitial->[$i];
		$pScaleExtracted->[$c] = $pScale->[$i];
		$c++;
	}
	if($c == 0) {
		print "Error in Optimized::Optimize: No variable specified\n\n";
		return 0;
	}

	if($Method =~ /Amoeba::Simplex/i) {
		my $verbos = 0;
		$verbos = 1 if($iPrintLevel);
#print "EPS=$EPS, n=$nMaxIter\n";
		my ($pOptVars, $MinVal) = MinimiseND($pGuess, $pScaleExtracted, 
				sub {&$pMinimizationFunc(&RecoverParams(\@_, $pInitial, $pOptId), $iPrintLevel);}, 
				$EPS, $nMaxIter, $verbos);
		$pOptVars = &RecoverParams($pOptVars, $pInitial, $pOptId);
#print "(",join(',',@{$pOptVars}),")=$MinVal\n";
		$this->{MinVal}     = $MinVal;
		$this->{pVariables} = $pOptVars;
		return ($pOptVars, $MinVal);
	}
	elsif($Method =~ /ModifiedNewton1D/i) {
		my $MinVal = $this->ModifiedNewton1D($pGuess, 
			sub {&$pMinimizationFunc(&RecoverParams(\@_, $pInitial, $pOptId), $iPrintLevel);}, 
			$EPS, $nMaxIter, $iPrintLevel,
			$pBuildDifferentialMatrixesFunc);
		my $pOptVars = &RecoverParams($pGuess, $pInitial, $pOptId);
#print "(",join(',',@$pOptVars),")=$MinVal\n";
		$this->{MinVal}     = $MinVal;
		$this->{pVariables} = $pOptVars;
		return ($pOptVars, $MinVal);
	}
	elsif($Method =~ /^ModifiedNewton$/i) {
		my $MinVal = $this->ModifiedNewton($pGuess, 
			sub {&$pMinimizationFunc(&RecoverParams(\@_, $pInitial, $pOptId), $iPrintLevel);}, 
			$EPS, $nMaxIter, $iPrintLevel,
			$pBuildDifferentialMatrixesFunc);
		my $pOptVars = &RecoverParams($pGuess, $pInitial, $pOptId);
#print "(",join(',',@$pOptVars),")=$MinVal\n";
		$this->{MinVal}     = $MinVal;
		$this->{pVariables} = $pOptVars;
		return ($pOptVars, $MinVal);
	}
	elsif($Method =~ /LinearOptimization/i) {
		my $MinVal = &LinearOptimization($pGuess, 
			sub {&$pMinimizationFunc(&RecoverParams(\@_, $pInitial, $pOptId), $iPrintLevel);}, 
			$iPrintLevel,
			$pBuildDifferentialMatrixesFunc);
		my $pOptVars = &RecoverParams($pGuess, $pInitial, $pOptId);
#print "(",join(',',@$pOptVars),")=$MinVal\n";
		$this->{MinVal}     = $MinVal;
		$this->{pVariables} = $pOptVars;
		return ($pOptVars, $MinVal);
	}
	elsif($Method =~ /PDL::Simplex/i) {
		$pPDLDisplayFunc = sub {} if(!defined $pPDLDisplayFunc);
		my $init = pdl( [@$pGuess] );
#print "init=$init\n";
		my $initsize = @$pGuess;
		my ($optimum, $ssize) = simplex($init, $initsize, $EPS, $nMaxIter,
			sub { &$pMinimizationFunc(&RecoverParams(&PDLtoListRef($_[0]), $pInitial, $pOptId), $iPrintLevel); },
			sub { &$pPDLDisplayFunc($_[0]); }
#			sub { &$pPDLDisplayFunc(&RecoverParams(&PDLtoListRef($_[0]), $pInitial, $pOptId)); }
			);
# 2重のpiddleで$optimumが返ってくる
		my @a = PDLtoList($optimum);
#print "optimum=$optimum}\n";
#print "ssize=$ssize\n";
		$this->{MinVal}     = $ssize;
		$this->{pVariables} = \@a;
		return (\@a, $ssize);
	}
	else {
		print "Error: Invalid Method [$Method].\n";
		return 0;
	}
	return 0;
}

1;