#include "Stntuple/obj/TSiAlign.hh"
#include <fstream>
#include "TMath.h"

ClassImp(TSiAlign)
ClassImp(TSiAlign::TSiHalfLadder)

// Default constructor is meant to be used with streamer
TSiAlign::TSiAlign() 
  : fMap()
{
  fMap.SetOwner(true); // Map will take ownership of pointers and take care of delete
}

// If you 
TSiAlign::TSiAlign(const std::string &geoFile) 
  : fMap(TStnSiDigiCode(2,1,35,7,1).fDigiCode/3) // Set size of hash table
{
  fMap.SetOwner(true); // Map will take ownership of pointers and take care of delete

  int b,h,w,l,s;
  int n;
  double zlow, zhi;
  double hx,hy,hz,nhx,nhy,nhz;
  double hpax,hpay,hpaz;
  double hpsx,hpsy,hpsz;
  double wx,wy,wz,nwx,nwy,nwz;
  double wpax,wpay,wpaz;
  double wpsx,wpsy,wpsz;
  std::ifstream f(geoFile.c_str());
  std::string dum;
  std::getline(f,dum,'\n'); // Skip first line with comments
  while (f >> b >> h >> w >> l >> s >> n >> zlow >> zhi) {
    TSiHalfLadder *lad = new TSiHalfLadder();
    lad->n = n;
    lad->zlow = zlow;
    lad->zhi = zhi;

    f >> hx >> hy >> hz; // half ladder center
    f >> nhx >> nhy >> nhz; // half ladder normal
    f >> hpax >> hpay >> hpaz; // half perp axial direction
    f >> hpsx >> hpsy >> hpsz; // half perp stereo direction
    lad->h  = TVector3(hx,hy,hz);
    lad->nh = TVector3(nhx,nhy,nhz);
    lad->pah = TVector3(hpax,hpay,hpaz);
    lad->psh = TVector3(hpsx,hpsy,hpsz);
    
    for (int iw=0; iw<n; iw++) {
      f >> wx >> wy >> wz; // Wafer center
      f >> nwx >> nwy >> nwz; // Wafer normal
      f >> wpax >> wpay >> wpaz; // Wafer perp axial direction
      f >> wpsx >> wpsy >> wpsz; // Wafer perp stereo direction
      lad->w[iw]  = TVector3(wx,wy,wz);
      lad->nw[iw] = TVector3(nwx,nwy,nwz);
      lad->paw[iw] = TVector3(wpax,wpay,wpaz);
      lad->psw[iw] = TVector3(wpsx,wpsy,wpsz);
    }
    TStnSiDigiCode *digi = new TStnSiDigiCode(b,h,w,l,s);
    fMap.Add(digi,lad);
  }
}

TSiAlign::~TSiAlign(){
  // fMap is supposed to delete all its stuff on its own
}

TVector3* TSiAlign::GetCenter(TStnSiDigiCode *digi) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad) {
    return &(lad->h);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetNormal(TStnSiDigiCode *digi) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad) {
    return &(lad->nh);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetPerpAx(TStnSiDigiCode *digi) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad) {
    return &(lad->pah);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetPerpSt(TStnSiDigiCode *digi) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad) {
    return &(lad->psh);
  }
  else {
    return NULL;
  }
}

int TSiAlign::GetNWafer(TStnSiDigiCode *digi) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad) {
    return lad->n;
  }
  else {
    return -1;
  }
}

int TSiAlign::GetWafer(TStnSiDigiCode *digi, double locz) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  int wafer=-1;
  if (lad) {
    if(locz<lad->zlow) wafer=0;
    else if(locz<lad->zhi) wafer=1;
    else wafer=2;
  }
  return wafer;
}

TVector3* TSiAlign::GetWaferCenter(TStnSiDigiCode *digi,int iw) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad && (iw>=0 && iw<2)) {
    return &(lad->w[iw]);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetWaferNormal(TStnSiDigiCode *digi,int iw) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad && (iw>=0 && iw<2)) {
    return &(lad->nw[iw]);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetWaferPerpAx(TStnSiDigiCode *digi,int iw) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad && (iw>=0 && iw<2)) {
    return &(lad->paw[iw]);
  }
  else {
    return NULL;
  }
}

TVector3* TSiAlign::GetWaferPerpSt(TStnSiDigiCode *digi,int iw) {
  TSiHalfLadder *lad = (TSiHalfLadder *)fMap.GetValue(digi);
  if (lad && (iw>=0 && iw<2)) {
    return &(lad->psw[iw]);
  }
  else {
    return NULL;
  }
}