#include <cometEventLoop.hxx>
#include <ICOMETLog.hxx>
#include <IOADatabase.hxx>
#include <sstream>
#include <TGeoManager.h>
#include <TVector3.h>
#include <TRotation.h>
#include "IGeometryLoopFunction.hxx"


class PrintPlacement:public COMET::IGeomVisitor{
    public:
    PrintPlacement():COMET::IGeomVisitor(){}
    ~PrintPlacement(){}

    virtual bool VisitNode(
            int depth,const std::string& name,const TGeoNode* node){
        double local[3] = {0,0,0};
        double master[3];
        // Get Origin
        node->LocalToMaster(local,master);
        TVector3 position(master[0], master[1], master[2]);

        // Get X axis
        double x_dir[3] = {1,0,0};
        node->LocalToMasterVect(x_dir,master);
        TVector3 x_axis(master[0], master[1], master[2]);
        //COMETError(" x_axis: ("<<x_axis.x()<<", "<<x_axis.y()<<", "<<x_axis.z()<<")");

        // Get Y axis
        double y_dir[3] = {0,1,0};
        node->LocalToMasterVect(y_dir,master);
        TVector3 y_axis(master[0], master[1], master[2]);
        //COMETError(" y_axis: ("<<y_axis.x()<<", "<<y_axis.y()<<", "<<y_axis.z()<<")");

        // Get Z axis
        double z_dir[3] = {0,0,1};
        node->LocalToMasterVect(z_dir,master);
        TVector3 z_axis(master[0], master[1], master[2]);
        //COMETError(" z_axis: ("<<z_axis.x()<<", "<<z_axis.y()<<", "<<z_axis.z()<<")");

        TRotation rot;
        rot.RotateAxes(x_axis,y_axis,z_axis);
        rot.Invert();
        
        double angle=0;
        TVector3 axis;
        rot.AngleAxis(angle,axis);

        std::stringstream line;
        for(int i=0;i<depth;++i) line<<"|  ";
        line<<name<<": ";
        line<<" Position: ("<<position.x()<<", "<<position.y()<<", "<<position.z()<<")";
        line<<" Direction: angle="<<angle<<", axis=("<<axis.x()<<", "<<axis.y()<<", "<<axis.z()<<")";
        line<<" Direction cosines: x="<<rot.ThetaX()<<", y="<<rot.ThetaY()<<", z="<<rot.ThetaZ();
        COMETLog(line.str());
        return true;
    }
};

int main(int argc, char **argv) {
  PrintPlacement operation;
  COMET::IGeometryLoopFunction userCode(operation);
  cometEventLoop(argc,argv,userCode,1);
}