#include "hvector.h"
#include "hgeotrd1.h"

typedef HVector<float> FVec;

void HGeoTrd1::calcShape(HGeoParam & ele, HGeoShapePar & p) {
  float fac=20.F;
  p.noPar=4;
  if (p.volPar.length() != p.noPar ) p.volPar=FVec(0.0F,p.noPar);
  p.volPar[0]=(ele.vol[0][0] - ele.vol[3][0])/fac;
  p.volPar[1]=(ele.vol[1][0] - ele.vol[2][0])/fac;
  p.volPar[2]=(ele.vol[4][2] - ele.vol[0][2])/fac;
  p.volPar[3]=(ele.vol[1][1] - ele.vol[0][1])/fac;
  return;
}


int HGeoTrd1::posShape(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  if (mo.shape=="TRAP") {
    posInTrap(ele,mo,p);
    return HSUCCESS;
  }
  if (mo.shape=="TRD1") {
    posInTrd1(ele,mo,p);
    return HSUCCESS;
  }
  cerr << ele.shape << " in " << mo.shape << " is not yet implemeted" << endl;
  return HFAILURE;
}


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


void HGeoTrd1::posInTrd1(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;
  ele.geaPos[0]=0.0F;
  ele.geaPos[1]=ele.vol[1][1] - fac * p.volPar[3];
  ele.geaPos[2]=ele.vol[4][2] - fac * p.volPar[2];
  p.nRot=0;
  p.pos[0]=0.0F;
  p.pos[1]=-(ele.trans[2] - mo.geaPos[2] + ele.geaPos[2]) /fac;
  p.pos[2]=(ele.trans[1] - mo.geaPos[1] + ele.geaPos[1]) /fac;
  return;
}