#include <CLHEP/Matrix/Matrix.h>
#include <CLHEP/Matrix/Vector.h>

using namespace std;

#include "FDC/DFDCPseudo_factory.h"
#include "MyTrajectory.h"
#include "residFunc.h"
#include "combinedResidFunc.h"

// constructor, takes pointer to vector of pseudopoints and a pointer
// to a trajectory
combinedResidFunc::combinedResidFunc(vector<const DFDCPseudo*> *pseudopoints,
				     vector<const DCDCTrackHit*> *trackHits,
				     MyTrajectory *trajectory) : 
  n(pseudopoints->size()), n_cdc(trackHits->size()), ppPtr(pseudopoints),
  trkhitPtr(trackHits), trajPtr(trajectory), delta(trajPtr->getDelta()),
  debug_level(1) {
}

void combinedResidFunc::resid(const HepVector *x, void *data, HepVector *f){
// input parameters
//   x: pointer to a vector of fit parameters
//   data: ???
// output parameters
//   f: pointer to vector of residuals
  double doca;
  if (debug_level > 2) {
    cout << "combinedResidFunc::resid: resid called\n";
    cout << "                          params: " << *x;
  }
  // do a swim with the input parameters
  trajPtr->swim(*x);
  // populate f vector with residuals
  HepVector point(3);
  for (unsigned int i = 0; i < n; i++) {
    point = pseudo2HepVector(*((*ppPtr)[i]));
    (*f)(i + 1) = trajPtr->doca(point);
  }
  DLine line;
  for (unsigned int j = 0; j < n_cdc; j++) {
    line = trackhit2line(*((*trkhitPtr)[j]));
    doca = trajPtr->doca(line);
    if (debug_level > 2) cout << "combinedResidFunc::resid: doca from trajectory = " << doca << endl;
    (*f)(n + j + 1) = doca;
  }
  if (debug_level > 2) cout << "combinedResidFunc::resid: resids:" << *f;
  trajPtr->clear();
};

void combinedResidFunc::deriv(const HepVector *x, void *data, HepMatrix *J){
  if (debug_level > 2) {
    cout << "combinedResidFunc::deriv: deriv called\n";
    cout << "                          params: " << *x << endl;
  }
  // save base parameters
  HepVector xBase = *x;
  // do central swim
  trajPtr->swim(xBase);
  // store pseudo points as three vectors
  vector<HepVector *>pPoints;
  HepVector *thisPointPtr;
  for (unsigned int i = 0; i < n; i++) {
    thisPointPtr = new HepVector(3);
    *thisPointPtr = pseudo2HepVector(*((*ppPtr)[i]));
    pPoints.push_back(thisPointPtr);
  }
  // store base residuals
  HepVector docaBase(n);
  for (unsigned int i = 0; i < n; i++) {
    //    cout << "storing base resid for pp "<< i << *(pPoints[i]);
    docaBase(i + 1) = trajPtr->doca(*(pPoints[i]));
  }
  //  cout << "base resids:" << docaBase;
  trajPtr->clear();
  // calculate Jacobian
  unsigned int p = trajPtr->getNumberOfParams();
  //  cout << "combinedResidFunc::deriv p = " << p << endl;
  HepVector xThis(p);
  double docaThis;
  int iHep, jHep; // index for HepVector () notation
  for (unsigned int i = 0; i < p; i++) {
    iHep = i + 1;
    xThis = xBase; // set params back to base
    xThis(iHep) = xBase(iHep) + delta[i];
    //    cout << "perturbed params: iHep = " << iHep << ", values:" << xThis << endl;
    // do the perturbed swim
    trajPtr->swim(xThis);
    for (unsigned int j = 0; j < n; j++) {
      jHep = j + 1;
      docaThis = trajPtr->doca(*(pPoints[j]));
      //      cout << "resid " << j << " = " << docaThis << endl;
      (*J)(jHep, iHep) = (docaThis - docaBase(jHep))/delta[i];
    }
    trajPtr->clear();
  }
  //  cout << "Jacobian:" << *J;
  for (unsigned int i = 0; i < n; i++) {
    delete pPoints[i];
  }
};

void combinedResidFunc::residAndDeriv(const HepVector *x, void *data, HepVector *f,
		   HepMatrix *J){};

HepVector combinedResidFunc::pseudo2HepVector(const DFDCPseudo &ppoint) {
  HepVector point(3);
  point(1) = ppoint.x;
  point(2) = ppoint.y;
  point(3) = ppoint.wire->origin(2);
  return point;
}

DLine combinedResidFunc::trackhit2line(const DCDCTrackHit &trkhit) {
  const DCDCWire *wire = trkhit.wire;
  double x = wire->origin.X();
  double y = wire->origin.Y();
  double z = wire->origin.Z();
  double phi_wire = atan2(y, x);
  double phi = phi_wire + PIOVER2;
  double theta = wire->stereo;
  DLine line(x, y, z, theta, phi);
  return line;
}