#include "geantdef.h"
#include "hfloatvec.h"
#include "hgeotrap.h"

typedef HFloatVec FVec;

void HGeoTrap::calcShape(HGeoParam & ele, HGeoShapePar & p) {
//  Top and Bottom Faces perpendicular to z-axis
  float fac=20.F;
  float alpha, beta;
  p.noPar=11;
  if (p.volPar.length() != p.noPar )  p.volPar=FVec(0.0F,p.noPar);
  FVec cb(0.0F,3), ct(0.0F,3), dc(0.0F,3);
  for(int i=0;i<4;i++){cb+=ele.vol[i];}     // bottom plane
  for(int i=4;i<8;i++){ct+=ele.vol[i];}     // top plane
  dc=(ct-cb)/4.F;                           // vector from bottom to top plane
  dc[0]=-dc[0];                             // GEANT coordinate system
  dc[1]=-dc[1];
  p.volPar[0]=fabs(dc[2]) /fac;
  alpha=atan(sqrt(dc[0]*dc[0]+dc[1]*dc[1])/dc[2]) * p.raddeg;
  if (fabs(alpha)<0.0001F) {
    alpha=0.0F;
    beta=0.0F;
  } else {
    if (fabs(dc[0])<0.0001F) {
      if (dc[1]>0) beta=90.0F;
      else beta=270.0F;
    } else {
      beta=atan(dc[1]/dc[0]) * p.raddeg;
      if (dc[0]<0) beta=180.0F + beta;
      if (beta<0) beta=360.0F + beta;
    }
  }
  p.volPar[1]=alpha;
  p.volPar[2]=beta;
  p.volPar[3]=(ele.vol[1][1] - ele.vol[0][1])/fac;
  p.volPar[4]=(ele.vol[1][0] - ele.vol[2][0])/fac;
  p.volPar[5]=(ele.vol[0][0] - ele.vol[3][0])/fac;
  p.volPar[6]=atan((ele.vol[1][0]-ele.vol[0][0]+ele.vol[2][0]-
                    ele.vol[3][0])/40.F/p.volPar[3]) * p.raddeg;
  if (fabs(p.volPar[6])<=0.0001) p.volPar[6]=0.0F;
  p.volPar[7]=(ele.vol[5][1] - ele.vol[4][1])/fac;
  p.volPar[8]=(ele.vol[5][0] - ele.vol[6][0])/fac;
  p.volPar[9]=(ele.vol[4][0] - ele.vol[7][0])/fac;
  p.volPar[10]=atan((ele.vol[5][0]-ele.vol[4][0]+ele.vol[6][0]-
                    ele.vol[7][0])/40.F/p.volPar[7]) * p.raddeg;
  if (fabs(p.volPar[10])<=0.0001) p.volPar[10]=0.0F;
// check if coplanar
  float dx=(p.volPar[4] - p.volPar[5]) / p.volPar[3] * p.volPar[7] -
           (p.volPar[8] - p.volPar[9]);
  if (fabs(dx)>=0.001)
  { cout << "top and bottom plane are not coplanar for shape TRAP\n";
    cout << "lenght in x-direction of top plane is changed\n";
    cout << "old values: " << p.volPar[8] << "  " << p.volPar[9] << "\n";
    p.volPar[8]=p.volPar[8] + dx/2.F;
    p.volPar[9]=p.volPar[9] - dx/2.F;
    cout << "new values: " << p.volPar[8] << "  " << p.volPar[9] << "\n";
  }
  return;
}


int HGeoTrap::posShape(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float dx=0.F;
  for(int i=0;i<8;i++){dx+=ele.vol[i][0];}
  ele.geaPos[0]=dx/8.F;
  ele.geaPos[1]=(ele.vol[1][1] + ele.vol[0][1] + ele.vol[5][1] +
                          ele.vol[4][1] ) /4.F;
  ele.geaPos[2]=(ele.vol[4][2] + ele.vol[0][2] ) /2.F;
  ele.geaRot=cgRot;
  return posInMother(ele,mo,p);
}