#include "Alignment/CustomBeam.hh"
#include <iostream>
#include <iomanip>

#include "AbsEnv/AbsEnv.hh"

#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Geometry/Point3D.h"
#include "CLHEP/Geometry/Vector3D.h"

using namespace std;

CustomBeam::CustomBeam(string filename):
                       _fname(filename),
		       _fhandle(NULL),
		       _cov(3,0){ 

  string defstring("default");
  _cov[0][0] = 0.0026*0.0026;
  _cov[1][1] = 0.0026*0.0026;
  _cov[2][2] = 35.0*35.0;

  _x0 = 0.; 
  _y0 = 0.;
  _Ax = 0.;
  _Ay = 0.;

  _isValid   = false;
  if(_fname!=defstring){
    newFile(_fname);
  }
}

CustomBeam::~CustomBeam(){
  if(_fhandle != NULL)  fclose(_fhandle);
}

void CustomBeam::newFile(std::string filename){
  _fname     = filename;
  _isValid   = false;
  
  if(_fhandle != NULL)  fclose(_fhandle);

  _fhandle = fopen(_fname.c_str(),"r");
  if (_fhandle == NULL) {
    cout << endl << endl;
    cout << "Could not open file: " << _fname 
	 << endl << endl;
    
    _fileValid = false;
  }else{
    char dummy[300];
    cout << endl  << "BEAMLINE FILE" << endl;
    fgets(dummy,300,_fhandle);
    cout << dummy << endl;
    fgets(dummy,300,_fhandle);
    cout << dummy << endl;
  }


}

// true if the current contents are correct for this run
bool CustomBeam::isValid() { 
  return  _isValid;
}

// the following accessors will give the best beam position
// available in the database

// the beam position: double beamx = CustomBeam.position().x();
HepPoint3D CustomBeam::position(double z){
  if(!isValid()) return  HepPoint3D(-99.0,-99.0,-999.0);
  return HepPoint3D(_x0 + _Ax*z, _y0 + _Ay*z, z);
}

HepVector CustomBeam::positionVector(double z){
  HepPoint3D p = position(z);
  HepVector v(3);
  v[0] = p.x();
  v[1] = p.y();
  v[2] = p.z();
  return v;
}

// the beam slope: double xslope = CustomBeam.slope.x();
Hep3Vector CustomBeam::slope(){
  if(!isValid()) return  Hep3Vector(-99.0,-99.0,-999.0);
  double sx = _Ax;
  double sy = _Ay;
  double zn = sqrt( 1.0 - sx*sx - sy*sy);
  return Hep3Vector(sx, sy, zn);
}

// a 3x3 covariance matrix of position, element (3,3) = 35cm.
// the x,y elements include intrinsic beam size and beam position 
// measurement errors (position and slope)
HepSymMatrix CustomBeam::cov3(double z) {

  HepSymMatrix cov(3,0);
  // Use sensible defaults
  cov[0][0] = 0.0026*0.0026;
  cov[1][1] = 0.0026*0.0026;
  cov[2][2] = 35.0*35.0;

  if(isValid())return _cov;
  else return cov;
}

// these dump the contents, good for calling at begin run
// print a few-line summary
void CustomBeam::printSummary(){

  if(!isValid()) {
    std::cout << "CustomBeam: Database handle does not contain data " 
	      << std::endl;
    return;
  }

  int  run = (AbsEnv::instance())->runNumber();
  std::cout << "CustomBeam Postion for run " << run << " :" << std::endl;

  std::ios_base::fmtflags iflsave = std::cout.flags(ios::showpoint|ios::fixed);
  int ipre = std::cout.precision(5);

  std::cout << setprecision(5) 
	    << " x0: " << setw(9) <<  position().x()
	    << " y0: " << setw(9) <<  position().y()
	    << setprecision(4) 
	    << " slope x (mr): " << setw(7) << slope().x() *1000
	    << " slope y (mr): " << setw(7) << slope().y() *1000
	    << std::endl;

  std::cout.flags(iflsave);
  std::cout.precision(ipre);

}

// print everything
void CustomBeam::printAll(){

  if(!isValid()) {
    std::cout << "CustomBeam: Database handle does not contain data " 
	      << std::endl;
    return;
  }

  int  run = (AbsEnv::instance())->runNumber();
  std::cout << "CustomBeam Postion for run " << run << " :" << std::endl;
  
  std::ios_base::fmtflags iflsave = std::cout.flags(ios::showpoint|ios::fixed);
  int ipre = std::cout.precision(5);

  std::cout << setprecision(5) 
	    << " x0: " << setw(9) <<  position().x()
	    << " y0: " << setw(9) <<  position().y()
	    << setprecision(4) 
	    << " slope x (mr): " << setw(7) << slope().x() *1000
	    << " slope y (mr): " << setw(7) << slope().y() *1000
	    << std::endl;


  std::cout << "         covariance matrix of x,y,x',y' : " << std::endl;
  std::cout.unsetf(ios::showpoint|ios::fixed);
  std::cout << "   " <<  cov3()
	    << std::endl;

  std::cout.flags(iflsave);
  std::cout.precision(ipre);

}


/////////////////////////////////////////////////////////////////////////
//  expert interface below here, typical user does not need these
/////////////////////////////////////////////////////////////////////////

// set run and version number, -1 means current run/latest version
// and retrieve db record
// once this interface is triggered with setDatabase, this needs to be 
// called explicitly at begin run (database manager no longer does it)
int CustomBeam::loadRun(int run, int version) {

  if(run < 0) run = (AbsEnv::instance())->runNumber();

  float x,ax,y,ay;
  int runN;
  _isValid = false;

  if (_fhandle == NULL) {
    cout << endl << endl;
    cout << "Could not open file: " << _fname 
	 << endl << endl;

    _fileValid = false;
    _isValid   = false;
  }else{
    char dummy[300];
    int n;
    rewind(_fhandle);
    fgets(dummy,300,_fhandle);fgets(dummy,300,_fhandle);
    while (true) {
      n = fscanf(_fhandle,"%d %f %f %f %f",&runN,&x,&ax,&y,&ay);

      if (runN == run)  { _isValid = true;  break; }
      if (n <= 0)        { _isValid = false; break; }
    }
  }
  if(isValid()){
    _x0= x; _Ax = ax; _y0 = y; _Ay = ay;
  }

  return 0;

}

string CustomBeam::getStatus() {
  if(!isValid()) return string("invalid table");
  return string("undefined");
}

int CustomBeam::getVersion() {
  if(!isValid()) return -1;
  return 0;
}

int CustomBeam:: getCid() {
  if(!isValid()) return -1;
  return 0;
}

int CustomBeam::loadCid(int cid)
{
  return 0;
}