#include <G4Box.hh>
#include <G4Tubs.hh>
#include <G4Torus.hh>
#include <G4EllipticalTube.hh>
#include <G4RotationMatrix.hh>
#include <G4DisplacedSolid.hh>
#include <G4UnionSolid.hh>

#include <RAT/BellyGroove.hh>
#include <RAT/Log.hh>
using namespace RAT;
using namespace RAT::geo;

#include <CLHEP/Units/SystemOfUnits.h>
#include <CLHEP/Units/PhysicalConstants.h>
#include <string>
using namespace CLHEP;
using namespace std;

G4VSolid*
BellyGroove::Construct( const std::string& name,
                        DBLinkPtr table ) const
{
  const double rGrooveX = table->GetD( "r_groove_x" ) * mm;
  const double rGrooveY = table->GetD( "r_groove_y" ) * mm;
  const double rTorus = table->GetD( "r_torus" ) * mm;
  const double grooveHeight =table->GetD( "z_groove" ) * mm;
  const double openingX = table->GetD( "x_opening" ) * mm;
  const double openingY = table->GetD( "y_opening" ) * mm;
  const double rGroove = table->GetD( "r_groove" ) * mm;
  const double rSphere = table->GetD( "r_sphere" ) * mm;
  return ConstructBellyGroove( name, rGrooveX, rGrooveY, rTorus, grooveHeight, openingX, openingY, rGroove - rSphere );
}

G4VSolid*
BellyGroove::ConstructBellyGroove( const std::string& name,
                                   const double rGrooveX,
                                   const double rGrooveY,
                                   const double rTorus,
                                   const double grooveHeight,
                                   const double openingX,
                                   const double openingY,
                                   const double offset ) const
{
  // Logic:
  // The belly groove can be considered as two U shapes with one on top of the other.
  // The larger U sits well within the belly plate and has an elliptical cross section (in SNOMAN).
  // The drawings describe it more like a rectangle with two half circular edges, however this proves too difficult to represent in geant4.
  // Therefore an elliptical cross section is used where possible, although geant4 cannot build elliptical torii so a box-torus is used.
  // The smaller U sites nearer the belly plate surface than the larger U and breaks both the surface and larger U.
  // This U has a box cross section.
  // The offset is used to correctly place the groove in the belly plate.

  // Create the bottom solids

  // This would be the ideal shape if Geant4 had it. - PGJ
  //G4EllipticalTorus* grooveBottom = new G4Torus( name + "_groove_bottom_solid",
  //                                               0.0, rGrooveX,
  //                                               0.0, rGrooveY,
  //                                               rTorus,
  //                                              -pi / 2.0, pi );
  // Alternative Box-Torus instead
  G4Tubs* grooveBottom = new G4Tubs( name + "_groove_bottom_solid",
                                     rTorus - rGrooveY,
                                     rTorus + rGrooveY,
                                     rGrooveX,
                                     -pi / 2.0, pi );
  // Subtract the bottom solids
  G4RotationMatrix* bottomRotation = new G4RotationMatrix();
  bottomRotation->rotateY( -pi / 2.0 );

  G4VSolid* bellyGroove = new G4DisplacedSolid( name + "_groove_bottom_displaced_solid",
                                                grooveBottom,
                                                bottomRotation, G4ThreeVector( offset, 0.0, 0.0 ) );
  G4Tubs* openingBottom = new G4Tubs( name + "_opening_bottom_solid",
                                      rTorus - openingY / 2.0,
                                      rTorus + openingY / 2.0,
                                      openingX / 2.0,
                                      -pi / 2.0, pi );
  bellyGroove = new G4UnionSolid( name + "__bottom_solid",
                                       bellyGroove,
                                       openingBottom,
                                       bottomRotation, G4ThreeVector( offset + rGrooveY, 0.0, 0.0 ) );
  // Create the top solids
  G4EllipticalTube* grooveTop = new G4EllipticalTube( name + "_groove_top_solid", rGrooveX, rGrooveY, grooveHeight / 2.0 );
  G4Box* openingTop = new G4Box( name + "_opening_top_solid", openingX / 2.0, openingY / 2.0, grooveHeight / 2.0 );
  // Subtract the top solids
  bellyGroove = new G4UnionSolid( name + "_top_left_groove_solid",
                                  bellyGroove,
                                  grooveTop,
                                  NULL, G4ThreeVector( offset, -rTorus, grooveHeight / 2.0 - 1.0 * mm ) );
  bellyGroove = new G4UnionSolid( name + "_top_right_groove_solid",
                                  bellyGroove,
                                  grooveTop,
                                  NULL, G4ThreeVector( offset, rTorus, grooveHeight / 2.0 - 1.0 * mm ) );
  bellyGroove = new G4UnionSolid( name + "_top_left_opening_solid",
                                  bellyGroove,
                                  openingTop,
                                  NULL, G4ThreeVector( offset + rGrooveY, -rTorus, grooveHeight / 2.0 - 1.0 * mm ) );
  bellyGroove = new G4UnionSolid( name + "_top_right_opening_solid",
                                  bellyGroove,
                                  openingTop,
                                  NULL, G4ThreeVector( offset + rGrooveY, rTorus, grooveHeight / 2.0 - 1.0 * mm ) );
  return bellyGroove;
}