///
/// Description:
/// Linearity correction. CALDET only.
///
////////////////////////////////////////////////////////////////////
// $Id: CalLinearity.cxx,v 1.7 2007/01/15 19:52:00 rhatcher Exp $
//
// pa@hep.ucl.ac.uk
//
// This would have been CalNonLinearity, but that table doesn't have 
// aggregate numbers in.
//
////////////////////////////////////////////////////////////////////
 

#include "Calibrator/CalLinearity.h"
#include "MessageService/MsgService.h"
#include "DatabaseInterface/DbiOutRowStream.h"
#include "DatabaseInterface/DbiResultSet.h"
#include "DatabaseInterface/DbiValidityRec.h"
#include "TGraph.h"
#include "TF1.h"
#include "TCanvas.h"
#include "TMath.h"
#include <cmath>
ClassImp(CalLinearity)

  //............

CVSID("$Id: CalLinearity.cxx,v 1.7 2007/01/15 19:52:00 rhatcher Exp $ \n CVSID_DBIRESULTPTR ");

#include "DatabaseInterface/DbiResultPtr.tpl"
template class  DbiResultPtr<CalLinearity>;

#include "DatabaseInterface/DbiWriter.tpl"
template class  DbiWriter<CalLinearity>;

CalLinearity::CalLinearity(int aggno, PlexStripEndId &seid, int task): fAggregateNo(aggno), fStripEndKey(seid.BuildPlnStripEndKey()), fStripEndId(seid.GetEncoded())
{
  fTask = task;
  switch (task) {
  case 2:
    fSplines[0].startx = 0;
    fSplines[0].starty = 0;
    fSplines[0].slope = 1;
    fSplines[0].alpha = 0;
    fSplines[0].endx = 7000;
    fSplines[0].endy = 7000;
    fNsplines = 1;
    break;
  default:
    MSG("Calib",Msg::kWarning)<<"CalLinearity doesn't know how to construct a default map for task "<<task<<endl;
  };

}

CalLinearity::CalLinearity(int aggno, PlexStripEndId &seid,
			   vector<float> &x,vector<float> &y): fAggregateNo(aggno), fStripEndKey(seid.BuildPlnStripEndKey()),fStripEndId(seid.GetEncoded())
{
  // y is the data for this channel, x is the data for the "linear" scale.
  assert(x.size()==y.size());
  fTask=2;
  // Go up to 7000 in y
  unsigned int i;
unsigned int q;
  int target=0;
for (q=0;q<x.size();q++) {
  //	cout<<"****"<<q<<" "<<x[q]<<" "<<y[q]<<endl;
if (x[q]>100 && y[q]>100) break;
}
// Don't fit to small numbers!

  for (i=q;i<x.size();i++) {
    // 	cout<<"****"<<i<<" "<<x[i]<<" "<<y[i]<<endl;
    if (y[i]>6000) break;

  }
  if (i==q) {
   fSplines[0].startx = 0;
   fSplines[0].starty = 0;
   fSplines[0].slope = 1;
   fSplines[0].alpha = 0;
   fSplines[0].endx = 16000;
   fSplines[0].endy = 16000;
   ++target;
   goto banana;

  }
 
  {
  TGraph g1(i-q,&(x[q]),&(y[q]));
  TF1 line("line","x*[0]",0,x[i-1] + 1);
   g1.Fit("line","RQ");
   fSplines[0].startx = 0;
   fSplines[0].starty = 0;
   fSplines[0].slope = line.GetParameter(0);
   fSplines[0].alpha = 0;
   fSplines[0].endx = x[i-1];
   fSplines[0].endy = (x[i-1])*fSplines[0].slope;
//   cout<<fSplines[0].endx<<" "<<fSplines[0].endy<<" "
//       <<fSplines[0].slope<<" "<<fSplines[0].alpha
//       <<" "<<fSplines[0].slope+2*(fSplines[0].endx- fSplines[0].startx)* 
//                                   fSplines[0].alpha<<endl;
   ++target;
  }
  cout<<fSplines[0].slope<<fSplines[0].endx<<" "<<fSplines[0].endy<<endl;

  {
// Suck up up to 2000 ADC counts in X.
TGraph g2(x.size(),&(x[0]),&(y[0]));
for(;;){
  unsigned int j;
   for (j=i;j<x.size();j++) {
      if (x[j]-x[i]>1000) break;
   }
   if (j==x.size()) break;
   if (target>8) target=8;
   if ((j-i)>1) {
     cout<<"target "<<target<<" Fit from "<<i<<" to "<<j<<" of "<<x.size()<< " ["<<x[i]<<":"<<x[j]<<" "<<y[i]<<":"<<y[j]<<"]\n";
   fSplines[target].startx = fSplines[target-1].endx;
   fSplines[target].starty = fSplines[target-1].endy;
   TF1 func("func","[0]+[2]*(x-[1]) + [3]*(x-[1])*(x-[1])",x[i]-1,x[j]+1);
   func.SetParameter(0,fSplines[target].starty);
   func.SetParameter(1,fSplines[target].startx);
   func.SetParLimits(0,fSplines[target].starty,fSplines[target].starty); // 1>0, so this means fixed
   func.SetParLimits(1,fSplines[target].startx,fSplines[target].startx); // 1>0, so this means fixed
   func.SetParLimits(3,-10,0);
   g2.Fit("func","RQ");
   fSplines[target].endx = x[j];
   fSplines[target].endy = func.Eval(x[j]);
   fSplines[target].slope = func.GetParameter(2);
   fSplines[target].alpha = func.GetParameter(3);
}
else if ((j-i)==1) {
  cout<<"Line from "<<i<<" to "<<j<<" ["<<x[i]<<":"<<x[j]<<"]\n";
   fSplines[target].startx = fSplines[target-1].endx;
   fSplines[target].starty = fSplines[target-1].endy;
   fSplines[target].endx = x[j];
   fSplines[target].endy = y[j];
   fSplines[target].slope = (fSplines[target].endy-fSplines[target].starty)/(fSplines[target].endx-fSplines[target].startx);
   fSplines[target].alpha = 0;
}
else if ((j-i)==0) {
  cout<<"No data\n";
--target;
}
   i=j;
++target;
cout<<" "<<fSplines[target-1].endx<<" "<<fSplines[target-1].endy<<endl;
}
  }

 banana:

   // and now extrapolate
    fSplines[target].startx = fSplines[target-1].endx;
    fSplines[target].starty = fSplines[target-1].endy;
    fSplines[target].endx = 160000;
    fSplines[target].slope = fSplines[target-1].slope + 2*fSplines[target-1].alpha*(fSplines[target-1].endx-fSplines[target-1].startx); 
    if (  fSplines[target].slope<0)  fSplines[target].slope =0;
    fSplines[target].endy = fSplines[target].starty + fSplines[target].slope*(fSplines[target].endx-fSplines[target].startx);
    fSplines[target].alpha = 0;

  fNsplines = target+1;
  // Now fiddle the numbers - our linear scale is x, and we want to fiddle
  // the scale to have a 1:1 mapping for the linear region.
  float xscale = fSplines[0].slope;
  // cout<<xscale<<endl;
  // so we have y = y0 + m(x-x0) + a(x-x0)(x-x0), and map x->sx, x0->sx0, so
  // y = y0 + (m/s)(sx-sx0) + (a/s^2)(sx-sx0)(sx-sx0)


  cout<<xscale<<" "<<fSplines[fNsplines-1].endx<<" "<<fSplines[fNsplines-1].endy<<endl;
     
    for (int i=0;i<fNsplines;i++) { 
      fSplines[i].startx*=xscale;   
    fSplines[i].endx*=xscale;
    fSplines[i].slope/=xscale;
    fSplines[i].alpha/=(xscale*xscale);
    }

 // Final sanity check
  if (this->ADCtoLin(8000) < 8000)
    {
      cout<<" "<<this->ADCtoLin(8000)<<"   ^^^^^^^^^^^^^\n";
      fNsplines=1;
      fSplines[0].startx = 0;
      fSplines[0].starty = 0;
      fSplines[0].slope = 1;
      fSplines[0].alpha = 0;
      fSplines[0].endx = 20000;
      fSplines[0].endy = 20000;


    }


    if (fSplines[fNsplines-1].endx < 20000) {
      fSplines[fNsplines-1].endx = 20000 ;
      fSplines[fNsplines-1].endy = fSplines[fNsplines-1].starty + fSplines[fNsplines-1].slope*(fSplines[fNsplines-1].endx-fSplines[fNsplines-1].startx) ;

    }

  cout<<xscale<<" "<<fSplines[fNsplines-1].endx<<" "<<fSplines[fNsplines-1].endy<<endl;
     
}




CalLinearity::CalLinearity(int aggno, PlexStripEndId &seid, vector<float> &x, vector<float> &y, vector<float> &/* ex */, vector<float> & /* ey */, CalLinearity & lin, bool horizontal) :fAggregateNo(aggno), fStripEndKey(seid.BuildPlnStripEndKey()), fStripEndId(seid.GetEncoded())
{
  bool zero = false;
  fTask = 2;
 top:
  if (zero||x.size()==0||y.size()==0) {
    fSplines[0].startx = 0;
    fSplines[0].starty = 0;
    fSplines[0].slope = 1;
    fSplines[0].alpha = 0;
    fSplines[0].endx = 7000;
    fSplines[0].endy = 7000;
    fNsplines = 1;
    fNumXPoints = 0;
  } 
  else {

  // This is basically the old IterFit()
  // Fit y against x', where x' is the x linearised by the func parameterised
  // in lin. Gives a mapping from y to "a linear scale"

  // First convert x to the linear scale.
 vector<float> xlin;
 //cout<<"x size: "<<x.size()<<" y size: "<<y.size()<<" limit: "<<lin.GetLimit()<<endl;
  int middle=0;
  for (unsigned int i=0;i<x.size();i++) {
    if (x[i]<lin.GetLimit()) {
      xlin.push_back(lin.ADCtoLin(x[i]));
      //   cout<<"x: "<<x[i]<<"  xlin: "<<i<<" "<<xlin[i]<<endl;
    } else {
      break;
    }
    if (middle==0) {
      if (y[i]>7000) middle = i-1;
    }
  }
  if (xlin.size()==0) {
    zero = true;
    goto top;
  }
  if (middle==0) middle = xlin.size()-1;
  fNumXPoints = xlin.size();
  TGraph graph(xlin.size(),&(xlin[0]),&(y[0]));
  // graph.Print();
  TF1 line("line","x*[0]",0,xlin[middle] + 1);
   graph.Fit("line","RQ");
  //  TCanvas c("C","C",0,0,500,500);
  //  graph.Draw("AP");
  // c.Update();
  //getchar();
  fSplines[0].startx = 0;
  fSplines[0].starty = 0;
  fSplines[0].slope = line.GetParameter(0);
  fSplines[0].alpha = 0;
  fSplines[0].endx = xlin[middle] + 1;
  fSplines[0].endy = (xlin[middle] + 1)*fSplines[0].slope;
  int target=0;
  //    cout<<target<<" "<<fSplines[target].startx<<" "<<fSplines[target].endx<<" "
  //	<<fSplines[target].starty<<" "<<fSplines[target].endy<<" "
  //	<<fSplines[target].slope<<" "<<fSplines[target].alpha<<endl;
    ++target;
  for (unsigned int i=middle+1;i<xlin.size();i++) {
    // Now fSplines-interpolate all the way up.
    if (target>6) target=6;
    fSplines[target].startx = fSplines[target-1].endx;
    fSplines[target].endx = xlin[i];
    fSplines[target].endy = y[i];
    fSplines[target].starty = fSplines[target-1].endy;
    fSplines[target].slope = fSplines[target-1].slope + fSplines[target-1].alpha*(fSplines[target-1].endx-fSplines[target-1].startx);
    fSplines[target].alpha = (fSplines[target].endy-fSplines[target].starty - 
			    fSplines[target].slope*
			    (fSplines[target].endx-fSplines[target].startx))/
      ((fSplines[target].endx-fSplines[target].startx)*(fSplines[target].endx-fSplines[target].startx));

    //   cout<<target<<" "<<fSplines[target].startx<<" "<<fSplines[target].endx<<" "
    //	<<fSplines[target].starty<<" "<<fSplines[target].endy<<" "
    //	<<fSplines[target].slope<<" "<<fSplines[target].alpha<<endl;
    ++target;
  }

  // And now the linear extrapolation
  if (horizontal) {
    fSplines[target].startx = fSplines[target-1].endx;
    fSplines[target].starty = fSplines[target-1].endy;
    fSplines[target].endx = 160000;
    fSplines[target].endy = fSplines[target].starty;
    fSplines[target].slope = 0;
    fSplines[target].alpha = 0;
    //    cout<<target<<" "<<fSplines[target].startx<<" "<<fSplines[target].endx<<" "
    //	<<fSplines[target].starty<<" "<<fSplines[target].endy<<" "
    //	<<fSplines[target].slope<<" "<<fSplines[target].alpha<<endl;
  } 
  else {
    fSplines[target].startx = fSplines[target-1].endx;
    fSplines[target].starty = fSplines[target-1].endy;
    fSplines[target].endx = 160000;
    fSplines[target].slope = fSplines[target-1].slope + fSplines[target-1].alpha*(fSplines[target-1].endx-fSplines[target-1].startx);
    fSplines[target].endy = fSplines[target].starty + fSplines[target].slope*(fSplines[target].endx-fSplines[target].startx);
    fSplines[target].alpha = 0;
    //    cout<<target<<" "<<fSplines[target].startx<<" "<<fSplines[target].endx<<" "
    //	<<fSplines[target].starty<<" "<<fSplines[target].endy<<" "
    //	<<fSplines[target].slope<<" "<<fSplines[target].alpha<<endl;
  }
  fNsplines = target + 1;
  //  for (float i=0;i<10001;i+=1000) cout<<i<<"  "<<ADCtoLin(LintoADC(i))<<endl;
  // Now fiddle the numbers - our linear scale is x, and we want to fiddle
  // the scale to have a 1:1 mapping for the linear region.
  float xscale = fSplines[0].slope;
  // cout<<xscale<<endl;
  // so we have y = y0 + m(x-x0) + a(x-x0)(x-x0), and map x->sx, x0->sx0, so
  // y = y0 + (m/s)(sx-sx0) + (a/s^2)(sx-sx0)(sx-sx0)

  for (int i=0;i<fNsplines;i++) {
    fSplines[i].startx*=xscale;
    fSplines[i].endx*=xscale;
    fSplines[i].slope/=xscale;
    fSplines[i].alpha/=(xscale*xscale);
  }

  // Final sanity check
  if (this->ADCtoLin(8000) < 8000)
    {
      cout<<" "<<this->ADCtoLin(8000)<<"   ^^^^^^^^^^^^^\n";
      fNsplines=1;
      fSplines[0].startx = 0;
      fSplines[0].starty = 0;
      fSplines[0].slope = 1;
      fSplines[0].alpha = 0;
      fSplines[0].endx = 20000;
      fSplines[0].endy = 20000;


    }

  }
}


void CalLinearity::Fill(DbiResultSet& rs, 
			const DbiValidityRec* /* vrec */) 
{
  rs >> fAggregateNo >> fTask >> fStripEndKey >> fStripEndId >> fNumber;
  for (int i=0;i<40;i++) rs >> fParam[i];

  // Unpack Param[] into fSplines[].
  fNsplines = fNumber;
  int pos = 0;
  for (int i=0;i<fNsplines;i++) {
    fSplines[i].startx =  fParam[pos++];
    fSplines[i].starty = fParam[pos++]; 
    fSplines[i].slope = fParam[pos++];
    fSplines[i].alpha = fParam[pos++];
    if (i>0) {
      fSplines[i-1].endx = fSplines[i].startx;
      fSplines[i-1].endy = fSplines[i].starty;
    }
  }
  fSplines[fNsplines-1].endx = fParam[pos];
  fSplines[fNsplines-1].endy = fSplines[fNsplines-1].starty + 
                               fSplines[fNsplines-1].slope*(fSplines[fNsplines-1].endx-fSplines[fNsplines-1].startx) + 
                               fSplines[fNsplines-1].alpha*(fSplines[fNsplines-1].endx-fSplines[fNsplines-1].startx)*
                                                           (fSplines[fNsplines-1].endx-fSplines[fNsplines-1].startx);
}


void CalLinearity::Store(DbiOutRowStream& ors,
			 const DbiValidityRec* /* vrec */) const 
{
  // Pack fSplines[] into fParam[]
  int pos = 0;
  for (int i=0;i<fNsplines;i++) {
    fParam[pos++] = fSplines[i].startx;
    fParam[pos++] = fSplines[i].starty;
    fParam[pos++] = fSplines[i].slope;
    fParam[pos++] = fSplines[i].alpha;
  }
  fParam[pos] = fSplines[fNsplines-1].endx;
  for (int i=pos+1;i<40;i++) fParam[i] =0;
  fNumber = fNsplines;
  ors << fAggregateNo << fTask << fStripEndKey << fStripEndId << fNumber;
  for (int i=0;i<40;i++) ors << fParam[i];

}


Float_t CalLinearity::ADCtoLin(const Float_t charge) const 
{
  switch (fTask) {
  default:
    MSG("Calib",Msg::kWarning)<<"CalLinearity::ADCtoLin has task "
				   << fTask <<" but doesn't know how to decode it. \n Defaulting to task 2\n";
  case 2:
    // This is the straight line plus a set of quadratic splines.
    // Everything is represented by a quadratic spline, though.
    for (int i=0;i<fNsplines; i++) {
      if (fSplines[i].endy >= charge && fSplines[i].starty<= charge) {
	// use segment i
	float m = fSplines[i].slope;
	float a = fSplines[i].alpha;
	float c = charge - fSplines[i].starty;
	float x0 = fSplines[i].startx;
	if (a==0) return x0 + c/m;
	return x0 + (a/fabs(a)) * (-m + TMath::Sqrt(m*m+4*a*c))/(2*fabs(a));
      }
    }
    // No suitable spline segment - return xmax.
    //   (but not the max of the extrapolation spline!)
    if (fNsplines==1) return fSplines[0].endx;
    return fSplines[fNsplines-2].endx;
    break;
  };
}

Float_t CalLinearity::LintoADC(const Float_t charge) const 
{
  switch (fTask) {
  default:
    MSG("Calib",Msg::kWarning)<<"CalLinearity::ADCtoLin has task "
				   << fTask <<" but doesn't know how to decode it. \n Defaulting to task 2\n";
  case 2:
    // This is the straight line plus a set of quadratic splines.
    // Everything is represented by a quadratic spline, though.
    for (int i=0;i<fNsplines; i++) {
      if (fSplines[i].endx >= charge && fSplines[i].startx<= charge) {
	// use segment i
	float m = fSplines[i].slope;
	float a = fSplines[i].alpha;
	float y0 = fSplines[i].starty;
	float x0 = fSplines[i].startx;
	return y0 + m*(charge-x0) + a*(charge-x0)*(charge-x0);
      }
    }
    // No suitable spline segment - return xmax.
    //   (but not the max of the extrapolation spline!)
    // WOn't happe - we'll always extrapolate.
    if (fNsplines==1) return fSplines[0].endy;
    return fSplines[fNsplines-2].endy;
    break;
  };
}