/*

  Currently this is out of date and should be ignored,
it is a place holder  - 4/01 Ray 


  standalone program that converts the mechincal database files
into a partial alignment

uses hardware layer numbering (0-4 for SVX-II)
except for the last step of inserting into AlHolder

the input global.dat is in the following format:
N wedge (1-36) in +phi direction  (these changed to 0-n on input)
N_wafer (1-6) 1 is outermost
delta_R
x_global      radius is center of Silicon
y_global      meas in mm
z_global
delta r   again


internal coordinates are all x outward, y = phi, z = cdf z


z barrels:
  {-48.8680,-28.132 ,NONEXISTENT},  b0 l6, outer hb, inner hb
  {-78.1970,-55.735,NONEXISTENT}},  b0 l7

  {28.3940,49.1300,NONEXISTENT},   b2 l6
  {55.6880,78.1500,NONEXISTENT}}   b2 l7

radii
19.77,28.57
20.23,28.93

if hb=1, rotate about X
p = wedge number
double dphi = p * 2.0 * M_PI / nPhi;
dphi = dphi + SVX_LADDER_PHI[b][l];       for 6 = (360/48.0)*(M_PI/180.0), 7=0

 */


#include <math.h>
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <cstring>
#include <cmath>
#include <vector>
#include "TROOT.h"
#include "TFile.h"
#include "TTree.h"
#include "TBranch.h"
#include "TMinuit.h"
#include "TH1.h"
#include "TString.h"

#include "Alignment/AlHolder.hh"

using namespace std;

class Ladder {
public:
  int barrel;
  int wedge;
  int layer;
  bool flip;
  char name[5];
  double width;  // ladder width
  double wafLength;  // silicon wafer length
  // the transverse measurements, two positions for each wafer
  double rr[6];   // transverse measurements of the wafers
  double pp[6];   // in global CDF coordinates
  double xx[6];
  double yy[6];
  double zz[6];
  double rrnom;   // nominal transverse measurements of the wafers
  double ppnom;   // in global CDF coordinates
  double xxnom;
  double yynom;
  double zznom[6];

  double ladAx,ladAy,ladAz,ladTx,ladTy,ladTz;
  double bow;
  double wafTx[6]; // wafer displacements
  double wafTy[6];
  double wafTz[6];
  double wafAx[6];
  double wafAy[6];
  double wafAz[6];


  Ladder(){};
  Ladder(const char* namea, int b, int w, int l, bool fl);
  void process();
  void print(std::ostream& os);
  //void store(AlHolder& al);
};

class FileReader {
public:
  FileReader(){};
  void setDir(const char* dira);
  void readGlobal(Ladder** det);
  void processGlobal(Ladder** det);
  char dir[200];
  void readFileXY(Ladder** det);
  void readFileQuadratic(Ladder** det);
};

class Plotter {
public:
  Plotter();
  void makeTuple(Ladder& lad);
  void writeTuple();

  Ladder plotLadder;
  TFile* hfile;
  TTree* trTree;

  TH1D *hdy,*hdyl[5];
  TH1D *hax,*haxl[5];
  TH1D *hdz,*hdzl[5];
  TH1D *hbo,*hbol[5];
  TH1D *hbochi,*hbochil[5];
};

//////////////////////////////////////////
int main(int argv, char* argc[]){

  TROOT simple("simple","Translate mechanical alignment");

  Ladder* det[28+24*2+36*2];
  for(int i=0; i<148; i++) {det[i]=NULL;}
  FileReader fr;

  fr.setDir("mech/isl");
  fr.readGlobal(det);

  for (int i=0; i<148; i++) {
    if(det[i]) det[i]->process();
  }

  for (int i=0; i<148; i++) {
    if(det[i]) det[i]->print(std::cout);
  }

  Plotter* pPlo = new Plotter();

  for (int i=0; i<148; i++) {
  //for (int i=0; i<20; i++) {
    if(det[i]) pPlo->makeTuple(*det[i]);
  }

  pPlo->writeTuple();

  return 0;

}

                 ///////////// Ladder ///////////////////

Ladder::Ladder(const char* namea, int b, int w, int l, bool fl):
   barrel(b),wedge(w),layer(l),flip(fl)
{
  strncpy(name,namea,4);
  for(int i=0; i<6; i++) {
    xx[i]=-1001.0;
    yy[i]=-1001.0;
    zz[i]=-1001.0;
  }
  width = ( l==6 ? 5.8720 : 5.9260 );
  wafLength = ( l==6 ? 6.9020 : 7.4772 );
  double r0 = 0;
  double z0 = 0;
  double phi0 = 0;
  //double wgap = (l==6? 0.0100:0.0950); // between waf in hlad
  //>>>>>>>>>>>>>> patch until Aaron answers re gaps
  double wgap = (l==6? 0.0100:0.0100); // between waf in hlad
  double cgap = 0;  // gap between half ladders
  if(b==0) {
    if(l==6) z0 = (-48.8680 -28.132)/2.0;
    if(l==7) z0 = (-78.1970 -55.735)/2.0;
    if(l==6) cgap = (48.8680 -28.132 - 3*6.9020 -2*wgap);
    if(l==7) cgap = (78.1970 -55.735 - 3*7.4772 -2*wgap);
    //std::cout <<" cgap " << b << " " << l << " " << cgap << std::endl;
    if(l==6 && w%2==0) r0 = 19.77;
    if(l==7 && w%2==0) r0 = 28.57;
    if(l==6 && w%2!=0) r0 = 20.23;
    if(l==7 && w%2!=0) r0 = 28.93;
    if(l==6) phi0 = (2*M_PI/24.0)*w + (360/48.0)*(M_PI/180.0);
    if(l==7) phi0 = (2*M_PI/36.0)*w;
    z0 -= 0.2089;  // shift due to pshing against pins
  } else if(b==1) {
    z0 = 0;
    if(l==6) z0 = (-10.5780+10.1580)/2.0;
    if(l==6) cgap = (10.5780+10.1580 - 3*6.9020 -2*wgap)/2.0;
    if(w%2==0) r0 = 22.57;
    if(w%2!=0) r0 = 23.03;
    phi0 = (2*M_PI/28.0)*w;
    z0 -= 0.2089;  // shift due to pshing against pins
  } else {
    if(l==6) z0 = ( 48.8680 +28.132 )/2.0;
    if(l==7) z0 = ( 78.1970 +55.735)/2.0;
    if(l==6) cgap = (48.8680 -28.132 - 3*6.9020 -2*wgap);
    if(l==7) cgap = (78.1970 -55.735 - 3*7.4772 -2*wgap);
    if(l==6 && w%2==0) r0 = 19.77;
    if(l==7 && w%2==0) r0 = 28.57;
    if(l==6 && w%2!=0) r0 = 20.23;
    if(l==7 && w%2!=0) r0 = 28.93;
    if(l==6) phi0 = (2*M_PI/24.0)*w + (360/48.0)*(M_PI/180.0);
    if(l==7) phi0 = (2*M_PI/36.0)*w;
    z0 += 0.2089;  // shift due to pshing against pins
  }

  rrnom = r0;
  ppnom = phi0;
  xxnom = r0*cos(phi0);
  yynom = r0*sin(phi0);
  for(int i=0; i<6; i++) {
    float zn = (float(i)-2.5)*(l==6? 6.9020: 7.4772);
    zn += z0 + (i<=2 ? -cgap/2.0 : cgap/2.0 );
    zznom[i] = zn + ( i<=2 ? -(2-i)*wgap : (i-3)*wgap );
  }

  /*

  if (Layer<6) {
290     _phiGapSpecification=new SiPhiGapSpecification(l.svxDetectorLength,
291                                                    l.deadDistanceNearChipPhi, 
292                                                    l.deadDistanceAwayChipPhi, 
293                                                    l.distanceBetweenWafer);
294   }
295   else { // 3-wafer ladders
296     _phiGapSpecification=new SiPhiGapSpecification(l.svxDetectorLength,
297                                                    l.deadDistanceNearChipPhi, 
298                                                    l.deadDistanceAwayChipPhi, 
299                                                    l.distanceBetweenWafer,
300                                                    l.distanceBetweenWafer);
301  

offset =  -l.distanceBetweenWafer/2.0 + sum gaps
2*l.deadDistanceAwayChipZ + l.distanceBetweenWafer       gap1


  // Make a zGap specification
261     _zGapSpecification=new SiGapSpecification(l.nStripsZ,l.zPitch,
262                                               2*l.deadDistanceAwayChipZ
263                                               +l.distanceBetweenWafer);
264     // Then make the strip specs with corrected offset
265     _zStripSpecification=new SiZStripSpecification( l.nChannelsZ, l.zPitch,
266                   -l.svxDetectorLength + l.distanceToFirstZStrip
267                            -l.distanceBetweenWafer/2.0,
268                             l.svxDetectorsPerLadder,
269                           multiplexing,  l.stereoAngle );
270 

l6
 l.distanceBetweenWafer       =0.0100;
 l.svxDetectorLength          =6.9020;
 l.distanceToFirstZStrip      =0.1014;

l7
 l.distanceBetweenWafer     =0.0950;
 l.svxDetectorLength        =7.4772;
 l.distanceToFirstZStrip    =0.0850+0.0074;

  for (int i=1; i<_nwafer; i++) {
 27     float gapStart=static_cast<float>(i*_numStrips)/_nwafer;
 28     _stripGaps.push_back(make_pair(gapStart,gapStart));
 29     gapStart *= _pitch;
 30     _localGaps.push_back(make_pair(gapStart,gapStart+_gaps[i-1]));
 31

    const SiStripSpecification *pss=getPhiStripSpecification();
 89     return pss->getStripPitch()*stripLengths + pss->getOffset();
 90   }
 91   else {
 92     const SiZStripSpecification *zss=getZStripSpecification();
 93     const SiGapSpecification *zgap=getZGapSpecification();
 94     float loc=0;
 95     loc= zss->getStripPitch()*(  stripLengths +
 96                                  float(zss->getNumStrips() * ambig)  )
 97       + zss->getOffset();
 98     if (zgap)
 99       loc+=zgap->getGapFromStrip(stripLengths+float(zss->getNumStrips()*ambig));
100     return loc;
101   }
102

 */
}

void Ladder::process()
{ 
  double sxz=0,syz=0,szz=0,sz=0,sz2=0,sx=0,sy=0;
  double dx,dy,dz;
  double z;
  // position of ladder center in CDF coords
  double zl = (zznom[2] + zznom[3])/2.0;

  // fit for ladder position given all the wafers
  for(int i=0; i<6; i++) {
    z = zznom[i] - zl;

    dx = rr[i]-rrnom;
    dy = (pp[i]-ppnom)*rrnom;
    dz = zz[i]-zznom[i];

    sx += dx/6.0;
    sy += dy/6.0;

    sxz += dx*z;
    syz += dy*z;
    szz += dz/6.0;

    sz  += z/6.0;
    sz2 += z*z/6.0;
  }

  double den = sz2 - sz*sz;

  double dxdz = sxz / den;
  double dydz = syz / den;

  ladTx = sx;
  ladTy = sy;
  ladTz = szz;

  ladAx = -dydz;
  ladAy = dxdz;
  ladAz = 0.0;

  // now copy wafer pos with ladder pos removed
  double xw,yw,zw;
  double xwl,ywl,zwl;
  for(int i=0; i<6; i++) {
    // nominal wafer pos in ladder coords
    z = zznom[i] - zl;
    // the wafer in ladder coordinates
    xw = rr[i]-rrnom;
    yw = (pp[i]-ppnom)*rrnom;
    zw = zz[i] - zl;

    // the predicted wafer position based on the ladder position
    // in ladder coordinates
    xwl =  ladAy*z + ladTx;
    ywl = -ladAx*z + ladTy;
    zwl = ladTz + z;

    wafTx[i] = xw - xwl;
    wafTy[i] = yw - ywl;
    wafTz[i] = zw - zwl;
    std::cout << zw << " " << zwl << std::endl;
    wafAx[i] = -ladAx;
    wafAy[i] = -ladAy;
    wafAz[i] = 0.0;
  }

  double xcent = wafTx[2] + wafAy[2]*wafLength/2.0;
  double xlow =  wafTx[0] - wafAy[0]*wafLength/2.0;
  double xhgh =  wafTx[5] + wafAy[5]*wafLength/2.0;
  bow = xcent - (xhgh+xlow)/2.0;

}

void Ladder::print(std::ostream& os){
  os << name << " l,w,b,flip: " << layer << setw(3) << wedge 
     << " " << barrel << "   " << flip 
     << std::endl;
}

//void Ladder::store(AlHolder& al){}


               ///////////// FileReader  /////////////////

void FileReader::setDir(const char* dira) 
{
  strncpy(dir,dira,200);
}

void FileReader::readGlobal(Ladder** det) 
{
  char file[100];
  char line[1000];
  //string str;

  sprintf(file,"%s/global.dat",dir);
  //sprintf(file,"global.dat",dir);
  std::cout << "reading: " << file << std::endl;
  std::ifstream ifile(file);
  if(!ifile.good()) {
    std::cout << "Couldn't open file!!!!!!!!!  " << file << std::endl;
  }

  int wed,waf,index;
  double dr,x,y,z;

  // all of layer 7
  for(int b=0; b<3; b+=2) {
    int nb = (b==0? 0:1);
    for(int w=0; w<36; w++) {
      index = 28+2*24+nb*36+w;
      det[index] = new Ladder("ISL",b,w,7,false);
      Ladder& lad = *(det[index]);
      for(int i=0; i<6; i++) {
	ifile >> wed >> waf >> dr >> x >> y >> z;
	ifile.getline(line,1000);
	int ind = i; if(b>0) ind = 5-i;
	lad.xx[ind] = x/10.0;        // convert to cm
	lad.yy[ind] = y/10.0;
	lad.zz[ind] = z/10.0;
	//std::cout << lad.zz[ind] << " " << lad.zznom[ind] << std::endl;
	lad.rr[ind] = sqrt(x*x+y*y)/10.0;
	lad.pp[ind] = atan2(-y,-x)+M_PI;
	if(lad.pp[ind]>6. && lad.ppnom<1.) lad.pp[ind] -= 2*M_PI;
      }
    }
  }

}

// fit a barrel from ladders
void FileReader::processGlobal(Ladder** det) 
{
}

      //////////////////////  Plotter   //////////////////
Plotter::Plotter() {

 hfile = new 
   TFile("TranslateISL.root","RECREATE","Translate Ntuple");

 trTree = new TTree("TranslateTree","Translate Tree");
 trTree->Branch("barrel",&(plotLadder.barrel),"barrel/I");
 trTree->Branch("layer", &(plotLadder.layer),"layer/I");
 trTree->Branch("wedge", &(plotLadder.wedge),"wedge/I");
 trTree->Branch("width", &(plotLadder.width),"width/D");

 trTree->Branch("xx",&(plotLadder.xx),"xx[6]/D");
 trTree->Branch("yy",&(plotLadder.yy),"yy[6]/D");
 trTree->Branch("zz",&(plotLadder.zz),"zz[6]/D");
 trTree->Branch("rr",&(plotLadder.rr),"rr[6]/D");
 trTree->Branch("pp",&(plotLadder.pp),"pp[6]/D");

 trTree->Branch("xxnom",&(plotLadder.xxnom),"xxnom/D");
 trTree->Branch("yynom",&(plotLadder.yynom),"yynom/D");
 trTree->Branch("zznom",&(plotLadder.zznom),"zznom[6]/D");
 trTree->Branch("rrnom",&(plotLadder.rrnom),"rrnom/D");
 trTree->Branch("ppnom",&(plotLadder.ppnom),"ppnom/D");

 trTree->Branch("ladTx",&(plotLadder.ladTx),"ladTx/D");
 trTree->Branch("ladTy",&(plotLadder.ladTy),"ladTy/D");
 trTree->Branch("ladTz",&(plotLadder.ladTz),"ladTz/D");
 trTree->Branch("ladAx",&(plotLadder.ladAx),"ladAx/D");
 trTree->Branch("ladAy",&(plotLadder.ladAy),"ladAy/D");
 trTree->Branch("ladAz",&(plotLadder.ladAz),"ladAz/D");

 trTree->Branch("wafTx",&(plotLadder.wafTx),"wafTx[6]/D");
 trTree->Branch("wafTy",&(plotLadder.wafTy),"wafTy[6]/D");
 trTree->Branch("wafTz",&(plotLadder.wafTz),"wafTz[6]/D");
 trTree->Branch("wafAx",&(plotLadder.wafAx),"wafAx[6]/D");
 trTree->Branch("wafAy",&(plotLadder.wafAy),"wafAy[6]/D");
 trTree->Branch("wafAz",&(plotLadder.wafAz),"wafAz[6]/D");

 trTree->Branch("bow",&(plotLadder.bow),"bow/D");

}

void Plotter::makeTuple(Ladder& lad) 
{
  plotLadder = lad;
  std::cout << " filling " << plotLadder.layer << std::endl;
  trTree->Fill();
}

void Plotter::writeTuple() 
{
  hfile->Write();
  hfile->Close(); 
}