#!/usr/bin/perl
#
# Make perfect data with a 1-D gaussian
#    Just one gaussian, at a given location
#
# Then, given an initial guess at amplitude and time,
#    try to find those two parameters using the
#    method of steepest ascent.

use POSIX;

$true_amp = 4.68;
$fwhm = 1.0;
$sigma = $fwhm / 2.35;
$true_t0 = 7.55;


$ndata = 0;
$start_t = 0;
$end_t = 20.0;
$dt = 0.1;
for ($t = $start_t; $t < $end_t; $t += $dt) {

  $arg = 0.0 - ($t - $true_t0)*($t - $true_t0)/(2.0*$sigma*$sigma);
  $y = $true_amp*exp($arg);
  $t_array[$ndata] = $t;
  $y_array[$ndata] = $y;
  $ndata++;

  printf "# %8.3f %8.3f \n", $t, $y;

}


##################################
# now, the fitting part

# our initial guess
$guess_amp = 4.0;
$guess_t = 7.0;



# make a scan around guessed location:
#   compute the K-squared statistic at each point
$sigma = 1.0/2.35;
$nscan = 4;
$amp_range = 1.0;
$t_range = 1.0;


if (0 == 1) {
		  $min_amp = $guess_amp - $amp_range;
		  $max_amp = $guess_amp + $amp_range;
		  $d_amp = ($max_amp - $min_amp) / $nscan;
		  $min_t = $guess_t - $t_range;
		  $max_t = $guess_t + $t_range;
		  $d_t = ($max_t - $min_t) / $nscan;
		  for ($amp = $min_amp; $amp <= $max_amp; $amp += $d_amp) {
			 for ($t = $min_t; $t <= $max_t; $t += $d_t) {

				$sum = 0;
				for ($i = 0; $i < $ndata; $i++) {
				  $arg = (($t_array[$i] - $t)*($t_array[$i] - $t))/(2.0*$sigma*$sigma);
				  $model = $amp*exp(0.0 - $arg);
				  $data = $y_array[$i];
				  $diff = $model - $data;
				  $sum += $diff*$diff;
				}

				# print out result
				printf "   amp %9.4f  t %9.4f    ksq %9.4f \n", $amp, $t, $sum;

			 }
		  }
}


######
# find the gradient and walk along it


# pick initial stepsize for d_t and d_amp
$d_t = 0.5;
$d_amp = 0.5;
$d_h = 0.1;
$h_range = 1.0;

$niter = 20;

# iterate 
for ($i_iter = 0; $i_iter < $niter; $i_iter++) {

        $ksq = comp_ksq($guess_amp, $guess_t);

        printf " iter %3d  amp %9.4f  t %9.4f   ksq  %9.4f \n", 
               $i_iter, $guess_amp, $guess_t, $ksq;
  
		  # first, find slope in t
		  $d_t = 0.5;
		  $top =  comp_ksq($guess_amp, $guess_t + $d_t) - 
					 comp_ksq($guess_amp, $guess_t - $d_t);
		  $bot = 2.0*$d_t;
		  $slope_t = $top/$bot;
        $sign_t = 1;
        if ($slope_t > 0) {
          $sign_t = -1;
        }

		  # now, find slope in amp
		  $d_amp = 0.5;
		  $top =  comp_ksq($guess_amp + $d_amp, $guess_t) - 
					 comp_ksq($guess_amp - $d_amp, $guess_t);
		  $bot = 2.0*$d_amp;
		  $slope_amp = $top/$bot;
        $sign_amp = 1;
        if ($slope_amp > 0) {
          $sign_amp = -1;
        }

		  $t_frac = fabs($slope_t)/(sqrt(($slope_amp*$slope_amp) + ($slope_t*$slope_t)));
		  $amp_frac = fabs($slope_amp)/(sqrt(($slope_amp*$slope_amp) + ($slope_t*$slope_t)));

        $min_ksq = 1e99;
        $min_amp = -1.0;
        $min_t = -1.0;
		  for ($h = 0; $h < $h_range; $h += $d_h) {
			 $new_amp = $guess_amp + $sign_amp*$h*$amp_frac;
			 $new_t = $guess_t + $sign_t*$h*$t_frac;
			 $ksq = comp_ksq($new_amp, $new_t);
			 printf "  h %9.4f  amp %9.4f  t %9.4f  ksq %9.4f \n",
					 $h, $new_amp, $new_t, $ksq;
          if ($ksq < $min_ksq) {
            $min_ksq = $ksq;
            $min_amp = $new_amp;
            $min_t = $new_t;
          }
          if ($ksq > $min_ksq) {
            # we have gone too far (if this is monotonic)
            # so stop
            last;
          }
		  }

        if ($min_ksq == 1e99) {
          printf " error: did not find a minimum value of ksq h = 0 to %lf \n",
                 $h_range;
          exit(1);
        }

   # update the best guess so far
   $guess_amp = $min_amp;
   $guess_t = $min_t;

   # decrease the size of changes in amp, t used to find gradient
   $d_t *= 0.2;
   $d_amp *= 0.5;
   $d_h *= 0.5;

   

}














#####################################################
# Compute K-square metric for a given (amp, t) pair
# Returns the metric

sub comp_ksq {

  my($amp, $t);
  my($sum, $i, $arg, $model, $data);
  my($diff);

  $amp = $_[0];
  $t = $_[1];

	 $sum = 0;
	 for ($i = 0; $i < $ndata; $i++) {
		$arg = (($t_array[$i] - $t)*($t_array[$i] - $t))/(2.0*$sigma*$sigma);
		$model = $amp*exp(0.0 - $arg);
		$data = $y_array[$i];
		$diff = $model - $data;
		$sum += $diff*$diff;
	 }

  return($sum);
}

