#include <G4Tubs.hh>
#include <G4Torus.hh>
#include <G4UnionSolid.hh>

#include <RAT/HoldDownRope.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*
HoldDownRope::Construct( const std::string& name,
                         DBLinkPtr table ) const
{
  const vector<double> xPositions = table->GetDArray( "rope_x" );
  const vector<double> yPositions = table->GetDArray( "rope_y" );
  const vector<double> zPositions = table->GetDArray( "rope_z" );
  Log::Assert( xPositions.size() == 6 && yPositions.size() == 6 && zPositions.size() == 6, "HoldDownRope::Construct: Insufficient rope positions are defined." );
  const double ropeRadius = table->GetD( "r_rope" ) * mm;
  const double avRadius = table->GetD( "r_av" ) * mm;
  const vector<double> slingPosition = table->GetDArray( "sling_position" );
  Log::Assert( slingPosition.size() == 3, "HoldDownRope::Construct: Sling position is incorrectly defined." );
  return ConstructHoldDownRope( name,
                                G4ThreeVector( xPositions[0] * mm, yPositions[0] * mm, zPositions[0] * mm ),
                                G4ThreeVector( xPositions[1] * mm, yPositions[1] * mm, zPositions[1] * mm ),
                                G4ThreeVector( xPositions[2] * mm, yPositions[2] * mm, zPositions[2] * mm ),
                                G4ThreeVector( xPositions[3] * mm, yPositions[3] * mm, zPositions[3] * mm ),
                                G4ThreeVector( xPositions[4] * mm, yPositions[4] * mm, zPositions[4] * mm ),
                                G4ThreeVector( xPositions[5] * mm, yPositions[5] * mm, zPositions[5] * mm ),
                                G4ThreeVector( slingPosition[0] * mm, slingPosition[1] * mm, slingPosition[2] * mm ),
                                ropeRadius,
                                avRadius );
}

G4VSolid*
HoldDownRope::ConstructHoldDownRope( const std::string& name,
                                     const G4ThreeVector base1,
                                     const G4ThreeVector equator1,
                                     const G4ThreeVector knot1,
                                     const G4ThreeVector knot2,
                                     const G4ThreeVector equator2,
                                     const G4ThreeVector base2,
                                     const G4ThreeVector sling,
                                     const double ropeRadius,
                                     const double avRadius ) const
{
  /// Logic summary:
  /// Build each segment separately and then rotate into position before unioning together.
  /// Shapes are defined as a torus
  const double arc1 = equator1.angle( knot1 ); // The angle between the equator and knot points
  G4Torus* equatorKnot = new G4Torus( name + "_equator_knot_solid", 0.0, ropeRadius, ropeRadius + avRadius, pi / 2.0 - arc1, arc1 );

  const double arc2 = knot1.angle( knot2 ); // The angle between the two knot points
  G4Torus* knotKnot = new G4Torus( name + "_knot_knot_solid", 0.0, ropeRadius, ropeRadius + avRadius, pi / 2.0 - arc2 / 2.0, arc2 );

  // Now rotate and position the top (knot1 to knot2)
  G4RotationMatrix* rotateKnotKnot = new G4RotationMatrix();// Rotates around Y axe only
  rotateKnotKnot->rotateZ( pi / 2.0 - knot1.cross( knot2 ).phi() );
  rotateKnotKnot->rotateY( pi );
  rotateKnotKnot->rotateX( pi - knot1.cross( knot2 ).theta() );
  G4DisplacedSolid* displacedKnotKnot = new G4DisplacedSolid( name + "_displaced_knot_knot_solid", knotKnot, rotateKnotKnot, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  // Now add the side curved part (equator1 to knot2)
  const G4ThreeVector equatorKnotNormal = equator1.cross( knot1 );
  G4RotationMatrix* rotateEquatorKnot = new G4RotationMatrix();// Rotates around Y axe only
  rotateEquatorKnot->rotateZ( pi + equatorKnotNormal.phi() );
  rotateEquatorKnot->rotateY( equatorKnotNormal.theta() );
  G4UnionSolid* topSide = new G4UnionSolid( name + "_top_side1_solid", displacedKnotKnot, equatorKnot, rotateEquatorKnot, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  // Now add the other side curved part (knot2 to equator2)
  const G4ThreeVector knotEquatorNormal = equator2.cross( knot2 );
  G4RotationMatrix* rotateKnotEquator = new G4RotationMatrix();// Rotates around Y axe only
  rotateKnotEquator->rotateZ( -knotEquatorNormal.phi() );
  rotateKnotEquator->rotateY( knotEquatorNormal.theta() );
  G4UnionSolid* topSides = new G4UnionSolid( name + "_top_sides_solid", topSide, equatorKnot, rotateKnotEquator, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  // Now add the side straight part (base1 to equator1)
  const double length = (equator1 - base1).mag(); // The length of the rope from the base to the equator
  G4Tubs* baseEquator = new G4Tubs( name + "_base_equator_solid", 0.0, ropeRadius, length / 2.0, 0.0, twopi );

  const G4ThreeVector pos1 = ( base1 + equator1 ) / 2.0;
  const G4ThreeVector dir1 = ( equator1 - base1 ).unit();
  G4RotationMatrix* rotateBaseEquator1 = new G4RotationMatrix();// Rotates around Y axe only
  rotateBaseEquator1->rotateZ( dir1.phi() );
  rotateBaseEquator1->rotateY( dir1.theta() );
  G4UnionSolid* topSidesBase = new G4UnionSolid( name + "_top_sides_base1_solid", topSides, baseEquator, rotateBaseEquator1, pos1 );

  const G4ThreeVector pos2 = ( base2 + equator2 ) / 2.0;
  const G4ThreeVector dir2 = ( equator2 - base2 ).unit();
  G4RotationMatrix* rotateBaseEquator2 = new G4RotationMatrix();// Rotates around Y axe only
  rotateBaseEquator2->rotateZ( dir2.phi() );
  rotateBaseEquator2->rotateY( dir2.theta() );
  G4UnionSolid* topSidesBases = new G4UnionSolid( name + "_top_sides_bases_solid", topSidesBase, baseEquator, rotateBaseEquator2, pos2 );

  const double slingArc = knot1.angle( sling );
  G4Torus* slingSolid = new G4Torus( name + "_sling_solid", 0.0, ropeRadius, ropeRadius + avRadius, -slingArc / 2.0, slingArc );

  const G4ThreeVector knotSlingNormal = knot1.cross( sling );
  G4RotationMatrix* rotateKnotSling = new G4RotationMatrix();
  rotateKnotSling->rotateZ( pi - knotSlingNormal.phi() );
  rotateKnotSling->rotateY( knotSlingNormal.theta() );
  G4UnionSolid* holdDownRope = new G4UnionSolid( name + "_solid", topSidesBases, slingSolid, rotateKnotSling, G4ThreeVector( 0.0, 0.0, 0.0 ) );

  return holdDownRope;
}