#include "hvector.h"
#include "hgeobox.h"

typedef HVector<float> FVec;

void HGeoBox::calcShape(HGeoParam & ele, HGeoShapePar & p) {
  p.noPar=3;
  p.volPar=(ele.vol[5] - ele.vol[3])/20.F;
  return; 
}

int HGeoBox::posShape(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  if ((ele.mother=="CAVE") || (mo.shape=="PGON")) {
    posInLab(ele,mo,p);
    return HSUCCESS;
  }
  if (mo.shape=="BOX ") {
    posInBox(ele,mo,p);
    return HSUCCESS;
  }
  if (mo.shape=="TRAP") {
    posInTrap(ele,mo,p);
    return HSUCCESS;
  }
  cerr << ele.shape << " in " << mo.shape << " is not yet implemeted" << endl;
  return HFAILURE;
}


void HGeoBox::posInLab(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;
  float ctheta=ele.rot[8];
  float stheta=sqrt(1.F - ctheta*ctheta);
  float tad=acos(ctheta) * p.raddeg;
#ifdef WITHGEANT
  GSROTM(++p.geantRotNo,90.,0.,(90.+tad),90.,tad,90.);
#endif
  p.nRot=p.geantRotNo;
  float dz=(ele.vol[3][2] + ele.vol[5][2])/2.F + (float)ele.trans.norm();
  float dy=(ele.vol[3][1] + ele.vol[5][1])/2.F;
  ele.geaPos=ele.vol[5] - (fac * p.volPar);
  p.pos[0]=0.0F;
  p.pos[1]=(dz * stheta + dy * ctheta) / fac;
  p.pos[2]=(dz * ctheta - dy * stheta) / fac;
  return;
}


void HGeoBox::posInBox(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;  
  p.nRot=0;
  ele.geaPos=ele.vol[5] - fac * p.volPar;
  p.pos=(ele.trans - mo.geaPos + ele.geaPos) / fac;
  return;
}

void HGeoBox::posInTrap(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;
#ifdef WITHGEANT
  GSROTM(++p.geantRotNo,90.,180.,90.,270.,0.,0.);
#endif
  p.nRot=p.geantRotNo;
  ele.geaPos=ele.vol[5] - fac * p.volPar;
  FVec x= (ele.trans - mo.geaPos + ele.geaPos) / fac; 
  p.pos[0]=-x[0];
  p.pos[1]=-x[1];
  p.pos[2]=x[2];
  return;
}