/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Include., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <iostream>
#include <fstream>
#include <string>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#ifndef __APPLE__
#include <byteswap.h> 
#endif

#include "../gnuplot-i/gnuplot_i-2.10/src/gnuplot_i.h"

#define VERBOSE 0
#define RESET_CHISQ 0
#define FIND_BACKGROUND 1
#define FIND_FWHM 1
#define FIND_HEIGHT 1
#define FIND_BACKSCATTER_PEAK 1
#define QUAD_STRING "read quad"

int 
main (int argc, char * const argv[]) 
{
  const uint32_t N_BINS = 256;
  
  std::string progname, infileName, outfileName, quadrant_str;
  uint32_t quadrant;
	
  if (argc != 3)
    {
      std::cout << "usage: " << argv[0] << " infilename quadrant" << std::endl;
      exit (0);
    }

  progname = argv[0];
  infileName = argv[1];
  quadrant = atoi (argv[2]);

  if (quadrant < 1 || quadrant >> 3)
    {
      std::cout << "quadrant must be in range [1..3]" << std::endl;
      exit (0);
    }

  char s[256];
  sprintf (s, "%d", quadrant);
  quadrant_str = s;

  char quadrantFlagStr[256];
  sprintf (quadrantFlagStr, "%s %1d", QUAD_STRING, quadrant);
  std::cout << "Look for quadrant section string " << quadrantFlagStr << std::endl;

  outfileName = infileName + "." + quadrant_str;

  std::ifstream infile (infileName.c_str ());

  uint32_t data[N_BINS];

  while (!infile.eof() && infile.good ())
    {
      std::cout << ".";
      char buffer[256];
      infile.getline (buffer, sizeof (buffer));
      if (strstr (buffer, quadrantFlagStr))
        {
          std::cout << std::endl << "Found quadrant histo data" << std::endl;

          for (uint32_t i = 0; i < N_BINS; ++i)
            {
              //
              // for some reason, an FF is printed first
              //
              infile.getline (buffer, sizeof (buffer));
              //
              // Then a ^M followed by a four digit hex value
              //
              infile.getline (buffer, sizeof (buffer));
              data[i] = strtoul (buffer + 1, 0, 16);
            }
          break;
        }
    }

  infile.close ();

  //
  // Find the highest count in the histo.  We assume this is the photopeak.
  // Find the first non-zero count.  Assume anything below that bin is cutoff.
  //
  uint32_t cutoff_bin = N_BINS;
  int32_t histo_max = 0;
  int32_t histo_max_bin = N_BINS;

  for (uint32_t i = 0; i < N_BINS; ++i)
    {
      if (data[i] > histo_max)
        {
          histo_max = data[i];
          histo_max_bin = i;
        }
    }

  for (uint32_t i = N_BINS - 1; i; --i)
    {
      if (data[i])
        {
          cutoff_bin = i;
        }
    }

  std::cout << "Looking for photopeak" << std::endl;

  // ----------------------------------------------------------------------
  // Find the first peak from the right (high energy) side.  This is presumably
  // the photopeak.
  //
  // Slide a window for the fit into the histo from the right, starting from a
  // shape with the peak at the right side of the histo down to a minimum legal
  // photopeak energy.
  //
  // The criterion for the best candidate for a photopeak is the lowest chi-square
  // statistic of the candidate photopeak as compared to a Lorentzian shape with 
  // the same FWHM and maximum.
  // ---------------------------------------------------------------------------

  uint32_t model[N_BINS];

  double lowest_chisq;

  const int32_t PEAK_ENERGY_MIN = 30;

  const int32_t PEAK_FWHM_MIN = 30;
  const int32_t PEAK_FWHM_MAX = 100;

  const int32_t PEAK_FWHM_START = PEAK_FWHM_MIN;  // better to start small


  const int32_t BACKGROUND_MIN = 1;
  const int32_t BACKGROUND_MAX = 200;

  //
  // Start with some initial guesses
  //
  int32_t photopeak_center = N_BINS;
  int32_t photopeak_fwhm = PEAK_FWHM_START;
  int32_t photopeak_hwhm = photopeak_fwhm / 2;
  int32_t photopeak_max = histo_max;
  int32_t photopeak_background = 0;
  //
  // Loop until nothing changes (we've found the best fit).  This is an
  // adaptive fit for performance reasons.  That is, we do different 
  // things at different iterations.
  //
  for (uint32_t iteration = 0; iteration < 4; ++iteration) {
    std::cout << "  Iterate (" << iteration << ")" << std::endl;

    int32_t last_photopeak_center = photopeak_center;
    int32_t last_photopeak_fwhm = photopeak_fwhm;
    int32_t last_photopeak_hwhm = photopeak_hwhm;
    int32_t last_photopeak_max = photopeak_max;
    int32_t last_photopeak_background = photopeak_background;

    lowest_chisq = std::numeric_limits<double>::max ();
  
    uint32_t centerStart = 0, centerEnd = 0, centerIncrement = 0;
    uint32_t backgroundStart = 0, backgroundEnd = 0, backgroundIncrement = 0;

    switch (iteration)
      {
      case 0:
        centerStart = N_BINS - photopeak_hwhm;
        centerEnd = PEAK_ENERGY_MIN;
        centerIncrement = 5;

        backgroundStart = BACKGROUND_MIN;
        backgroundEnd = BACKGROUND_MAX;
        backgroundIncrement = 5;
        break;
      default:
        centerStart = last_photopeak_center + 20;
        centerEnd = last_photopeak_center - 20;
        centerIncrement = 1;

        backgroundStart = 
          ((int32_t)last_photopeak_background - 20) < 1
          ? BACKGROUND_MIN : last_photopeak_background - 20;
        backgroundEnd =
          ((int32_t)last_photopeak_background + 20) > BACKGROUND_MAX
          ? BACKGROUND_MAX : last_photopeak_background + 20;
        backgroundIncrement = 1;
        break;
      }

    for (int32_t center = centerStart; center > centerEnd; center -= centerIncrement)
      {
        //
        // Create a Lorentzian shape to compare with the actual data.  Were only going to 
        // generate and compare this shape from the left side of the peak (at hwhm) on out
        // to the right side hwhm.  Here we want to go with a more peaked shape that will
        // generate big errors until the shape slides into the photopeak.
        //

        memset(model, 0, sizeof(model));
        for (int32_t i = center - photopeak_hwhm; i < center + photopeak_hwhm; ++i)
          {
            model[i] = ((photopeak_max - photopeak_background) * 
              (photopeak_fwhm * photopeak_fwhm)) / 
              (4 * (i - center) * (i - center) + (photopeak_fwhm * photopeak_fwhm));
            model[i] += photopeak_background;
            //
            // Clamp the model at 1 to avoid divide by zero problems.
            //
            if (model[i] <= 1)
              {
                model[i] = 1;
              }
          }

        double variances[N_BINS];
        double chisq = 0.;
      
        for (int32_t i = center - photopeak_hwhm; i < center + photopeak_hwhm; ++i)
          {
            variances[i] = double (data[i]) - double(model[i]);
            variances[i] *= variances[i];
            variances[i] /= double (model[i]);
            chisq += variances[i];
          }

#if VERBOSE
        std::cout << "Center" << std::endl;
        std::cout << "  trying center " << center << std::endl;
        std::cout << "  chisq " << chisq << std::endl;
        std::cout << "  lowest chisq " << lowest_chisq << std::endl;
        std::cout << "  best center " << photopeak_center << std::endl;
        std::cout << "  best fwhm " << photopeak_fwhm << std::endl;
        std::cout << "  best background " << photopeak_background << std::endl;
#endif // VERBOSE
        if (chisq < lowest_chisq)
          {
#if VERBOSE
            std::cout << "  new best center " << center << std::endl;
#endif // VERBOSE
            lowest_chisq = chisq;
            photopeak_center = center;
          }
      }


#if RESET_CHISQ
    //
    // The chisq calculated for the first go around when we use a very peaked shape to 
    // find the initial photopeak is artificially low because of the tiny range of bins
    // included in the search.  So if the photopeak_fwhm is still set to this tiny sliver
    // we need to reset the lowest_chisq so the fwhm finder has a fair chance of actually
    // finding a reasonable fwhm
    //
    if (photopeak_fwhm == PEAK_FWHM_START)
      {
#if VERBOSE
        std::cout << "Reset lowest_chisq" << std::endl;
#endif // VERBOSE
        lowest_chisq = std::numeric_limits<double>::max ();
      }

#endif // RESET_CHISQ

    lowest_chisq = std::numeric_limits<double>::max ();

#if FIND_BACKGROUND
    //
    // Now let's find a background value.  Don't include the chi squared statistic in
    // the normal calculation since we're only looking off at part of the distribution
    // at one of the tails.
    //
    double lowest_bg_chisq = std::numeric_limits<double>::max ();
    for (int32_t background = BACKGROUND_MIN; background < BACKGROUND_MAX; ++background)
      {
        //
        // Create a lorentzian shape to compare with the actual data.  We're interested
        // in the data off about a fwhm (2 * hwhm) to the right of the peak where the 
        // peak has tapered off into the noise.
        //

        memset(model, 0, sizeof(model));
        for (int32_t i = photopeak_center + photopeak_fwhm; i < N_BINS; ++i)
          {
            model[i] = ((photopeak_max - background) * (photopeak_fwhm * photopeak_fwhm)) / 
              (4 * (i - photopeak_center) * (i - photopeak_center) + 
              (photopeak_fwhm * photopeak_fwhm));
            model[i] += background;
            //
            // Clamp the model at 1 to avoid divide by zero problems.
            //
            if (model[i] <= 1)
              {
                model[i] = 1;
              }
          }

        double variances[N_BINS];
        for (uint32_t i = 0; i < N_BINS; variances[i++] = 0.);
        double chisq = 0.;

        for (int32_t i = photopeak_center + photopeak_fwhm; i < N_BINS; ++i)
          {
            variances[i] = double (data[i]) - double (model[i]);
            variances[i] *= variances[i];
            variances[i] /= double (model[i]);
            chisq += variances[i];
          }

#if VERBOSE
        std::cout << "Background" << std::endl;
        std::cout << "  trying background " << background << std::endl;
        std::cout << "  chisq " << chisq << std::endl;
        std::cout << "  lowest chisq " << lowest_chisq << std::endl;
        std::cout << "  best fwhm " << photopeak_fwhm << std::endl;        
        std::cout << "  best center " << photopeak_center << std::endl;
        std::cout << "  best background " << photopeak_background << std::endl;
#endif // VERBOSE

        if (chisq < lowest_bg_chisq)
          {
#if VERBOSE
            std::cout << "  new best background " << background << std::endl;
#endif // VERBOSE
            lowest_bg_chisq = chisq;
            photopeak_background = background;
          }
      }
#endif // FIND_BACKGROUND

    lowest_chisq = std::numeric_limits<double>::max ();

#if FIND_FWHM
    //
    // We've found the best candidate peak, so now loop on reasonable fwhm values to get a
    // better fit.  We have to be careful here about what range to of bins to check.  On one
    // hand, we know that the photopeak is only going to be Lorentzian out to a certain hwhm
    // at which point we run into a compton shelf on the left side.  On the other hand, a 
    // very peaked shape will generate small errors if we only look at that tiny piece.
    //
    // So we only work only work from the hwhm value on the left side of the peak to avoid
    // the Compton shelf area, but find the best fit all the way out to the right side of 
    // the histogram to avoid the problems with small peaks giving unreasonably small chisq
    // statistics.
    //
    for (int32_t fwhm = PEAK_FWHM_MIN; fwhm < PEAK_FWHM_MAX; ++fwhm)
      {
        //
        // Create a lorentzian shape to compare with the actual data.
        //
        int32_t hwhm = fwhm / 2;

        memset(model, 0, sizeof(model));
        for (int32_t i = photopeak_center - hwhm; i < N_BINS; ++i)
          {
            model[i] = ((photopeak_max - photopeak_background) * (fwhm * fwhm)) / 
              (4 * (i - photopeak_center) * (i - photopeak_center) + (fwhm * fwhm));
            model[i] += photopeak_background;
            //
            // Clamp the model at 1 to avoid divide by zero problems.
            //
            if (model[i] <= 1)
              {
                model[i] = 1;
              }
          }

        double variances[N_BINS];
        double chisq = 0.;

        for (int32_t i = photopeak_center - hwhm; i < N_BINS; ++i)
          {
            variances[i] = double(data[i]) - double(model[i]);
            variances[i] *= variances[i];
            variances[i] /= double (model[i]);
            chisq += variances[i];
          }

#if VERBOSE
        std::cout << "Fwhm" << std::endl;
        std::cout << "  trying fwhm " << fwhm << std::endl;
        std::cout << "  chisq " << chisq << std::endl;
        std::cout << "  lowest chisq " << lowest_chisq << std::endl;
        std::cout << "  best fwhm " << photopeak_fwhm << std::endl;
        std::cout << "  best center " << photopeak_center << std::endl;
        std::cout << "  best background " << photopeak_background << std::endl;
#endif // VERBOSE

        if (chisq < lowest_chisq)
          {
#if VERBOSE
            std::cout << "  new best fwhm " << fwhm << std::endl;
#endif // VERBOSE
            lowest_chisq = chisq;
            photopeak_fwhm = fwhm;
            photopeak_hwhm = hwhm;
          }
      }
#endif // FIND_FWHM

    lowest_chisq = std::numeric_limits<double>::max ();

#if FIND_HEIGHT
    //
    // We've found the best shape, so now loop on reasonable values of the maximum height to
    // see if we can get a better fit.  Assume that the variance can be +- sqrt(max)
    //
    for (int32_t max = histo_max - int32_t(sqrt(histo_max));
         max < histo_max + int32_t(sqrt(histo_max)); 
         ++max)
      {
        //
        // Create a lorentzian shape to compare with the actual data.
        //
        memset(model, 0, sizeof(model));
        for (int32_t i = photopeak_center - photopeak_hwhm; 
             i < photopeak_center + photopeak_hwhm; 
             ++i)
          {
            model[i] = ((max - photopeak_background) * (photopeak_fwhm * photopeak_fwhm)) / 
              (4 * (i - photopeak_center) * (i - photopeak_center) + 
               (photopeak_fwhm * photopeak_fwhm));
            model[i] += photopeak_background;
            //
            // Clamp the model at 1 to avoid divide by zero problems.
            //
            if (model[i] <= 1)
              {
                model[i] = 1;
              }
          }
        
        double variances[N_BINS];
        for (uint32_t i = 0; i < N_BINS; variances[i++] = 0.);
        double chisq = 0.;

        for (int32_t i = photopeak_center - photopeak_hwhm; 
             i < photopeak_center + photopeak_hwhm; 
             ++i)
          {
            variances[i] = double (data[i]) - double (model[i]);
            variances[i] *= variances[i];
            variances[i] /= double (model[i]);
            chisq += variances[i];
          }

#if VERBOSE
        std::cout << "Max" << std::endl;
        std::cout << "  trying max " << max << std::endl;
        std::cout << "  chisq " << chisq << std::endl;
        std::cout << "  lowest chisq " << lowest_chisq << std::endl;
        std::cout << "  best fwhm " << photopeak_fwhm << std::endl;        
        std::cout << "  best center " << photopeak_center << std::endl;
        std::cout << "  best background " << photopeak_background << std::endl;
        std::cout << "  best max " << photopeak_max << std::endl;
#endif // VERBOSE

        if (chisq < lowest_chisq)
          {
            lowest_chisq = chisq;
#if VERBOSE
            std::cout << "  new best max " << max << std::endl;
#endif // VERBOSE
            photopeak_max = max;
          }
      }
#endif // FIND_HEIGHT

    if (last_photopeak_center == photopeak_center &&
        last_photopeak_fwhm == photopeak_fwhm &&
        last_photopeak_max == photopeak_max &&
        last_photopeak_background == photopeak_background)
      {
        break;
      }
  }

#if VERBOSE
  std::cout << "  found center at " << photopeak_center << std::endl;
  std::cout << "  found fwhm at " << photopeak_fwhm << std::endl;
  std::cout << "  found max at " << photopeak_max << std::endl;
  std::cout << "  found background at " << photopeak_background << std::endl;
#endif // VERBOSE

  // ----------------------------------------------------------------------
  // And now for something completely different.  We want to figure out 
  // what kind of effect the Constant Fraction Discriminator threshold has
  // had on the histogram.
  //
  // The first thing to observe is that no matter what the CFD threshold, 
  // there will be a number of low-energy histogram bins that contain no
  // hits at all -- ever.  We call the highest bin with zero counts the 
  // cutoff bin.
  //
  // The second thing to observe is that a too-low CFD threshold will 
  // simply destroy the nice shape of the histogram, it won't change the 
  // number of zeroed histo bins.  It seems that what happens is that when 
  // the CFD threshold is set "too low" the system spends all of its time 
  // dealing with noise and is unable to recover sufficiently to look at 
  // real hits.
  //
  // As the CFD threshold is raised from "too low" into the "just right" 
  // range the expected histogram shape with a photopeak, compton shelf and 
  // backscatter peak suddenly appears as we break the noise threshold.  As
  // the CFD threshold is increased, the cutoff bin remains the same, but
  // the count of low-energy bins with close-to-zero counts starts to 
  // increase, effectively creating a knee in the backscatter peak curve 
  // left-hand-side.  This knee eats into the backscatter peak and moves to
  // the right as the CFD threshold is increased.
  // 
  // What we want to see in the CFD calibration is the beginning of such a
  // knee.  We don't want to set the CFD threshold too low, and take a 
  // chance that we are close to the "too low" noise limit; but we also 
  // don't want the CFD threshold set "too high" so that it starts eating 
  // into interesting events.
  //
  // The goal of the CFD calibration is then to create a knee in the
  // backscatter peak curve, and place it just to the right of the cutoff.
  // Our goal here is then to identify this knee.  The simplest way to do 
  // this is to identify this edge in a way that is relatively immune from
  // backscatter peak amplitudes.
  //
  // In a kind of recursive irony, this is exactly the problem for which
  // constant fraction discriminators are the solution.  The shape of the 
  // backscatter peak does not lend itself to that solution, so problem is
  // different enough that we just use an edge detector.  We will trigger
  // on the "rising edge" of the backscatter peak as viewed in the histo.
  //
  // We pull a threshold out of the air and say that when the backscatter
  // peak breaks 20% of the photopeak maximum, we trigger
  // ----------------------------------------------------------------------

  int32_t bs_peak_edge = N_BINS;
  int32_t bs_threshold = (int32_t)(0.2 * (double)photopeak_max);

  for (int32_t edge = 0; edge < photopeak_center - photopeak_fwhm; ++edge)
    {
      if (data[edge] > bs_threshold)
        {
          bs_peak_edge = edge;
          break;
        }
    }

#if VERBOSE
  std::cout << "  bs peak edge " << bs_peak_edge << std::endl;
#endif // VERBOSE

  std::ofstream datafile;
  datafile.open (outfileName.c_str ());

  //
  // Put together the best model we found, clipped appropriately
  //
  memset(model, 0, sizeof(model));

  //
  // Calculate the final chi square only from the photopeak fit.
  //
  for (int32_t i = photopeak_center - photopeak_hwhm; i < N_BINS; ++i)
    {
      model[i] = ((photopeak_max - photopeak_background) * (photopeak_fwhm * photopeak_fwhm)) / 
        (4 * (i - photopeak_center) * (i - photopeak_center) + (photopeak_fwhm * photopeak_fwhm));
      model[i] += photopeak_background;
    }

  double variances[N_BINS];
  for (uint32_t i = 0; i < N_BINS; variances[i++] = 0.);
  double final_chisq = 0.;

  for (int32_t i = photopeak_center - photopeak_hwhm; i < N_BINS; ++i)
    {
      variances[i] = double (data[i]) - double (model[i]);
      variances[i] *= variances[i];
      variances[i] /= double (model[i]);
      final_chisq += variances[i];
    }

  for (uint32_t i = 0; i < N_BINS; ++i)
    {
      datafile << data[i] << "\t" << model[i] << std::endl;
    }
  
  datafile.close ();

  gnuplot_ctrl *handle = gnuplot_init () ;

  gnuplot_cmd (handle, "set terminal png size 1024,768");
  gnuplot_cmd (handle, "set output \"%s.png\"", outfileName.c_str ());
  gnuplot_cmd (handle, "set grid");
  gnuplot_cmd (handle, "set title \"Energy Histogram\"");
  gnuplot_cmd (handle, "set xlabel \"Bin\"");
  gnuplot_cmd (handle, "set xrange [-1:257]");
  gnuplot_cmd (handle, "set ylabel \"Count\"");
  gnuplot_cmd (handle, "set boxwidth 1");
  gnuplot_cmd (handle, "set label \"chisq %f\" at %d, %d", final_chisq, 20, photopeak_max);
  gnuplot_cmd (handle, "set label \"fwhm %d\" at %d, %d", photopeak_fwhm, 20, photopeak_max - 20);

  gnuplot_cmd (handle, "set label \"photopeak %d\" at %d, %d", photopeak_center, photopeak_center + 5, photopeak_max);
  gnuplot_cmd (handle, "set arrow from %d, %d to %d, %d", photopeak_center, 0, photopeak_center, photopeak_max);

  gnuplot_cmd (handle, "set label \"bs peak edge %d\" at %d, %d", bs_peak_edge, bs_peak_edge + 5, bs_threshold);
  gnuplot_cmd (handle, "set arrow from %d, %d to %d, %d", bs_peak_edge, 0, bs_peak_edge, bs_threshold);

  gnuplot_cmd (handle, "set label \"background %d\" at %d, %d", photopeak_background, 20, photopeak_background + 20);
  gnuplot_cmd (handle, "set arrow from %d, %d to %d, %d", 0, photopeak_background, 255, photopeak_background);

  gnuplot_cmd (handle, "plot \"%s\" using \"%%lf%%*lf\" title \"Actual\" with boxes lw 1,"
                            "\"%s\" using \"%%*lf%%lf\" title \"Fit\" with linespoints", 
               outfileName.c_str (), outfileName.c_str ());

  gnuplot_close (handle);
  return 0;
}