#include <G4Polycone.hh>
#include <G4RotationMatrix.hh>
#include <G4DisplacedSolid.hh>
#include <G4Sphere.hh>
#include <G4IntersectionSolid.hh>

#include <RAT/BellyPlate.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*
BellyPlate::Construct( const std::string& name,
                       DBLinkPtr table ) const
{
  const double avRadius = table->GetD( "r_av" ) * mm;
  const double bellyRadius = table->GetD( "r_belly" ) * mm;
  const double halfSideMax = table->GetD( "side_max" ) * mm;
  const double halfSideMin = table->GetD( "side_min" ) * mm;
  const double offset = table->GetD( "offset" ) * mm;
  const bool inner = table->GetI( "inner" ) == 1;

  if( inner )
    {
      const double cutMax = sqrt( avRadius * avRadius - halfSideMax * halfSideMax );
      const double cutMin = sqrt( bellyRadius * bellyRadius - halfSideMin * halfSideMin );
      const double rPolyInner = cutMin - ( cutMax - cutMin ) * halfSideMin / ( halfSideMax - halfSideMin );
      const double halfHeight = halfSideMin * ( avRadius - rPolyInner ) / ( cutMin - rPolyInner );

      return ConstructBellyPlate( name, halfHeight, avRadius, rPolyInner, avRadius, bellyRadius, avRadius, offset );
    }
  else
    {
      const double cutMax = sqrt( bellyRadius * bellyRadius - halfSideMin * halfSideMin );
      const double cutMin = sqrt( avRadius * avRadius - halfSideMax * halfSideMax );
      const double rPolyOuter = cutMax + ( cutMax - cutMin ) * halfSideMin / ( halfSideMax - halfSideMin );

      return ConstructBellyPlate( name, halfSideMax, cutMin, cutMin, rPolyOuter, avRadius, bellyRadius, offset );
    }
}

G4VSolid*
BellyPlate::ConstructBellyPlate( const std::string& name,
                                 const double halfHeight,
                                 const double rPolyNominal,
                                 const double rPolyInner,
                                 const double rPolyOuter,
                                 const double rSphereInner,
                                 const double rSphereOuter,
                                 const double offset ) const
{
  // Logic:
  // The belly plate is an intersection of two polycones and a sphere.
  // The two polycones create the conal edges of the belly plate whilst the sphere creates the face.

  double z[] = { -halfHeight, 0.0, halfHeight};
  double rInner[] = { rPolyNominal, rPolyInner, rPolyNominal };
  double rOuter[] = { rPolyNominal, rPolyOuter, rPolyNominal };

  G4Sphere* sphere = new G4Sphere( name + "_sphere_solid", rSphereInner, rSphereOuter, -pi / 10.0, pi / 5.0, 4.0 * pi / 10.0, pi / 5.0 );
  G4Polycone* polycone = new G4Polycone( name + "_polycone_solid", -pi / 10.0, pi / 5.0, 3, z, rInner, rOuter );
  // The first step is too create two polycones : horizontal and vertical
  G4RotationMatrix* rotation = new G4RotationMatrix();
  rotation->rotateX( pi/2.0 );
  G4IntersectionSolid* polycones = new G4IntersectionSolid( name + "_intersected_polycones_solid", polycone, polycone, rotation, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  // Then to intersect that volume with a sphere
  G4VSolid* bellyPlate = new G4IntersectionSolid( name + "_displaced_belly_plate_solid", sphere, polycones, NULL, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  // Finally move the net solid back to the origin
  return new G4DisplacedSolid( name + "_belly_plate", bellyPlate, NULL, G4ThreeVector( -offset, 0.0, 0.0 ) );
}