#include "geantdef.h"
#include "hgeobasicshapes.h"
#include "geafuncdec.h"

int HGeoBasicShapes::readPointsDb(HGeoInfo & geoInfo, HGeoParam & ele) {
#ifdef WITHORACLE
  int np=geoInfo.geoDb->getPoints(ele.objIndex,ele.vol);
#ifdef ILSESHOW
  cout << "number of points: " << np << endl;
#endif
  if (np==nCorners) return HSUCCESS;
  cerr<<" number of points read from database doesn't fit shape "<<endl;
#endif
  return HFAILURE;
}


int HGeoBasicShapes::readPoints(HGeoInfo & geoInfo, HGeoParam & ele) {
  FVec x(0.0F,3);
  ele.vol.assign(x,nCorners);
  geoInfo.fin >> ele.vol;
  return HSUCCESS;
}

int HGeoBasicShapes::posInMother(HGeoParam & ele, HGeoParam & mo,
                                 HGeoShapePar & p) {
  float fac=10.F;
  FVec r(0.0F,9), rmt(0.0F,9), rmst(0.0F,9), v(0.0F,3);
  transposeMatrix(mo.geaRot,rmst);
  multiplyMatrices(rmst,ele.rot,r);
  multiplyMatrices(r,ele.geaRot,rmt);
  if (fabs(rmt.max())>1.0F) {
    cerr << "something wrong with the rotation matrices" << endl;
    cerr << "calculated matrix for Geant using GFANG: "<< endl;
    cerr << rmt << endl;
    return HFAILURE;
  }
  if (checkUnity(rmt)) p.nRot=0;
  else {
#ifdef WITHGEANT
    float rowmat[3], theta[3], phi[3], sinth, costh, sinph, cosph;
    int rotate;
    const double deg2rad=0.01745329252;
    for (int i=0;i<3;i++) {
      for (int j=0;j<3;j++) { rowmat[j]=rmt[i+j*3]; }
      GFANG(rowmat,costh,sinth,cosph,sinph,rotate);
      theta[i]=(float)(atan2((sinth*deg2rad),(costh*deg2rad))/deg2rad);
      if (theta[i]<0.0F) theta[i]+=180.0F;
      theta[i]=((int)(theta[i]*1000.F+0.5F))/1000.F;
      phi[i]=(float)(atan2((sinph*deg2rad),(cosph*deg2rad))/deg2rad);
      if (phi[i]<0.0F) phi[i]+=360.0F;
      phi[i]=((int)(phi[i]*1000.F+0.5F))/1000.F;
#ifdef ILSESHOW
      cout << "rmt: " << rmt << endl;
      cout<<"theta["<<i<<"] "<<theta[i]<<endl;
      cout<<"   phi["<<i<<"]   "<<phi[i]<<endl;
#endif
    }
    GSROTM(++p.geantRotNo,theta[0],phi[0],theta[1],phi[1],theta[2],phi[2]);
#else
    ++p.geantRotNo;
#endif
    p.nRot=p.geantRotNo;
  }
  rotateVector(ele.rot,ele.geaPos,v);
  v+=(ele.trans-mo.geaPos);
  if (checkUnity(rmst)) p.pos=v;
  else rotateVector(rmst,v,p.pos);
  p.pos=p.pos/fac;
  return HSUCCESS;
}


int HGeoBasicShapes::writePoints(ofstream & fout, HGeoParam & ele) {
  for (int i=0;i<nCorners;i++) {
    fout<<ele.vol[i][0]<<"  "<<ele.vol[i][1]<<"  "<<ele.vol[i][2]<<'\n';
  }
  return HSUCCESS;
}


bool HGeoBasicShapes::checkUnity(FVec & r) {
  if (r.norm(uniRot)<0.0001) return true;
  return false;
}


void HGeoBasicShapes::transposeMatrix(FVec & r, FVec & tr) {
  for(int i=0;i<3;i++) {
    for(int j=0;j<3;j++) { tr[j+3*i]=r[i+3*j]; }
  }
  return;
}


void HGeoBasicShapes::multiplyMatrices(FVec & lr, FVec & rr, FVec & pr) {
  pr=0.0F;
  for(int i=0;i<3;i++) {
    for(int j=0;j<3;j++) {
      int n=3*i+j;
      for(int k=0;k<3;k++) { pr[n]+=lr[3*i+k]*rr[3*k+j]; }
    }
  }
  return;
}


void HGeoBasicShapes::rotateVector(FVec & r, FVec & v, FVec & rv) {
  rv=0.0F;
  for(int i=0;i<3;i++) {
    for(int j=0;j<3;j++) { rv[i]+=r[3*i+j]*v[j]; }    
  }
  return;
}