//
//-----------------------------------------------------------------------
/// @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	DriftCorrection.cpp
///
/// @author	John Burton	<j.c.burton@gats-inc.com>
///
/// @date  	Mon Aug 18 16:58:37 2008
///
//-----------------------------------------------------------------------
//
//-----------------------------------------------------------------------
// Include Files:
//-----------------------------------------------------------------------
//

#include <string>
#include <valarray>
#include "Event.h"
#include "EventVar.h"
#include "DriftCorrection.h"
#include "lmmin.h"

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

typedef struct {
  double *y;
  double *x;
  double R2;
  double (*func) (double *par, double user_x);
} lm_data_type;


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



//
//-----------------------------------------------------------------------
// Utility Functions:
//-----------------------------------------------------------------------
//


static double lmLinearFit(double *par, double x)
{
  double y;

  y = x * par[0] + par[1];

  return y;
}


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;

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



static void lmDegreeOfFit(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 == -1) 
  {
    sum = 0.0;
    for(i=0; i<m_dat; i++)
      sum = sum + (mydata->y)[i];
    ave = sum / (double)m_dat;
    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);
    }
    mydata->R2 = 1.0 - (ss_reg/ss_tot);
  }
}


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

DriftCorrection::DriftCorrection(void)
{
}



void DriftCorrection::doLinearFit(EventVar<double> &Extent, EventVar<double> &Altitude, EventVar<double> &Time)
{
  int len, m_dat, n_par;
  int i,j;
  double missing = -1e24;
  EventVar<double> ExoExtent;
  EventVar<double> ExoTime;

  std::valarray<bool> valid  = (Altitude > missing);

  std::valarray<bool> good  = ((Altitude >= 100.0) &&
			       (Altitude <= 140.0));

  MeasuredExtent_  = Extent[valid];
  CorrectedExtent_ = Extent[valid];
  Altitude_        = Altitude[valid];
  Time_            = Time[valid];
  z_ref_           = 100.0;
  t_ref_           = Time_.Interpol(Altitude_,z_ref_);

//  Altitude_.Plot("Altitude","",Time_);
//  Time_.Plot("Time","",Altitude_);

//  std::cerr << "DriftCorrection::doLinearFit: " << Altitude_.size() << ", " << Time_.size() << "\n";

//  std::cerr << "DriftCorrection::doLinearFit: " << z_ref_ << ", " << t_ref_ << "\n";

  ExoExtent = Extent[good];
  ExoTime   = Time[good];

  m_dat = ExoExtent.size();
  n_par = 2;

  double par[n_par];
  double extent[m_dat];
  double time[m_dat];

  par[0] = 0.0;
  par[1] = 1.0;

  lm_control_type control;
  lm_data_type    data;

  lm_initialize_control(&control);

  for(i=0; i<m_dat; i++)
  {
    extent[i] = ExoExtent[i];
    time[i]   = ExoTime[i];
  }

  data.func = lmLinearFit;
  data.y    = extent;
  data.x    = time;

//  std::cerr << "DriftCorrection::doLinearFit: " << par[0] << ", " << par[1] << "\n";

  lm_minimize(m_dat,n_par,par,lmEvaluate,lmDegreeOfFit,&data,&control);
  m_  = par[0];
  b_  = par[1];
  R2_ = data.R2;

  std::cerr << "DriftCorrection::doLinearFit: " << par[0] << ", " << par[1] << ", " << R2_ << "\n";
  
  e_ref_ = m_ * t_ref_ + b_;

}


EventVar<double> DriftCorrection::doDriftCorrection(double &TrueExtent)
{
  double ext, e_fit;
  EventVar<double> Extent(MeasuredExtent_);

  for(int i = 0; i < MeasuredExtent_.size(); i++)
  {
    e_fit = m_ * Time_[i] + b_;
    e_fit = e_fit - e_ref_;
    ext   = MeasuredExtent_[i] - e_fit;
    Extent[i] = ext * TrueExtent / e_ref_;
  }
  std::cerr << "DriftCorrection::doDriftCorrection " << e_ref_ << ", " << TrueExtent << "\n";
  Extent = Extent.Smooth(5);
  return Extent;
}