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

#include <RAT/InternalRope.hh>
#include <RAT/Log.hh>
#include <RAT/GeoFactory.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*
InternalRope::Construct( const std::string& name,
                         DBLinkPtr table ) const
{
  const vector<double> xPositions = table->GetDArray( "x" );
  const vector<double> yPositions = table->GetDArray( "y" );
  const vector<double> zPositions = table->GetDArray( "z" );
  Log::Assert( xPositions.size() == 6 && yPositions.size() == 6 && zPositions.size() == 6, "InternalRope::Construct: Insufficient rope positions are defined." );
  const double ropeRadius = table->GetD( "r_max" ) * mm;

  G4VSolid* rope1 = ConstructRope( name + "1",
                                   G4ThreeVector( xPositions[0] * mm, yPositions[0] * mm, zPositions[0] * mm ),
                                   G4ThreeVector( xPositions[1] * mm, yPositions[1] * mm, zPositions[1] * mm ),
                                   ropeRadius );
  G4VSolid* rope2 = ConstructRope( name + "2",
                                   G4ThreeVector( xPositions[1] * mm, yPositions[1] * mm, zPositions[1] * mm ),
                                   G4ThreeVector( xPositions[2] * mm, yPositions[2] * mm, zPositions[2] * mm ),
                                   ropeRadius );
  G4VSolid* rope3 = ConstructRope( name + "3",
                                   G4ThreeVector( xPositions[2] * mm, yPositions[2] * mm, zPositions[2] * mm ),
                                   G4ThreeVector( xPositions[3] * mm, yPositions[3] * mm, zPositions[3] * mm ),
                                   ropeRadius );
  G4VSolid* rope4 = ConstructRope( name + "4",
                                   G4ThreeVector( xPositions[4] * mm, yPositions[4] * mm, zPositions[4] * mm ),
                                   G4ThreeVector( xPositions[3] * mm, yPositions[3] * mm, zPositions[3] * mm ),
                                   ropeRadius );
  G4VSolid* rope5 = ConstructRope( name + "5",
                                   G4ThreeVector( xPositions[5] * mm, yPositions[5] * mm, zPositions[5] * mm ),
                                   G4ThreeVector( xPositions[4] * mm, yPositions[4] * mm, zPositions[4] * mm ),
                                   ropeRadius );
  G4VSolid* rope = new G4UnionSolid( name + "_12_solid", rope1, rope2, NULL, G4ThreeVector( 0.0, 0.0, 0.0 ) );
  rope = new G4UnionSolid( name + "_123_solid", rope, rope3, NULL, G4ThreeVector( 0.0, 0.0, 0.0 ) );
  rope = new G4UnionSolid( name + "_1234_solid", rope, rope4, NULL, G4ThreeVector( 0.0, 0.0, 0.0 ) );
  rope = new G4UnionSolid( name + "_solid", rope, rope5, NULL, G4ThreeVector( 0.0, 0.0, 0.0 ) );
  return rope;
}

G4VSolid*
InternalRope::ConstructRope( const std::string& name,
                             const G4ThreeVector start,
                             const G4ThreeVector end,
                             const double radius ) const
{
  const G4ThreeVector direction = end - start;
  const double length = direction.mag();
  G4Tubs* rope = new G4Tubs( name + "_solid", 0.0, radius, length / 2.0, 0.0, twopi );
  G4ThreeVector mid = ( start + end ) / 2.0;
  G4RotationMatrix* rotation = GeoFactory::DirectionToRotation( direction );
  return new G4DisplacedSolid( name + "_displaced_solid", rope, rotation, mid );
}