#include <fstream>
#include <iomanip>
#include <cstdlib>
#include <iostream>

#include "Alignment/AlHolder.hh"
#include "Alignment/AlDigiCode.hh"


const double AlHolder::SMALLTX  = 0.0020;
const double AlHolder::SMALLTY  = 0.0005;
const double AlHolder::SMALLTZ  = 0.0010;
const double AlHolder::SMALLAX  = 0.00025;
const double AlHolder::SMALLAY  = 0.001;
const double AlHolder::SMALLAZ  = 0.0010;
const double AlHolder::SMALLDR  = 0.0020;
const double AlHolder::SMALLSTY = 0.0005;
const double AlHolder::SMALLSTZ = 0.0020;
const double AlHolder::SMALLSAX = 0.0002;
const double AlHolder::SMALLBYY = 0.00025;
const double AlHolder::SMALLBYZ = 0.0005;
const double AlHolder::SMALLBZZ = 0.001;

const double AlHolder::SMALLLADTX  = 0.0020;
const double AlHolder::SMALLLADTY  = 0.0005;
const double AlHolder::SMALLLADTZ  = 0.0010;
const double AlHolder::SMALLLADAX  = 0.00006;
const double AlHolder::SMALLLADAY  = 0.00025;
const double AlHolder::SMALLLADAZ  = 0.0010;

const double AlHolder::SMALLBARTX  = 0.0010;
const double AlHolder::SMALLBARTY  = 0.0010;
const double AlHolder::SMALLBARTZ  = 0.0020;
const double AlHolder::SMALLBARAX  = 0.00006;
const double AlHolder::SMALLBARAY  = 0.00006;
const double AlHolder::SMALLBARAZ  = 0.00004;

const double AlHolder::SMALLFRATX  = 0.0010;
const double AlHolder::SMALLFRATY  = 0.0010;
const double AlHolder::SMALLFRATZ  = 0.0020;
const double AlHolder::SMALLFRAAX  = 0.00002;
const double AlHolder::SMALLFRAAY  = 0.00002;
const double AlHolder::SMALLFRAAZ  = 0.00004;


AlHolder::AlHolder()
{
}

AlHolder::~AlHolder()
{
}

int AlHolder::writeToFile(char* file)
{
  fstream os( file, ios::out );
  os << *this;
  os.close();
  return 0;
}

int AlHolder::readFromFile(char* file)
{
  fstream is( file, ios::in );
  is >> *this;
  is.close();
  return 0;
}

void AlHolder::setToTestPattern0() {

  // set everything to zero
  *this = AlHolder();

  AlDigiCode ad(AlDigiCode::LADDER,AlDigiCode::ALL);

  for(ad=0; ad<NLADDERS; ad++){
    int b = ad.getBarrel();
    int w = ad.getWedge();
    int l = ad.getLayer();

    if( b==0 && w<=5 && l==(w%6) ) _ladder[ad].tx = SMALLLADTX;
    if( b==0 && w>=6 && l==(w%6) ) _ladder[ad].ty = SMALLLADTY;
    if( b==1 && w<=5 && l==(w%6) ) _ladder[ad].tz = SMALLLADTZ;
    if( b==1 && w>=6 && l==(w%6) ) _ladder[ad].ax = SMALLLADAX;
    if( b==2 && w<=5 && l==(w%6) ) _ladder[ad].ay = SMALLLADAY;
    if( b==2 && w>=6 && l==(w%6) ) _ladder[ad].az = SMALLLADAZ;
  }
}


void AlHolder::setToRandom(int seed )
{
  srand48(seed);

  for(int i=0; i<NWAFERS; i++){
    _wafer[i].tr.tx = (2.0*drand48()-1.0) * SMALLTX;
    _wafer[i].tr.ty = (2.0*drand48()-1.0) * SMALLTY;
    _wafer[i].tr.tz = (2.0*drand48()-1.0) * SMALLTZ;
    _wafer[i].tr.ax = (2.0*drand48()-1.0) * SMALLAX;
    _wafer[i].tr.ay = (2.0*drand48()-1.0) * SMALLAY;
    _wafer[i].tr.az = (2.0*drand48()-1.0) * SMALLAZ;
    _wafer[i].dr    = (2.0*drand48()-1.0) * SMALLDR;
    _wafer[i].sty   = (2.0*drand48()-1.0) * SMALLSTY;
    _wafer[i].stz   = (2.0*drand48()-1.0) * SMALLSTZ;
    _wafer[i].sax   = (2.0*drand48()-1.0) * SMALLSAX;
    _waferwarp[i].byy    = (2.0*drand48()-1.0) * SMALLBYY;
    _waferwarp[i].byz    = (2.0*drand48()-1.0) * SMALLBYZ;
    _waferwarp[i].bzz    = (2.0*drand48()-1.0) * SMALLBZZ;
  }
  for(int i=0; i<NLADDERS; i++){
    _ladder[i].tx = (2.0*drand48()-1.0) * SMALLLADTX;
    _ladder[i].ty = (2.0*drand48()-1.0) * SMALLLADTY;
    _ladder[i].tz = (2.0*drand48()-1.0) * SMALLLADTZ;
    _ladder[i].ax = (2.0*drand48()-1.0) * SMALLLADAX;
    _ladder[i].ay = (2.0*drand48()-1.0) * SMALLLADAY;
    _ladder[i].az = (2.0*drand48()-1.0) * SMALLLADAZ;
  }
  for(int i=0; i<NBARRELS; i++){
    _barrel[i].tx = (2.0*drand48()-1.0) * SMALLBARTX;
    _barrel[i].ty = (2.0*drand48()-1.0) * SMALLBARTY;
    _barrel[i].tz = (2.0*drand48()-1.0) * SMALLBARTZ;
    _barrel[i].ax = (2.0*drand48()-1.0) * SMALLBARAX;
    _barrel[i].ay = (2.0*drand48()-1.0) * SMALLBARAY;
    _barrel[i].az = (2.0*drand48()-1.0) * SMALLBARAZ;
  }
  _spaceframe.tx = (2.0*drand48()-1.0) * SMALLFRATX;
  _spaceframe.ty = (2.0*drand48()-1.0) * SMALLFRATY;
  _spaceframe.tz = (2.0*drand48()-1.0) * SMALLFRATZ;
  _spaceframe.ax = (2.0*drand48()-1.0) * SMALLFRAAX;
  _spaceframe.ay = (2.0*drand48()-1.0) * SMALLFRAAY;
  _spaceframe.az = (2.0*drand48()-1.0) * SMALLFRAAZ;

}

const AlHolder AlHolder::operator + (const AlHolder& rhs) 
{
  AlHolder r;
  for(int i=0; i<AlHolder::NWAFERS ; i++) 
                 { r._wafer[i]      = _wafer[i] + rhs._wafer[i]; 
                   r._waferwarp[i]  = _waferwarp[i] + rhs._waferwarp[i]; }
  for(int i=0; i<AlHolder::NLADDERS; i++) 
                 { r._ladder[i] = _ladder[i] + rhs._ladder[i]; }
  for(int i=0; i<AlHolder::NBARRELS; i++) 
                 { r._barrel[i] = _barrel[i] + rhs._barrel[i]; }
  r._spaceframe = _spaceframe + rhs._spaceframe;

  return r;
}

const AlHolder AlHolder::operator - (const AlHolder& rhs) 
{
  AlHolder r;
  for(int i=0; i<AlHolder::NWAFERS ; i++) 
                 { r._wafer[i]      = _wafer[i] - rhs._wafer[i]; 
                   r._waferwarp[i]  = _waferwarp[i] - rhs._waferwarp[i]; }
  for(int i=0; i<AlHolder::NLADDERS; i++) 
                 { r._ladder[i] = _ladder[i] - rhs._ladder[i]; }
  for(int i=0; i<AlHolder::NBARRELS; i++) 
                 { r._barrel[i] = _barrel[i] - rhs._barrel[i]; }
  r._spaceframe = _spaceframe - rhs._spaceframe;

  return r;
}


const AlHolder operator * (const double& lhs, const AlHolder& rhs) 
{
  AlHolder r;
  for(int i=0; i<AlHolder::NWAFERS ; i++) 
                 { r._wafer[i]      = lhs * rhs._wafer[i]; 
                   r._waferwarp[i]  = lhs * rhs._waferwarp[i]; }
  for(int i=0; i<AlHolder::NLADDERS; i++) 
                 { r._ladder[i] = lhs * rhs._ladder[i]; }
  for(int i=0; i<AlHolder::NBARRELS; i++) 
                 { r._barrel[i] = lhs * rhs._barrel[i]; }
  r._spaceframe = lhs * rhs._spaceframe;
  
  return r;
}

const AlTransform operator * (const double& lhs, 
				      const AlTransform& rhs)
{
  return AlTransform(lhs * rhs.tx, lhs * rhs.ty, lhs * rhs.tz, 
		   lhs * rhs.ax, lhs * rhs.ay, lhs * rhs.az);
}

const AlWaferTable operator * (const double& lhs, 
				  const AlWaferTable& rhs)
{
  return AlWaferTable(lhs * rhs.tr, lhs * rhs.dr, 
		      lhs * rhs.sty, lhs * rhs.stz, lhs * rhs.sax);
}

const AlWaferWarpTable operator * (const double& lhs, 
					   const AlWaferWarpTable& rhs)
{
  return AlWaferWarpTable(lhs * rhs.byy, lhs * rhs.byz, 
				  lhs * rhs.bzz);
}


std::istream& operator >> (std::istream& is, AlHolder& rhs) {

  AlDigiCode ad;

  ad.setUsedDetectors(AlDigiCode::ALL);

  char line[200];
  char c;
  int temp;

  while(!is.eof()) {

    is.get(c);
    if(c=='!') {
      is.getline(line,199);
    } else if(c=='0') {
      is >> temp >> temp >> ad >> rhs._spaceframe ;
    } else if(c=='1') {
      ad.setResolution(AlDigiCode::BARREL);
      is >> temp >> temp >> ad;
      is >> rhs._barrel[ad];
    } else if(c=='2') {
      ad.setResolution(AlDigiCode::LADDER);
      is >> temp >> temp >> ad;
      is >> rhs._ladder[ad];
    } else if(c=='3' || c=='5' ) {
      ad.setResolution(AlDigiCode::WAFER);
      is >> temp >> temp >> ad;
      is >> rhs._wafer[ad];
      if(c=='5') {
	is >> rhs._wafer[ad].sty >> rhs._wafer[ad].stz >> rhs._wafer[ad].sax;
      }
    } else if(c=='4') {
      ad.setResolution(AlDigiCode::WAFER);
      is >> temp >> temp >> ad;
      is >> rhs._waferwarp[ad];
    }

  }           // while loop for chars

  return is;
}


std::ostream& operator << (std::ostream& os, const AlHolder& rhs) {

  AlDigiCode ad;

  ad.setUsedDetectors(AlDigiCode::ALL);

  os 
    << "!       Si Alignment Table File V2"<< std::endl
    << "! t=table number: 0=spaceframe, 1=barrel, " 
         << "2=ladder, 3=wafer, 4=waferwarp " << std::endl
    << "!                 5=wafer with z side info " << std::endl
    << "! i=the table entry unofficial index " << std::endl
    << "! c=the table entry official ChannelID " << std::endl
    << "! b=barrel, l=layer, w=wedge, h=half-ladder, f=wafer, s=side" << std::endl
    << "! " << std::endl
    << "! " << std::endl
    << "! " << std::endl
    << "!           Space Frame Table " << std::endl
    << "!t    i      c   b l  w h f s    Tx        Ty       Tz        "
    << "Ax         Ay         Az" << std::endl;

  // put out the spaceframe alignment
  ad.setResolution(AlDigiCode::FRAME);
  ad=0;
  os << setw(2) << 0 << setw(5) << 0 
     << setw(7) << ad.getCode() << "   " << ad 
     << rhs._spaceframe << std::endl;

  os
    << "! " << std::endl
    << "!           Barrel Table " << std::endl
    << "!t    i      c   b l  w h f s    Tx        Ty       Tz        "
    << "Ax         Ay         Az" << std::endl;

  // put out the barrel alignment
  ad.setResolution(AlDigiCode::BARREL);
  for(ad=0; ad<AlHolder::NBARRELS; ad++) {
    os << setw(2) << 1 << setw(5) << ad.getIndex() 
       << setw(7) << ad.getCode() << "   " << ad 
       << rhs._barrel[ad] << std::endl;
  }

  os
    << "! " << std::endl
    << "!           Ladder Table " << std::endl
    << "!t    i      c   b l  w h f s    Tx        Ty       Tz        "
    << "Ax         Ay         Az" << std::endl;

  // put out the ladder alignment
  ad.setResolution(AlDigiCode::LADDER);
  for(ad=0; ad<AlHolder::NLADDERS; ad++) {
    os << setw(2) << 2 << setw(5) << ad.getIndex() 
       << setw(7) << ad.getCode() << "   " << ad 
       << rhs._ladder[ad] << std::endl;
  }

  os
    << "! " << std::endl
    << "!           Wafer Table " << std::endl
    << "!t    i      c   b l  w h f s    Tx        Ty       Tz        "
    << "Ax         Ay         Az       Dr       sTy       sTz       sAx" 
    << std::endl;

  // put out the wafer alignment
  ad.setResolution(AlDigiCode::WAFER);
  for(ad=0; ad<AlHolder::NWAFERS; ad++) {
    os << setw(2) << 5 << setw(5) << ad.getIndex()  
       << setw(7) << ad.getCode() << "   " << ad 
       << rhs._wafer[ad] << std::endl;
  }

  os
    << "! " << std::endl
    << "!           Wafer Warp Table " << std::endl
    << "!t    i      c   b l  w h f s     Byy        Byz         Bzz"<< std::endl;

  // put out the wafer warp tables
  for(ad=0; ad<AlHolder::NWAFERS; ad++) {
    os << setw(2) << 4 << setw(5) << ad.getIndex()  
       << setw(7) << ad.getCode() << "   " << ad 
       << rhs._waferwarp[ad] << std::endl;
  }
  return os;
}

std::ostream& operator << (std::ostream& os, const AlTransform& rhs) {
  os << setiosflags(ios::showpoint|ios::fixed) 
     << setprecision(5) 
     << setw(9) << rhs.tx << setw(9) << rhs.ty << setw(9) << rhs.tz 
     << setprecision(7) 
     << setw(11) << rhs.ax << setw(11) << rhs.ay << setw(11) << rhs.az 
     << resetiosflags(ios::showpoint|ios::fixed) ;
  return os;
}

std::ostream& operator << (std::ostream& os, const AlWaferTable& rhs) {
  os << rhs.tr 
     << setiosflags(ios::showpoint|ios::fixed)
     << setprecision(5) 
     << setw(9) << rhs.dr 
     << setw(9) << rhs.sty 
     << setw(9) << rhs.stz 
     << setprecision(7) 
     << setw(11) << rhs.sax 
     << resetiosflags(ios::showpoint|ios::fixed) ;
  return os;
}

std::ostream& operator << (std::ostream& os, const AlWaferWarpTable& rhs) {
  os << setiosflags(ios::showpoint|ios::fixed)
     << setprecision(7) 
     << setw(11) << rhs.byy 
     << setw(11) << rhs.byz 
     << setw(11) << rhs.bzz 
     << resetiosflags(ios::showpoint|ios::fixed) ;
  return os;
}

std::istream& operator >> (std::istream& is, AlTransform& rhs) {
  is >> rhs.tx >> rhs.ty >> rhs.tz >> rhs.ax >> rhs.ay >> rhs.az;
  return is;
}

std::istream& operator >> (std::istream& is, AlWaferTable& rhs) {
  is >> rhs.tr >> rhs.dr;
  // note the AlHolder operator reads sty, stz and sax if they are there
  return is;
}

std::istream& operator >> (std::istream& is, AlWaferWarpTable& rhs) {
  is >> rhs.byy >> rhs.byz >> rhs.bzz;
  return is;
}

////////////////////////////  set values form arbitrary source
void AlHolder::setFrame(const double tx, const double ty, const double tz,
			  const double ax, const double ay, const double az) 
{
  _spaceframe.tx = tx;  _spaceframe.ty = ty; _spaceframe.tz = tz;
  _spaceframe.ax = ax;  _spaceframe.ay = ay; _spaceframe.az = az;
}

void AlHolder::setBarrel(AlDigiCode& al,
	    const double tx, const double ty, const double tz,
	    const double ax, const double ay, const double az) 
{
  al.setResolution(AlDigiCode::BARREL);
  int i = al.getIndex();
  _barrel[i].tx = tx;  _barrel[i].ty = ty; _barrel[i].tz = tz;
  _barrel[i].ax = ax;  _barrel[i].ay = ay; _barrel[i].az = az;

}

void AlHolder::setLadder(AlDigiCode& al,
	    const double tx, const double ty, const double tz,
	    const double ax, const double ay, const double az)
{
  al.setResolution(AlDigiCode::LADDER);
  int i = al.getIndex();
  if(i>=352 || i<0) cout << "AlHolder::setLadder: bad wafer index " << endl;
  _ladder[i].tx = tx;  _ladder[i].ty = ty; _ladder[i].tz = tz;
  _ladder[i].ax = ax;  _ladder[i].ay = ay; _ladder[i].az = az;
}

void AlHolder::setWafer(AlDigiCode& al,
	    const double tx, const double ty, const double tz,
	    const double ax, const double ay, const double az,
	    const double dr,
	    const double sty, const double stz, const double sax)
{
  al.setResolution(AlDigiCode::WAFER);
  int i = al.getIndex();
  if(i>=1752 || i<0) cout << "AlHolder::setWafer: bad wafer index " << endl;
  _wafer[i].tr.tx = tx;  _wafer[i].tr.ty = ty; _wafer[i].tr.tz = tz;
  _wafer[i].tr.ax = ax;  _wafer[i].tr.ay = ay; _wafer[i].tr.az = az;
  _wafer[i].dr = dr; 
  _wafer[i].sty = sty;  _wafer[i].stz = stz; _wafer[i].sax = sax;
}

void AlHolder::setWarp(AlDigiCode& al,
	    const double byy, const double byz, const double bzz)
{
  al.setResolution(AlDigiCode::WAFER);
  int i = al.getIndex();
  _waferwarp[i].byy = byy;  _waferwarp[i].byz = byz; _waferwarp[i].bzz = bzz;

}

////////////////////////////  get values for arbitrary caller
void AlHolder::getFrame( double& tx,  double& ty,  double& tz,
			 double& ax,  double& ay,  double& az) 
{
  tx = _spaceframe.tx;  ty = _spaceframe.ty; tz = _spaceframe.tz;
  ax = _spaceframe.ax;  ay = _spaceframe.ay; az = _spaceframe.az;
}

void AlHolder::getBarrel(AlDigiCode& al,
	     double& tx, double& ty, double& tz,
	     double& ax, double& ay, double& az) 
{
  al.setResolution(AlDigiCode::BARREL);
  int i = al.getIndex();
  tx = _barrel[i].tx;  ty = _barrel[i].ty;  tz = _barrel[i].tz;
  ax = _barrel[i].ax;  ay = _barrel[i].ay;  az = _barrel[i].az;

}

void AlHolder::getLadder(AlDigiCode& al,
	     double& tx, double& ty, double& tz,
	     double& ax, double& ay, double& az)
{
  al.setResolution(AlDigiCode::LADDER);
  int i = al.getIndex();
  tx = _ladder[i].tx;  ty = _ladder[i].ty;  tz = _ladder[i].tz;
  ax = _ladder[i].ax;  ay = _ladder[i].ay;  az = _ladder[i].az;
}

void AlHolder::getWafer(AlDigiCode& al,
	     double& tx, double& ty, double& tz,
	     double& ax, double& ay, double& az,
	     double& dr,
	     double& sty, double& stz, double& sax)
{
  al.setResolution(AlDigiCode::WAFER);
  int i = al.getIndex();
  tx = _wafer[i].tr.tx;  ty = _wafer[i].tr.ty;  tz = _wafer[i].tr.tz;
  ax = _wafer[i].tr.ax;  ay = _wafer[i].tr.ay;  az = _wafer[i].tr.az;
  dr = _wafer[i].dr; 
  sty = _wafer[i].sty;  stz = _wafer[i].stz;  sax = _wafer[i].sax;
}

void AlHolder::getWarp(AlDigiCode& al,
	     double& byy, double& byz, double& bzz)
{
  al.setResolution(AlDigiCode::WAFER);
  int i = al.getIndex();
  byy = _waferwarp[i].byy;  byz = _waferwarp[i].byz;  bzz = _waferwarp[i].bzz;
}


  // get values to arbitrary caller
AlTransform AlHolder::getFrame() {
  return _spaceframe;
}

AlTransform AlHolder::getBarrel(AlDigiCode al) {
  al.setResolution(AlDigiCode::BARREL);
  return _barrel[al];
}

AlTransform AlHolder::getLadder(AlDigiCode al) {
  al.setResolution(AlDigiCode::LADDER);
  return _ladder[al];
}

AlWaferTable AlHolder::getWafer(AlDigiCode al) {
  al.setResolution(AlDigiCode::WAFER);
  return _wafer[al];
}

AlWaferWarpTable AlHolder::getWarp(AlDigiCode al) {
  al.setResolution(AlDigiCode::WAFER);
  return _waferwarp[al];
}