#include <TGeoManager.h>

#include <TCanvas.h>
#include <TGeoTrack.h>
#include <TList.h>
#include <TGeoTrack.h>
#include <TGeoManager.h>
#include <TGeoTube.h>
#include <TGeoCompositeShape.h>

#include <iostream>
#include <fstream>

// -- Bore shapes

TGeoCtub *bore_straight;
TGeoCtub *bore_bend;

TGeoCtub *in_bore_straight;
TGeoCtub *in_bore_bend;

TGeoCompositeShape *bore_cs;
TGeoCompositeShape *in_bore_cs;

// -- volumes

TGeoVolume *universe;
TGeoVolume *in_bore;
TGeoVolume *bore;

// -- materials and media

TGeoMaterial *vac_mat;
TGeoMaterial *trans_mat;
TGeoMaterial *solid_mat;
 
TGeoMedium *vac_med;
TGeoMedium *solid_med;

// -- World Geometry

Double_t WorldX = 60.0;   // size of world in X
Double_t WorldY = 60.0;   // size of world in Y
Double_t WorldZ = 200.0;

// -- bend angle

const Double_t angle_deg = 9.5;
Double_t angle = TMath::DegToRad()*angle_deg;  // bend angle in radians

// -- Bore Dimensions

const Double_t Oxford_offset = 3.0;        // z-offset between Oxford coordinates and end-of-bore coordinates

const Double_t Bore_straight_l = 45.5;
const Double_t Bore_r = 5.85;              // bore radius
const Double_t Bore_thick = 0.5;           // bore wall thickness

const Double_t Bore_bend_l = 26.3;

// -- Bore Positions

Double_t Bore_straight_x = 0.0;                                      // placement of bore straight part in UNIVERSE.  
Double_t Bore_straight_y = 0.0;                     
Double_t Bore_straight_z = -Bore_straight_l/2 + Oxford_offset;  

Double_t Bore_bend_x = -sin(angle)*Bore_bend_l/2;                     // placement of bore bend part  in UNIVERSE.  
Double_t Bore_bend_y = 0.0;                     
Double_t Bore_bend_z = -cos(angle)*Bore_bend_l/2 - Bore_straight_l + Oxford_offset;

// -- rotations, translations, etc.

TGeoRotation *r1;
TGeoTranslation *t1;
TGeoCombiTrans *t2;

// -- positions

Double_t pos[3];
Double_t dir[3];

Double_t *p, *p1;
Double_t *d, *d1;


TPolyLine3D *track;

void test(void)
{
	// -- materials and media definitions (not really used for physics, but just stand-ins)
		
	vac_mat = new TGeoMaterial("Vacuum", 0, 0, 0);
	vac_mat->SetTransparency(100);
	vac_med = new TGeoMedium("Vacuum", 1, vac_mat);
	
	solid_mat = new TGeoMaterial("Lead", gGeoManager->GetElementTable()->FindElement("lead"), 11.34);
	solid_mat->SetTransparency(0);  // solid material
	solid_med = new TGeoMedium("Lead", 1, solid_mat);

	// -- define the universe box
	
	universe = gGeoManager->MakeBox("UNIVERSE", vac_med, WorldX/2, WorldY/2, WorldZ/2);
	gGeoManager->SetTopVolume(universe);
	
	// -- define the magnet bore (the metal parts)
	
	bore_straight = new TGeoCtub("bore_straight", Bore_r, Bore_r + Bore_thick, Bore_straight_l/2, 0.0, 360.0, sin(-angle/2), 0.0, -cos(-angle/2), 0.0, 0.0, 1.0);
	bore_bend = new TGeoCtub("bore_bend", Bore_r, Bore_r + Bore_thick, Bore_bend_l/2, 0.0, 360.0, 0.0, 0.0, -1.0, sin(-angle/2), 0.0, cos(-angle/2));

	// -- define the in_bore shapes (the vacuum inside the bore)
		
	in_bore_straight = new TGeoCtub("in_bore_straight", 0, Bore_r, Bore_straight_l/2, 0.0, 360.0, sin(-angle/2), 0.0, -cos(-angle/2), 0.0, 0.0, 1.0);
	in_bore_bend = new TGeoCtub("in_bore_bend", 0, Bore_r, Bore_bend_l/2, 0.0, 360.0, 0.0, 0.0, -1.0, sin(-angle/2), 0.0, cos(-angle/2));

	// -- figure out all the rotations and translations we need

	r1 = new TGeoRotation();
	r1->SetName("R1");
	r1->RegisterYourself();
	
	r1->RotateX(0.0);
	r1->RotateY(angle_deg);  // rotation about Y axis in degrees
	r1->RotateZ(0.0);

	t1 = new TGeoTranslation("T1", Bore_straight_x, Bore_straight_y, Bore_straight_z);
	t1->RegisterYourself();

	t2 = new TGeoCombiTrans("T2", Bore_bend_x, Bore_bend_y, Bore_bend_z, r1);
	t2->RegisterYourself();

	//  -- create the composite shapes
	
	in_bore_cs = new TGeoCompositeShape("IN_BORE", "in_bore_straight:T1 + in_bore_bend:T2");
	bore_cs = new TGeoCompositeShape("BORE", "bore_straight:T1 + bore_bend:T2");
	
	// -- make the composite shapes the bore and in_bore volumes
	
	in_bore = new TGeoVolume("in_bore", in_bore_cs, vac_med);
	bore = new TGeoVolume("bore", bore_cs, solid_med);

	// -- make them pretty 
	
	bore->SetLineColor(kGreen);
	bore->SetLineWidth(1);
	bore->SetVisibility(kTRUE);
	
	in_bore->SetLineColor(kBlue);
	in_bore->SetLineWidth(1);
	in_bore->SetVisibility(kFALSE);

	// -- add bore and in_bore to the univese
	
	universe->AddNode(in_bore, 1);
	universe->AddNode(bore, 1);
		
	// -- close the geometry and show it
	
	gGeoManager->CloseGeometry();
	gGeoManager->SetVisOption(0);
	gGeoManager->GetMasterVolume()->Draw("gl");
	
	// -- initialize the track  
	
	pos[0] = -4.37253577032883756e-01;
	pos[1] = +1.24089168848469072e-01;
	pos[2] = -4.30538085697218733e+01;
	
	dir[0] = +8.12776496934288217e-01;
	dir[1] = +4.66238713170886110e-01;
	dir[2] = +3.49307641445164019e-01;
	
	track = new TPolyLine3D();
	track->SetPoint(0, pos[0], pos[1], pos[2]);
	
	cout << "old p[0] = " << pos[0] << endl;
	cout << "old p[1] = " << pos[1] << endl;
	cout << "old p[2] = " << pos[2] << endl;
	cout << "old d[0] = " << dir[0] << endl;
	cout << "old d[1] = " << dir[1] << endl;
	cout << "old d[2] = " << dir[2] << endl;
		
	TGeoNode *node = gGeoManager->InitTrack(pos, dir);
	cout << "node name = " << node->GetName() << endl;
	
	TGeoNode *snode = gGeoManager->FindNextBoundaryAndStep();
	p = gGeoManager->GetCurrentPoint();
	d = gGeoManager->GetCurrentDirection();
	cout << "snode name = " << snode->GetName() << endl;  // <==== this says UNIVERSE_1 when it should still be in_bore_1
	
	cout << "new p[0] = " << p[0] << endl;
	cout << "new p[1] = " << p[1] << endl;
	cout << "new p[2] = " << p[2] << endl;
	cout << "new d[0] = " << d[0] << endl;
	cout << "new d[1] = " << d[1] << endl;
	cout << "new d[2] = " << d[2] << endl;
	
	track->SetPoint(1, p[0], p[1], p[2]);
	
	node = gGeoManager->InitTrack(p, d);  // re-initialize after the step
	cout << "re-initialized name = " << node->GetName() << endl;  // <<==== why does this tell me that i'm back inside the bore?
	
	p = gGeoManager->GetCurrentPoint();
	d = gGeoManager->GetCurrentDirection();
		
	cout << "p[0] = " << p[0] << endl;
	cout << "p[1] = " << p[1] << endl;
	cout << "p[2] = " << p[2] << endl;
	cout << "d[0] = " << d[0] << endl;
	cout << "d[1] = " << d[1] << endl;
	cout << "d[2] = " << d[2] << endl;
	
	track->Draw();
	
	
}