/* -*- C++ -*- *************************************************************
 *
 *   CbmRichRingFinderImp.cxx, Thu Jul 15 11:44:57 CEST 2004
 *   lsf4rich.cpp, Tue Jun 22 17:12:50 MSD 2004
 *   lsf4rich_minuit.cpp, Tue Jun 22 17:14:20 MSD 2004
 *   Copyright (C) 2004 Soloviev Alexei <solovjev@cv.jinr.ru>
 *
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include "CbmRichRingFinderImp.h"

#include <algorithm>
#include <numeric>
#include <cmath>
using namespace std;

LSF4RICH::LSF4RICH() :
  TMinuit ( 3 ),
  __data(), 
  __ring_center_x(), 
  __ring_center_y(), 
  __ring_radius(), 
  __fun_ptr ( &LSF4RICH::standard ), 
  __guided ( false ), 
  __threshold ( 3 ) 
{
  /** MINUIT initialization */
  SetFCN ( fcn );
}

LSF4RICH::~LSF4RICH() {}

/** Clear data storage and ring guidance */
void LSF4RICH::clear()
{
  __data.clear();
  set_guidance ( 0, 0, 0 );
  __guided = false;
}

/** Add point to data storage */
void LSF4RICH::add_data_point ( const double & x, const double & y )
{
  __data.push_back ( point ( x, y) );
}

/** Set sought ring guidance */
void LSF4RICH::set_guidance ( const double & x, const double & y, 
			      const double & r )
{
  __ring_center_x = x;
  __ring_center_y = y;
  __ring_radius = r;
  __guided = true;
}

/** Set sought ring guidance over data */
void LSF4RICH::find_guidance()
{
  point center = accumulate ( __data.begin(), __data.end(), point() );
  center /= __data.size();
  __ring_radius = 0;
  for ( register unsigned int i = 0; i < __data.size(); ++i ) {
    __ring_radius += sqrt ( norm ( __data[i] - center ) );
  }
  __ring_radius /= __data.size();
  __guided = false;
}

/** LSF value (to be minimized) */
const double LSF4RICH::value() const
{
  return (this->*__fun_ptr)();
}

/** Standard LSF value */
const double LSF4RICH::standard() const
{
  if ( !__data.empty() ) {
    double res = 0;
    point center = point ( __ring_center_x, __ring_center_y );
    register double di;
    for ( register unsigned int i = 0; i < __data.size(); ++i ) {
      ( di = sqrt ( norm ( __data[i] - center ) ) ) -= __ring_radius;
      res += ( di *= di );
    }
    return res /= __data.size();
  }
  return 0;
}

/** Weighted LSF value */
const double LSF4RICH::weighted() const
{
  if ( __norm ) {
    double res = 0;
    point center = point ( __ring_center_x, __ring_center_y );
    register double di;
    for ( register unsigned int i = 0; i < __data.size(); ++i ) {
      if ( __weight[i] ) {
	( di = sqrt ( norm ( __data[i] - center ) ) ) -= __ring_radius;
	res += ( ( di *= di ) *= __weight[i] );
      }
    }
    return res /= __norm;
  }
  return 0;
}

/** Calculate Tukey's bi-square weights for weighted LSF value */
void LSF4RICH::set_weights()
{
  point center = point ( __ring_center_x, __ring_center_y );
  register double di;
  double range = sqrt ( value() ) * C_TUKEY;
  __norm = 0;
  for ( register unsigned int i = 0; i < __data.size(); ++i ) {
    ( di = sqrt ( norm ( __data[i] - center ) ) ) -= __ring_radius;
    if ( abs ( di ) < range ) {
      __weight[i] = ( ( ( ( di /= range ) *= di ) -= 1 ) *= di );
      __norm += __weight[i];
    } else {
      __weight[i] = 0;
    }
  }
}

/** Set weights = 1 for weighted LSF value 
    (should be called before any weights uasge!) */
void LSF4RICH::init_weights()
{
  if ( __weight.size() != __data.size() ) {
    __weight.clear();
    __weight.resize ( __data.size(), 1 );
  } else {
    fill ( __weight.begin(), __weight.end(), 1 );
  }
  __norm = __data.size();
}

/** Set weights = 0 for points out of guidance */
void LSF4RICH::guide_weights()
{
  point center = point ( __ring_center_x, __ring_center_y );
  register double di;
  for ( register unsigned int i = 0; i < __data.size(); ++i ) {
    di = sqrt ( norm ( __data[i] - center ) );
    if ( di > 1.5 * __ring_radius ) {
      __norm -= __weight[i];
      __weight[i] = 0;
    }
  }
}

/** MINUIT interface */
bool LSF4RICH::minimize ( bool robust, const int verbose )
{
  static int ierflg;
  __lsf_ptr = this;

  /** What should be minimized? */
  if ( robust || __guided ) {
    __fun_ptr = &LSF4RICH::weighted;
    init_weights();
    if ( __guided ) guide_weights();
    if ( __norm < __threshold ) return false;
  } else {
    __fun_ptr = &LSF4RICH::standard;
    if ( __data.size() < __threshold ) return false;
  }

  /** Here we go! */
  set_mn_printout ( verbose );
  set_mn_parms();
  if ( robust ) {
    for ( C_TUKEY = 5; C_TUKEY > 1; C_TUKEY *= 0.5 ) {
      set_weights();
      if ( __guided ) guide_weights();
      mncomd ( "MINIMIZE", ierflg );
    }
  } else {
    if ( __guided ) guide_weights();
    mncomd ( "MINIMIZE", ierflg );
  }
  get_mn_parms();

  return true;
}

/** LSF object output */
ostream & operator<< ( ostream & out, const LSF4RICH & lsf )
{
  return out << "center = " 
	     << complex<double> ( lsf.ring_center_x(), lsf.ring_center_y() ) 
	     << " "
	     << "radius = " 
	     << lsf.ring_radius();
}

/***************************************************************************
 *
 *   MINUIT stuff below...
 *
 ***************************************************************************/

const LSF4RICH * LSF4RICH::__lsf_ptr = 0;

/** User FCN function for MINUIT */
void LSF4RICH::fcn ( int &npar, double *gin, double &chi2, double *par, int iflag )
{
  __lsf_ptr->__ring_center_x = par[0];
  __lsf_ptr->__ring_center_y = par[1];
  __lsf_ptr->__ring_radius = par[2];
  chi2 = __lsf_ptr->value();
}

/** Set printout level for MINUIT */
void LSF4RICH::set_mn_printout ( const int level )
{
  static int ierflg;
  /** SET PRIntout <level>
   *  Sets the print level, determining how much output will be
   *  produced. Allowed values and their meanings are displayed
   *  after a SHOw PRInt command, and are currently <level>=:
   *    [-1]  no output except from SHOW commands
   *     [0]  minimum output
   *     [1]  default value, normal output
   *     [2]  additional output giving intermediate results
   *     [3]  maximum output, showing progress of minimizations
   */
  switch ( level ) {
  case -1: mncomd ( "SET PRINTOUT -1", ierflg ); break;
  case 0:  mncomd ( "SET PRINTOUT 0",  ierflg ); break;
  case 1:  mncomd ( "SET PRINTOUT 1",  ierflg ); break;
  case 2:  mncomd ( "SET PRINTOUT 2",  ierflg ); break;
  case 3:  mncomd ( "SET PRINTOUT 3",  ierflg ); break;
  default: mncomd ( "SET PRINTOUT 3",  ierflg ); break;
  }
  mncomd ( "SET NOWARNINGS", ierflg );
}

/** Set parameters for MINUIT */
void LSF4RICH::set_mn_parms()
{
  static int ierflg;
  mnparm ( 0, "center:x", __ring_center_x, 0.1, 0, 0, ierflg );
  mnparm ( 1, "center:y", __ring_center_y, 0.1, 0, 0, ierflg );
  mnparm ( 2, "radius", __ring_radius, 0.1, 0, 0, ierflg );
}

/** Get parameters from MINUIT */
void LSF4RICH::get_mn_parms() const
{
  static double err;
  GetParameter ( 0, __ring_center_x, err );
  GetParameter ( 1, __ring_center_y, err );
  GetParameter ( 2, __ring_radius, err );
}