#ifndef ClassQQQ_h
#define ClassQQQ_h

#include <cstdio>
#include <TMath.h>
#include <TVector3.h>
#include <TRandom.h>
#include "TGeoManager.h"
#include "TGeoVolume.h"
#include "TGeoBBox.h"

class QQQ{
public:
  QQQ(){Clear();};
  ~QQQ(){}
  
  short GetID() const {return id;}
  short GetChUp() const {return chUp;}
  short GetChDn() const {return chDn;}
  short GetChBk() const {return chBk;}

  TVector3 GetHitPos() const {return hitPos;}
  TVector3 GetHitPosWithSigma(double sigmaY_mm, double sigmaZ_mm);

  double GetZFrac() const {return zFrac;} // range from -0.5 to 0.5

  void Clear();
  void ConstructGeo();
  void FindQQQPos(TVector3 pos, TVector3 direction, bool verbose = false);
  void CalQQQPos(unsigned short ID, unsigned short chUp, unsigned short chDown, unsigned short chBack, float eUp, float eDown);

  double GetNumDet() const {return numDet;}

  void Print(){ 
    if( id == -1 ){
      printf("Did not hit any QQQ.\n"); 
    }else{
      printf("ID: %d, U,D,B: %d %d %d| zFrac : %.2f\n", id, chUp, chDn, chBk, zFrac); 
      printf("Hit Pos: %.2f, %.2f, %.2f\n", hitPos.X(), hitPos.Y(), hitPos.Z()); 
    }
  }

  // void CalZFrac(){
  //   zFrac = (eUp - eDn)/(eUp + eDn); 
  // }

private:

  const int numDet = 4;
  const float qqqR1 = 50;
  const float qqqR2 = 100;
  const float qqqZPos = 23 + 75 + 30;

  short id; // -1 when no hit
  short chUp;
  short chDn;
  short chBk;

  double zFrac; // from +1 (downstream) to -1 (upstream)

  double eUp;
  double eDn;
  double eBk;

  TVector3 hitPos;

  TGeoManager *geom;
  TGeoVolume *worldBox;
  TGeoMedium *Al;

  // helper function to calculate intersection between line segments, return pair of (fraction along line1, fraction along line2) where the intersection occurs. If no intersection, return (0, -1).
  std::pair<double, double> Intersect(TVector3 p1, TVector3 p2, TVector3 q1, TVector3 q2, bool verbose){ 

    //see https://nukephysik101.wordpress.com/2023/12/30/intersect-between-2-line-segments/
    //zero all z-component
    TVector3 a0 = p1; a0.SetZ(0);
    TVector3 a1 = p2; a1.SetZ(0);

    TVector3 b0 = q1; b0.SetZ(0);
    TVector3 b1 = q2; b1.SetZ(0);

    double h = 0, k = 0; // placeholder values, implementation of intersection logic
    if( verbose ) printf(" ----h, k : %f, %f\n", h, k);

    return std::pair<double,double>(h,k);
  }

};

inline void QQQ::Clear(){
  id = -1;
  chUp = -1;
  chDn = -1;
  chBk = -1;
  zFrac = TMath::QuietNaN();

  eUp = TMath::QuietNaN();
  eDn = TMath::QuietNaN();
  eBk = TMath::QuietNaN();
}

inline void QQQ::ConstructGeo(){
  TGeoVolume *qqq = geom->MakeTubs("qqq", Al, qqqR1, qqqR2, 0.5, 5, 85);
  qqq->SetLineColor(7);
  for( int i = 0; i < 4; i++){
    worldBox->AddNode(qqq, i+1, new TGeoCombiTrans( 0, 
                                                    0, 
                                                    qqqZPos, 
                                                    new TGeoRotation("rot1", 360/4 * (i), 0., 0.)));
  }
}


inline void QQQ::FindQQQPos(TVector3 pos,
                            TVector3 direction,
                            bool verbose){

  id   = -1;
  chUp = -1;
  chDn = -1;
  chBk = -1;

  //--------------------------------------------
  // Intersect trajectory with QQQ plane
  //--------------------------------------------

  if( TMath::Abs(direction.Z()) < 1e-10 ) return;

  double t = (qqqZPos - pos.Z()) / direction.Z();

  if( t <= 0 ) return;

  hitPos = pos + t * direction;

  //--------------------------------------------
  // Cylindrical coordinates
  //--------------------------------------------

  double x = hitPos.X();
  double y = hitPos.Y();

  double r = TMath::Sqrt(x*x + y*y);

  if( r < qqqR1 || r > qqqR2 ) return;

  double phi = hitPos.Phi() * TMath::RadToDeg();

  if( phi < 0 ) phi += 360.0;

  //--------------------------------------------
  // Determine detector ID
  //--------------------------------------------

  id = -1;

  for(int det = 0; det < 4; det++){

    double phiMin = det*90.0 + 5.0;
    double phiMax = phiMin + 85.0;

    if( phi >= phiMin && phi <= phiMax ){
      id = det;
      break;
    }
  }

  if( id < 0 ) return;

  //--------------------------------------------
  // Ring number (32 strips)
  //--------------------------------------------

  const double ringWidth =
      (qqqR2 - qqqR1)/32.0;

  int ring =
      (int)((r - qqqR1)/ringWidth);

  if( ring < 0 ) ring = 0;
  if( ring > 31 ) ring = 31;

  //--------------------------------------------
  // Sector number (4 strips)
  //--------------------------------------------

  double localPhi =
      phi - (id*90.0 + 5.0);

  int sector =
      (int)(localPhi/(85.0/4.0));

  if( sector < 0 ) sector = 0;
  if( sector > 3 ) sector = 3;

  chBk = ring;
  chDn = sector;
  chUp = sector;

  zFrac = 0.0;

  if(verbose){

    printf("\nQQQ Hit\n");
    printf(" ID     = %d\n", id);
    printf(" Ring   = %d\n", ring);
    printf(" Sector = %d\n", sector);
    printf(" r      = %.2f mm\n", r);
    printf(" phi    = %.2f deg\n", phi);

    hitPos.Print();
  }
}
/*s
inline TVector3 QQQ::GetHitPosWithSigma(double sigmaY_mm, double sigmaZ_mm){

  double phi = SNorml[id%numDet].Phi();

  TVector3 haha = hitPos;
  haha.RotateZ(-phi);

  double y = haha.Y() + gRandom->Gaus(0, sigmaY_mm);
  if( sigmaY_mm < 0 ){
    double deltaW = width/4;
    y = TMath::Floor((haha.Y()-deltaW)/deltaW)*deltaW + deltaW*1.5; // when ever land on each strip, set the position to be center of the strip.
    if( y >= 25 ) y = 15;
  }

  double z = haha.Z() + gRandom->Gaus(0, sigmaZ_mm);
  if( sigmaZ_mm < 0 ){
    haha.Z();
    double delta = length/4;
    int sign = z > 0 ? 1 : -1;
    z = TMath::Floor( (abs(z)-gap/2)/delta )*delta + 0.5 * delta + gap/2;
    if( z >= 107.375 ) z = 88.625;
    z = sign * z;
  }

  haha.SetY(y);
  haha.SetZ(z);
  haha.RotateZ(phi);

  return haha;

}*/


inline void QQQ::CalQQQPos(unsigned short ID,
                           unsigned short chUp,
                           unsigned short chDown,
                           unsigned short chBack,
                           float eUp,
                           float eDown){

  hitPos.Clear();

  if( ID > 3 ) return;
  if( chBack > 31 ) return;
  if( chDown > 3 ) return;

  const double ringWidth =
      (qqqR2 - qqqR1)/32.0;

  double r =
      qqqR1 + (chBack + 0.5)*ringWidth;

  const double sectorWidth =
      85.0/4.0;

  double phiDeg =
      ID*90.0 + 5.0 +
      (chDown + 0.5)*sectorWidth;

  double phi =
      phiDeg * TMath::DegToRad();

  hitPos.SetXYZ(
      r*TMath::Cos(phi),
      r*TMath::Sin(phi),
      qqqZPos
  );

  id   = ID;
  chBk = chBack;
  chDn = chDown;
  chUp = chUp;
}

#endif