#include "hvector.h"
#include "hgeotrap.h"

typedef HVector<float> FVec;

void HGeoTrap::calcShape(HGeoParam & ele, HGeoShapePar & p) {
//  Top and Bottom Faces perpendicular to z-axis and symmetric to y-axis
  float fac=20.F;
  p.noPar=11;
  if (p.volPar.length() != p.noPar )  p.volPar=FVec(0.0F,p.noPar);
  p.volPar[0]=(ele.vol[4][2] - ele.vol[0][2])/fac;
  float alpha=atan((ele.vol[1][1] + ele.vol[0][1] - ele.vol[5][1] - ele.vol[4][1]) /
              fac / 2.F / p.volPar[0]) * p.raddeg;
  if (fabs(alpha)<0.001) {
    p.volPar[1]=0.0F;
    p.volPar[2]=0.0F;
  }
  else {
    if (alpha>0) {
      p.volPar[1]=alpha;
      p.volPar[2]=90.F;
    }
    else {
      if (alpha<0) {
        p.volPar[1]=-alpha;
        p.volPar[2]=270.F;
      }
    }
  }
  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[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;
// 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) {
  if (mo.shape=="PGON") {
    posInPgon(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 HGeoTrap::posInPgon(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;
  ele.geaPos[0]=0.0F;
  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;
  float ctheta=ele.rot[8];
  float stheta=sqrt(1.F-ctheta*ctheta);
  float tad=acos(ctheta)*p.raddeg;
  float phi=acos(ele.rot[0]);
  float phid=acos(ele.rot[0])*p.raddeg;
  if (ele.rot[1]>0) phi=360.F - phi;
#ifdef ILSESHOW
cout << "************************************* phi: " << phid << endl;
#endif
#ifdef WITHGEANT
  GSROTM(++p.geantRotNo,90.,(180.+phid),(90.-tad),(270.+phid),tad,(90.+phid));
#else
  ++p.geantRotNo;
#endif
  p.nRot=p.geantRotNo;
  float dz=(float)ele.trans.norm() + ele.geaPos[2];
  float pos1=(dz*stheta + ele.geaPos[1] * ctheta) / fac;
  p.pos[0]= -pos1 * sin(phi);
  p.pos[1]= pos1 * cos(phi);
  p.pos[2]=(dz*ctheta - ele.geaPos[1] * stheta) / fac;
  return;
}


void HGeoTrap::posInTrap(HGeoParam & ele, HGeoParam & mo, HGeoShapePar & p) {
  float fac=10.F;
  ele.geaPos[0]=0.0F;
  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;
  p.nRot=0;
  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;
}