//
//-----------------------------------------------------------------------
/// @copyright
///            (c) Copyright 2008 by GATS, Inc.,
///     11864 Canon Blvd, Suite 101, Newport News VA 23606
///
///  All Rights Reserved. No part of this software or publication may be
///  reproduced, stored in a retrieval system, or transmitted, in any form
///  or by any means, electronic, mechanical, photocopying, recording, or
///  otherwise without the prior written permission of GATS, Inc.
///
//-----------------------------------------------------------------------
///
/// @file	lmRegression.cpp
///
/// @author	John Burton	<j.c.burton@gats-inc.com>
///
/// @date  	Tue May 13 12:28:57 2008
///
//-----------------------------------------------------------------------
//
//-----------------------------------------------------------------------
// Include Files:
//-----------------------------------------------------------------------
//

#include "EventVar.h"
#include "lmmin.h"

//
//-----------------------------------------------------------------------
// Defines and Macros:
//-----------------------------------------------------------------------
//


//extern "C" {
  typedef struct {
    double *y;
    double *x;
    double (*func) (double *par, double user_x);
    void   (*adjust) (double *par);
  } lm_data_type;
//}

//
//-----------------------------------------------------------------------
// Global Variables:
//-----------------------------------------------------------------------
//

EventVar<double> BaseModel_;
EventVar<double> EdgeModel_;
EventVar<double> BaseGrid_;
EventVar<double> CoordinateGrid_;
double nf;
double mx;
double R2;


//
//-----------------------------------------------------------------------
// Class Methods:
//-----------------------------------------------------------------------
//


static void lmAdjustFunction(double *p)
{

  CoordinateGrid_.setValueZero();
  CoordinateGrid_ = BaseGrid_ * p[0] + p[1];
}


  
static double lmFitFunction(double *par, double loc)
{
  double intensity;
  
//  intensity = par[2] * EdgeModel_.Interpol(CoordinateGrid_,loc) + par[3];
  intensity = EdgeModel_.Interpol(CoordinateGrid_,loc);
//  intensity = intensity * par[2] + par[3];
//  intensity = intensity + par[2];
//  std::cerr << "par[2], par[3] = " << par[2] << ", " << par[3] << "\n";
  return intensity;
}


static double lmIntensity(double *par, double x )
{
  double y = x * par[0] + par[1];
//  double y = x + par[0];
  return y;
}


static void lmAdjustNoiseFloorAndBlur(double *p)
{
  int maf = (int)(1000.0*p[0]);

  std::cerr << "lmAdjustNoiseFloorAndBlur::p[0] = " << p[0] << "\n";
  std::cerr << "lmAdjustNoiseFloorAndBlur::maf  = " << maf << "\n";
  EdgeModel_ = BaseModel_.Smooth(maf);
  EdgeModel_ = (EdgeModel_ + nf) * mx;
//  EdgeModel_ = EdgeModel_ + p[1];
}


static double lmNoiseFloorAndBlur(double *par, double x )
{
  double intensity = EdgeModel_.Interpol(BaseGrid_,x);
  return intensity;
}


static  void lmNoop(double *par)
{
}


static void lmEvaluate(double *par, int m_dat, double *fvec,
		void *data, int *info)
{
  int i;
  lm_data_type *mydata;

  mydata = (lm_data_type *)data;

  mydata->adjust(par);
  for(i=0; i<m_dat; i++)
    fvec[i] = mydata->y[i] - mydata->func(par,mydata->x[i]);
}

static void lmEvaluateBlur(double *par, int m_dat, double *fvec,
		void *data, int *info)
{
  int i;
  lm_data_type *mydata;

  mydata = (lm_data_type *)data;

  mydata->adjust(par);
  for(i=0; i<m_dat; i++)
    fvec[i] = mydata->y[i] - mydata->func(par,mydata->x[i]);
  for(i=7;i<14;i++)
    fvec[i] = 0.0;
}

static void lmPrint(int n_par, double *par, int m_dat, double *fvec,
	     void *data, int iflag, int iter, int nfev)
{
    double f, y, t;
    int i;
    double ss_reg = 0.0;
    double ss_tot = 0.0;
    double tmp_reg;
    double tmp_tot;
    double sum = 0.0;
    double ave = 0.0;
    lm_data_type *mydata;
    mydata = (lm_data_type *) data;

//    if (iflag == 2) {
//        printf("trying step in gradient direction\n");
//    } else if (iflag == 1) {
//        printf("determining gradient (iteration %d)\n", iter);
//    } else if (iflag == 0) {
//        printf("starting minimization\n");
//    } else if (iflag == -1) {
//        printf("terminated after %d evaluations\n", nfev);
//    }
//

    if (iflag == -1) 
    {
      sum = 0.0;
      for(i=0; i<m_dat; i++)
	sum = sum + (mydata->y)[i];
      ave = sum / (double)m_dat;
//      std::cerr << "  par: ";
//      for (i = 0; i < n_par; ++i)
//	std::cerr << " " << par[i];
      
//      std::cerr << " => norm: " << lm_enorm(m_dat, fvec);
      ss_tot = 0.0;
      ss_reg = 0.0;
      for (i = 0; i < m_dat; ++i) 
      {
	t = (mydata->x)[i];
	y = (mydata->y)[i];
	f = mydata->func(par,t);
	tmp_tot = ave - y;
	tmp_reg = f   - y;
	ss_tot = ss_tot + (tmp_tot * tmp_tot);
	ss_reg = ss_reg + (tmp_reg * tmp_reg);
      }
      R2 = 1.0 - (ss_reg/ss_tot);
//      std::cerr << "  R2 = " << R2 << "\n";
    }
}


      
void lmInitialize(EventVar<double> &EdgeModel, EventVar<double> &BaseGrid)
{
  EdgeModel_ = EdgeModel;
  BaseModel_ = EdgeModel;
  BaseGrid_  = BaseGrid;
}




EventVar<double> lmFitGrid(EventVar<double> &sample, double &lx, double &lxval, 
			   double &hx, double &hxval, double &r2)
{
//  int m_dat = 15;
  int m_dat = 21;
  int n_par = 2;
  int len   = EdgeModel_.size();
  int i,j;

  double x1,x2,y1,y2;
  double par[4];
//  double par[2];
  double vals[256];
  double locs[256];

  i = 0;
  while((i<len)&&(EdgeModel_[i]<lxval))
    i++;
  x1 = BaseGrid_[i];
  y1 = lx;

  i = len - 1;
  while((i>=0)&&(EdgeModel_[i]<hxval))
    i--;
  x2 = BaseGrid_[i];
  y2 = hx;

  par[0] = (y1-y2)/(x1-x2);
  par[1] = y1 - par[0] * x1;
  par[2] = 0.0;
  par[3] = 0.0;

//  std::cerr << "lmRegression: " << lx << ", " << lxval << "\n";
//  std::cerr << "lmRegression: " << hx << ", " << hxval << "\n";
//  std::cerr << "lmRegression: " << x1 << ", " << x2 << "\n";
//  std::cerr << "lmRegression: " << y1 << ", " << y2 << "\n";
//  std::cerr << "lmRegression: " << par[0] << ", " << par[1] << "\n";
  lm_control_type control;
  lm_data_type    data;

  lm_initialize_control(&control);

//  control.epsilon = 1.e-7;

  for(i=0,j=0; i<sample.size(); i++)
  {
    if(sample[i] > 0)
    {
      vals[j] = sample[i];
      locs[j] = (double)i;
      j++;
    }
  }
  m_dat       = j;
  data.func   = lmFitFunction;
  data.adjust = lmAdjustFunction;
  data.y      = vals;
  data.x      = locs;
  CoordinateGrid_ = BaseGrid_ * par[0] + par[1];
//  std::cerr << "findEdges: testing lm_minimize \n";
  lm_minimize(m_dat,n_par,par,lmEvaluate,lmPrint,&data,&control);
//  std::cerr << "findEdges:lm_minimize: status " << lm_shortmsg[control.info];
//  std::cerr << " after " << control.nfev << " evaluations\n";
  r2 = R2;
  return CoordinateGrid_;
}
  



EventVar<double> lmFitIntensities(EventVar<double> &vals, EventVar<int> &locs)
{
  int m_dat = vals.size();;
//  int n_par = 2;
  int n_par = 1;
  int len   = EdgeModel_.size();
  int i,j;

  double y[15],x[15];
  double x1,x2,y1,y2;
  double par[2];

  EventVar<double> param(2);

  for(i=0; i<m_dat; i++)
  {
    x1 = (double)locs[i];
    y[i] = EdgeModel_.Interpol(BaseGrid_,x1);
    x[i] = vals[i];
  }
  par[0] = 1.0;
  par[1] = 0.0;
//  par[0] = 0.0;

  lm_control_type control;
  lm_data_type    data;

  lm_initialize_control(&control);

  data.func   = lmIntensity;
  data.adjust = lmNoop;
  data.y      = y;
  data.x      = x;
//  std::cerr << "findEdges: testing lm_minimize \n";
  lm_minimize(m_dat,n_par,par,lmEvaluate,lmPrint,&data,&control);
//  std::cerr << "findEdges:lm_minimize: status " << lm_shortmsg[control.info];
//  std::cerr << " after " << control.nfev << " evaluations\n";
  param[0] = par[0];
  param[1] = par[1];
  return param;
}
  



EventVar<double> lmFitNoiseFloorAndBlur(EventVar<double> &vals, EventVar<int> &locs)
{
  const int nelem = 14;
  int elems[nelem] = {0,1,2,3,4,5,6,14,15,16,17,18,19,20};
  int m_dat = vals.size();
//  int n_par = 2;
  int n_par = 1;
  int len   = EdgeModel_.size();
  int i,j,l;

//  std::cerr << "lmfitNoiseFloorAndBlur: m_dat = " << m_dat << " \n";
//  double y[21],x[21];
  double y[14],x[14];
  double x1,x2,y1,y2,dy1,dy2;
  double par[4];

  EventVar<double> param(3);
  EventVar<double> tmp;

  x1 = (double)locs[0];
  x2 = (double)locs[m_dat-1];
  y1 = EdgeModel_.Interpol(BaseGrid_,x1);
  y2 = EdgeModel_.Interpol(BaseGrid_,x2);
  dy1 = vals[0] - y1;
  dy2 = vals[m_dat-1] - y2;
  nf = (dy1 > dy2) ? dy2 : dy1; // Noise Floor
//  std::cerr << "lmfitNoiseFloorAndBlur: vals[0] = " << vals[0] << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: y1      = " << y1 << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: vals[m_dat-1] = " << vals[m_dat-1] << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: y2            = " << y2 << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: dy2 = " << dy2 << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: dy1 = " << dy1 << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: dy2 = " << dy2 << " \n";

//  tmp = EdgeModel_;
  tmp = EdgeModel_ + nf;
//  mx  = 1.0 / tmp.max();
  mx  = vals.max() / tmp.max();

//  std::cerr << "lmfitNoiseFloorAndBlur: noisefloor = " << nf << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: mx         = " << mx << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: vals.max() = " << vals.max() << " \n";
//  std::cerr << "lmfitNoiseFloorAndBlur: tmp.max()  = " << tmp.max() << " \n";

  
  
  for(i=0; i<nelem; i++)
  {
    l = elems[i];
    y[i] = vals[l];
    x[i] = (double)locs[l];
  }
  par[0] = 0.0; 
  par[1] = nf;
  par[2] = mx;
//  par[0] = 0.0;

  lm_control_type control;
  lm_data_type    data;

  lm_initialize_control(&control);

  control.epsilon = 1.e-2;

  data.func   = lmNoiseFloorAndBlur;
  data.adjust = lmAdjustNoiseFloorAndBlur;
  data.y      = y;
  data.x      = x;
//  std::cerr << "lmfitNoiseFloorAndBlur: testing lm_minimize \n";
  lm_minimize(nelem,n_par,par,lmEvaluate,lmPrint,&data,&control);
//  std::cerr << "fitNoiseFloorAndBlur:lm_minimize: status " << lm_shortmsg[control.info];
//  std::cerr << " after " << control.nfev << " evaluations\n";
//  param[0] = par[0];
  param[0] = par[0];
//  param[1] = nf;
//  param[1] = par[1];
//  param[2] = par[2];
  param[1] = nf;
  param[2] = mx;
  return param;
}