#include "Utilities/Configuration/interface/Architecture.h"
#include "Visualisation/CustomTracker/interface/TkSceneUpdate.h"
#include  <string>

#include <Inventor/nodes/SoMaterial.h>
#include "Inventor/SbLinear.h"
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoRotation.h>
#include <Inventor/nodes/SoIndexedLineSet.h>
#include <Inventor/nodes/SoResetTransform.h>
#include "Ig_Extensions/IgOpenInventor/interface/IgSoG4Trap.h"
#include "Ig_Extensions/IgOpenInventor/interface/IgSoCube.h"

#include "Tracker/TkLayout/interface/FullTracker.h"
#include "Tracker/TkLayout/interface/CmsTracker.h"
#include "ClassReuse/GeomVector/interface/GlobalPoint.h"
#include "CommonDet/DetGeometry/interface/TkRotation.h"
#include "CommonDet/DetGeometry/interface/Surface.h"
#include "CommonDet/DetGeometry/interface/ActiveMediaShape.h"
#include "CommonDet/DetLayout/interface/DetLayer.h"
#include "CommonDet/BasicDet/interface/DetUnit.h"
#include "CommonDet/BasicDet/interface/Enumerators.h"
#include "CommonDet/BasicDet/interface/Det.h"
#include "CommonDet/BasicDet/interface/DetType.h"
#include "CommonDet/BasicDet/interface/SimHit.h"  
#include "CommonDet/BasicDet/interface/SimDet.h"
#include "Ig_Extensions/IgOpenInventor/interface/IgSoHitsKit.h"

#include <map>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoIndexedLineSet.h>

void TkSceneUpdate::loadTkModuleMap()
{ 
    const CmsTracker::LayerContainer& theLayerContainer = FullTracker::instance()->forwardLayers();      
    CmsTracker::layer_p_iter iLay;
    int det_type=0;
    int nlay=0;
    int nmod1=0;
    int anello = 0;
    int nmod = 0;
    int ntotmod = 0;
    float r;
    float phi,phi1;    
    float hbotedge,  htopedge, hapothem, hthickness;
    int spicchi[] ={24,24,40,56,40,56,80};
    float rmedioS[]={0.27665, 0.3671, 0.4474, 0.5617, 0.6768, 0.8189, 0.9907};
    //float rmedioP[]={0.064122, 0.0766412, 0.0899707, 0.10658, 0.12008, 0.137039, 0.145999};
     float rmedioP[]={0.0579826 , 0.0643087, 0.0763373, 0.0896861, 0.106095, 0.118814, 0.135634, 0.143709};
    for (iLay = theLayerContainer.begin(); iLay != theLayerContainer.end(); iLay++) {
        nlay++;
	Module layer_type = (*iLay)->module();
	const CmsTracker::DetContainer theDetContainer = (*iLay)->detUnits();
	CmsTracker::det_p_iter iDet;
	for (iDet = theDetContainer.begin() ; iDet != theDetContainer.end() ; iDet++) {	    
             float posx = (*iDet)->surface().position().x()/ 100.0;  // cm -> m
             float posy = (*iDet)->surface().position().y()/ 100.0;  // cm -> m
             float posz = (*iDet)->surface().position().z()/ 100.0;  // cm -> m
             r=pow((posx*posx + posy*posy),0.5);
             phi1=atan(posy/posx);
             phi=phi1;
             if(posy < 0.&&posx>0)phi=phi1+2.*3.1415;
             if(posx < 0.)phi=phi1+3.1415;
             if(fabs(posy)<0.000001&&posx>0)phi=0;
             if(fabs(posy)<0.000001&&posx<0)phi=3.1415;
             if(fabs(posx)<0.000001&&posy>0)phi=3.1415/2.;
             if(fabs(posx)<0.000001&&posy<0)phi=3.1415 + 3.1415/2.;
	     //if(fabs(posx)<0.000001&&posy<0)phi=3.1415/2.; 
             det_type=1;
             if (layer_type==silicon) {
                 det_type=2;
                 for (int i=0;i<7; i++){
                    if (fabs(r-rmedioS[i])<0.01){
                       anello =i+1;
                       break;
                    }
                 }
                 nmod=(int)((phi/(2.*3.1415))*spicchi[anello-1]+.1)+1;
             }
             else{
              for (int i=0; i<8; i++){	
                  if(fabs(r -rmedioP[i])<0.0001){
                     anello=i+1;
                     break;
                  } 
              }
               nmod=(int)((phi/(2.*3.1415))*24+.1)+1;    
             }
             if (layer_type==silicon && (*iDet)->type().isStereo()) nmod=nmod+100; 
	     
             const vector<float> par = (*iDet)->type().shape().parameters();  
       
	      ntotmod++;
	      
	    //tkModuleMap[nlay*100000+anello*1000+nmod]=(*iDet);  
	   
     TkModuleMap::iterator pos; 
     int chiave=nlay*100000+anello*1000+nmod;
     pos = tkModuleMap.find(chiave);
     if(pos!=tkModuleMap.end()){
            
	cout << ntotmod <<" " <<nlay <<" " << anello << " " << nmod <<" " << (*iDet) << " " <<posx <<" "<<posy << endl;
     } 
	  tkModuleMap[nlay*100000+anello*1000+nmod]=(*iDet);  
   }
    }
    const CmsTracker::LayerContainer& theLayerContainer1 = FullTracker::instance()->barrelLayers();
    CmsTracker::layer_p_iter iLay1;
    nmod1=0;
    nmod = 0;
    float oldz;
    int spicchib[] ={18,30,42,28,38,44,56,42,48,54,60,66,74};
    for (iLay1 = theLayerContainer1.begin(); iLay1 != theLayerContainer1.end(); iLay1++) {
        nlay++;
        anello = -1;
	Module layer_type1 = (*iLay1)->module();
	const CmsTracker::DetContainer theDetContainer1 = (*iLay1)->detUnits();
	CmsTracker::det_p_iter iDet1;
	for (iDet1 = theDetContainer1.begin() ; iDet1 != theDetContainer1.end() ; iDet1++) {	    
             float posx1 = (*iDet1)->surface().position().x()/ 100.0;  // cm -> m
             float posy1 = (*iDet1)->surface().position().y()/ 100.0;  // cm -> m
             float posz1= (*iDet1)->surface().position().z()/ 100.0;  // cm -> m
             if (anello == -1){
             oldz = posz1;
             anello = 1;
        }
        else{
            if((nlay<36 && fabs(oldz - posz1) > 0.001) || 
               ( nlay>35 && fabs(oldz - posz1) > 0.05 ))
            {
              oldz = posz1;
              anello++;
            }
        }     
             r=pow((posx1*posx1 + posy1*posy1),0.5);
             phi1=atan(posy1/posx1);
             phi=phi1;
             if(posy1 < 0.&&posx1>0)phi=phi1+2.*3.1415;
             if(posx1 < 0.)phi=phi1+3.1415;
             if(fabs(posy1)<0.000001&&posx1>0)phi=0;
             if(fabs(posy1)<0.000001&&posx1<0)phi=3.1415;
             if(fabs(posx1)<0.000001&&posy1>0)phi=3.1415/2.;
             if(fabs(posx)<0.000001&&posy<0)phi=3.1415 + 3.1415/2.;
             nmod=(int)((phi/(2.*3.1415))*spicchib[nlay-29]+.1)+1;
             det_type=1;
             if (layer_type1==silicon)  det_type=2;
             if (layer_type1==silicon && (*iDet1)->type().isStereo()) nmod=nmod+100; 
            const vector<float> par = (*iDet1)->type().shape().parameters();
 
            ntotmod++;
 
	 tkModuleMap[nlay*100000+anello*1000+nmod]=(*iDet1);   
	 //cout << ntotmod <<" " <<nlay <<" " << anello << " " << nmod <<" " << (*iDet1) <<endl; 
       
      }	 
    }
}

void TkSceneUpdate::createRotation( const DetUnit* det, SbVec3f& axis, float& angle )
{
    // Extract the coeff of the rotation matrix with a projection on the basis vectors
    TkRotation< float > rot = det->surface().rotation();
    Basic3DVector< float > nx( 1., 0., 0. ); // x basis vector
    Basic3DVector< float > ny( 0., 1., 0. ); // y basis vector
    Basic3DVector< float > nz( 0., 0., 1. ); // z basis vector
    Basic3DVector< float > px = rot * nx; // Rxx Ryx Rzx
    Basic3DVector< float > py = rot * ny; // Rxy Ryy Rzy
    Basic3DVector< float > pz = rot * nz; // Rxz Ryz Rzz
    /* the rotation matrix is
     *  Rxx Rxy Rxz
     *  Ryx Ryy Ryz
     *  Rzx Rzy Rzz
     */
    // SbMatrix in row major order. The 4th dimension is set empiricly set to the identity
    SbMatrix sbm( px.x(), py.x(), pz.x(), 0,
                  px.y(), py.y(), pz.y(), 0,
                  px.z(), py.z(), pz.z(), 0,
                  0,      0,      0, 1 );
    SbRotation sbr( sbm );
    float x, y, z;
    sbr.getValue( axis, angle );
    axis.getValue( x, y, z );
}

void TkSceneUpdate::addWafer( SoSeparator* separator, const DetUnit* det )
{
float vertexPositions[8][3] =
 {
   { 0.0, 0.0, 0.0 },    
   { 0.0, 0.0, 0.0 },  
   { 0.0, 0.0, 0.0 },
   { 0.0, 0.0, 0.0 },

   { 0.0, 0.0, 0.0 },
   { 0.0, 0.0, 0.0 },
   { 0.0, 0.0, 0.0 },
   { 0.0, 0.0, 0.0 }, 
 };
  
 int32_t indicesL[30] =
 {
    0, 1,  2,  3,  SO_END_LINE_INDEX, //bottom
    0, 4,  5,  1,  SO_END_LINE_INDEX,
    1, 2,  6,  5,  SO_END_LINE_INDEX,
    3, 2,  6,  7,  SO_END_LINE_INDEX,
    0, 3,  7,  4,  SO_END_LINE_INDEX,
    4, 5,  6,  7,  SO_END_LINE_INDEX, //top
  };
    // position of the wafer's center
    float posx = det->surface().position().x() / 100.0;  // cm -> m
    float posy = det->surface().position().y() / 100.0;  // cm -> m
    float posz = det->surface().position().z() / 100.0;  // cm -> m
    
    SoResetTransform *resetTransform = new SoResetTransform();
    separator->addChild(resetTransform);

   
    // Translation of the wafer
    SoTranslation* theTranslation = new SoTranslation();
    theTranslation->translation.setValue( posx, posy, posz );
    separator->addChild( theTranslation );

    float angle;
    SbVec3f axis;
    createRotation( det, axis, angle );
    SoRotation* theRotation = new SoRotation();
    theRotation->rotation.setValue( axis, angle );
    separator->addChild( theRotation );
    
    SoMaterialBinding *myBinding = new SoMaterialBinding;
   	myBinding->value = SoMaterialBinding::PER_FACE;

   	separator->addChild(myBinding);
	
        // Representation of the wafers as G4Boxes or G4Traps
    const vector< float > par = det->type().shape().parameters();
    if( par.size() == 3 )
    { // Rectangular
        float length = det->type().shape().length() / 100.0; // cm -> m
        float width = det->type().shape().width() / 100.0;   // cm -> m
        float thickness = det->type().shape().thickness() / 100.0;  // cm -> m

       IgSoCube* theCube   = new IgSoCube();
	//cout <<" cube "<<theCube<<endl;
	tkPickedModuleMap[theCube]= det;
        theCube->width     = width;
        theCube->length    = length;
        theCube->thickness = thickness;

        separator->addChild( theCube );
   }
    else if( par.size() == 4 )
    { // Trapezoidal
        float hbotedge=par[0]/100.0; // cm -> m
        float htopedge=par[1]/100.0; // cm -> m
        float hapothem=par[3]/100.0; // cm -> m
        float hthickness=par[2]/100.0; // cm -> m

     IgSoG4Trap *theG4Trap   = new IgSoG4Trap();
	//cout <<" trap "<<theG4Trap<<endl;
	tkPickedModuleMap[theG4Trap]= det;
        // see IgSoG4Trap.h and ActiveMediaTrapezoid.h for explanations
        theG4Trap->pDz = hthickness;
        theG4Trap->pDy1= hapothem;
        theG4Trap->pDy2= hapothem;
        theG4Trap->pDx1= hbotedge;
        theG4Trap->pDx2= htopedge;
        theG4Trap->pDx3= hbotedge;
        theG4Trap->pDx4= htopedge;
        theG4Trap->pAlp1= 0.0;
        theG4Trap->pAlp2= 0.0;
        separator->addChild(theG4Trap);

	vertexPositions[0][0] =hbotedge; 
        vertexPositions[1][0] =-hbotedge;
        vertexPositions[2][0] =-hbotedge;
        vertexPositions[3][0] =hbotedge;

	vertexPositions[0][1] = hapothem;
        vertexPositions[1][1] = hapothem;
        vertexPositions[2][1] = -hapothem;
        vertexPositions[3][1] = -hapothem;

        vertexPositions[0][2] = -hthickness;
        vertexPositions[1][2] = -hthickness;
        vertexPositions[2][2] = -hthickness;
        vertexPositions[3][2] = -hthickness;

        vertexPositions[4][0] =htopedge;
        vertexPositions[5][0] =-htopedge;
        vertexPositions[6][0] =-htopedge;
        vertexPositions[7][0] =htopedge;

        vertexPositions[4][1] = hapothem;
        vertexPositions[5][1] = hapothem;
        vertexPositions[6][1] = -hapothem;
        vertexPositions[7][1] = -hapothem;

        vertexPositions[4][2] = hthickness;
        vertexPositions[5][2] = hthickness;
        vertexPositions[6][2] = hthickness;
        vertexPositions[7][2] = hthickness;

        // Define coordinates for vertices
   	SoCoordinate3 *myCoords = new SoCoordinate3;
   	myCoords->point.setValues(0, 8, vertexPositions);
   	separator->addChild(myCoords);

	SoIndexedLineSet *myLineSet = new SoIndexedLineSet;
	myLineSet->coordIndex.setValues(0, 30, indicesL);
	separator->addChild(myLineSet);
   }
}

void TkSceneUpdate::updateModule(bool del_add, int clr_dsk, int ring, int module)
{

  root->enableNotify(FALSE);

  if(!del_add){
     
     if(MatrixSeparatorM[clr_dsk][ring][module]){
       root->removeChild(MatrixSeparatorM[clr_dsk][ring][module]); 
       MatrixSeparatorM[clr_dsk][ring][module]->unref();
       
     }
     else{
       cout << "module number "<< clr_dsk <<" " << ring << " " << module <<"not found"<<endl;
     }
  }else{
     del = new SoSeparator();
     cout <<del<<endl;
     
     MatrixSeparatorM[clr_dsk][ring][module] = del;
     cout << clr_dsk << " " << ring << " " << module << " " << del<<endl;
     
     root->addChild(del);
     del->ref();
     DetUnit *det;
     
     TkModuleMap::iterator pos; 
     int chiave=clr_dsk*100000+ring*1000+module;
     pos = tkModuleMap.find(chiave);
     if(pos!=tkModuleMap.end()){
        det=pos->second;
        addWafer(del,det);      
     } else {cout << "sbagliato"<<endl;}
  }
  root->enableNotify(TRUE);
  root->touch(); 
}

void TkSceneUpdate::updateRing(bool del_add, int clr_dsk, int ring)
{

 root->enableNotify(FALSE);

  if(!del_add){
     
     if(MatrixSeparator[clr_dsk][ring]){
       root->removeChild(MatrixSeparator[clr_dsk][ring]); 
       MatrixSeparator[clr_dsk][ring]->unref();
     }
     else{
       cout << "ring number "<< clr_dsk <<" " << ring << "not found"<<endl;
     }
  }else{
     del = new SoSeparator();
     MatrixSeparator[clr_dsk][ring] = del;
     
     root->addChild(del);
     del->ref();
     
     DetUnit *det;
     TkModuleMap::iterator pos; 
     
     for(int i=0;i<200;i++){      
        int chiave=clr_dsk*100000+ring*1000+(i+1);
        pos = tkModuleMap.find(chiave);
     	
       if(pos!=tkModuleMap.end()){
         det=pos->second;
         addWafer(del,det);     
       } 
     }
     
  }
  root->enableNotify(TRUE);
  root->touch(); 
}	

void TkSceneUpdate::updateLayer(bool del_add, int clr_dsk)
{

 root->enableNotify(FALSE);

  if(!del_add){
     
     if(MatrixSeparatorL[clr_dsk]){
       root->removeChild(MatrixSeparatorL[clr_dsk]); 
       MatrixSeparatorL[clr_dsk]->unref();
     }
     else{
       cout << "layer number "<< clr_dsk << " not found"<<endl;
     }
  }else{
     del = new SoSeparator();
     MatrixSeparatorL[clr_dsk] = del;
     
     root->addChild(del);
     del->ref();
     
     DetUnit *det;
     TkModuleMap::iterator pos; 
     
     for(int k=0;k<12;k++){
      for(int i=0;i<200;i++){      
        int chiave=clr_dsk*100000+k*1000+(i+1);
        pos = tkModuleMap.find(chiave);     	
        if(pos!=tkModuleMap.end()){
         det=pos->second;
         addWafer(del,det);     
        } 
      }
     }
  }
  root->enableNotify(TRUE);
  root->touch(); 
}	

void TkSceneUpdate::updateSet(bool del_add,CrossedDetUnitSet cdu)
{
  root->enableNotify(FALSE);

 /*  if(!del_add){
     
    if(MatrixSeparator[clr_dsk][ring]){
       root->removeChild(MatrixSeparator[clr_dsk][ring]); 
       MatrixSeparator[clr_dsk][ring]->unref();
     }
     else{
       cout << "ring number "<< clr_dsk <<" " << ring << "not found"<<endl;
     }
  }else{*/
     del = new SoSeparator();
    // MatrixSeparator[clr_dsk][ring] = del;
     
     root->addChild(del);
     del->ref();
     
     //DetUnit *det;
     CrossedDetUnitSet::iterator cross; 
     
    for (cross = cdu.begin(); cross != cdu.end(); ++cross) {
       // det=*cross;
	
        addWafer(del,*cross); 
    }

  //}
  root->enableNotify(TRUE);
  root->touch(); 
}

void TkSceneUpdate::drawSimHits() 
{
   root->enableNotify(FALSE);
   
  if(theSoSeparator1){  
    cout <<"entra"<<endl; 
    root->removeChild( theSoSeparator1 );
    theSoSeparator1->unref();
  }  
   theSoSeparator = new SoSeparator();
  theSoSeparator1=theSoSeparator;
  
    // Define material
    // ---------------
  
    SoMaterial *SoHitMat = new SoMaterial;                      
    SoHitMat->ambientColor.setValue      (0.000, 0.000, 0.000); //
    SoHitMat->diffuseColor.setValue      (1.000, 0.000, 0.000); // red 
    SoHitMat->specularColor.setValue     (0.000, 0.000, 0.000); //
    SoHitMat->emissiveColor.setValue     (0.000, 0.000, 0.000); //
    SoHitMat->shininess = 0.0;
    SoHitMat->transparency = 0.0;
 
    root->addChild( theSoSeparator );
    cout <<"entra1"<<endl; 
    theSoSeparator->ref();
    theSoSeparator->addChild( SoHitMat );
 
 
//  Count up total number of SimHit's
//  ---------------------------------

    int nSimHits = 0;
    const CmsTracker::DetContainer& dets = FullTracker::instance()->dets();
    CmsTracker::DetIterator iDet;
    
    //TkModuleMap::iterator pos; 
    for (pos1 = tkPickedModuleMap.begin(); pos1 != tkPickedModuleMap.end(); ++pos1) { 
        
	const DetUnit* iDet=pos1->second;
	const SimDet* simDet =iDet->simDet();
     //cout << "Tracker det (2)" << simDet <<" "<< simDet->simHits().size() << endl;
        if (simDet && !simDet->simHits().empty()) {

         // cout << "Tracker det " << endl;
         const SimDet::SimHitContainer& sits = simDet->simHits();
         for (SimDet::SimhitIterator is = sits.begin(); is != sits.end(); is++) {
           nSimHits++;
         }
       }  
    } 
    
  
 

//  Collect up the SimHit's
//  -----------------------

    SbVec3f *pts = new SbVec3f[nSimHits];
    SbVec3f *iPts = pts;
    cout << "Tracker found (2D)" << nSimHits << " SimHits " << endl;

    //TkModuleMap::iterator pos; 
    for (pos1 = tkPickedModuleMap.begin(); pos1 != tkPickedModuleMap.end(); ++pos1) { 
  	
       const DetUnit* iDet=pos1->second;
	const SimDet* simDet =iDet->simDet();
 
       if (simDet && !simDet->simHits().empty()) {
 
         const SimDet::SimHitContainer& sits = simDet->simHits();
 
         for (SimDet::SimhitIterator is = sits.begin(); is != sits.end(); is++) 
         {
          
          float x = is->globalPosition().x() / 100.0;  // cm -> m
          float y = is->globalPosition().y() / 100.0;  // cm -> m
          float z = is->globalPosition().z() / 100.0;  // cm -> m
         
          *iPts++ = SbVec3f(x,y,z);                     
         }
      }
    } 
 //  Draw the SimHit's
//  -----------------

    IgSoHitsKit *theIgSoHitsKit = new IgSoHitsKit;
    
    theIgSoHitsKit->init_xyz(nSimHits,pts);
    theIgSoHitsKit->set("drawStyle { pointSize 5 }");

    theSoSeparator->addChild( theIgSoHitsKit );

    delete [] pts;
  
  
  root->enableNotify(TRUE);
  root->touch(); 
}
 void TkSceneUpdate::clear() 
{
cout <<root<< " -rootpcl-" <<rootp<<endl;
tkPickedModuleMap.clear();
//tkModuleMap.clear();
for(int i=0;i<41;i++){
       for(int j=0;j<12;j++){
          for(int k=0;k<200;k++){ 
             MatrixSeparatorM[i][j][k]=0;
	  }
	  MatrixSeparator[i][j]=0;
       }
       MatrixSeparatorL[i]=0;
    }

rootp->enableNotify(FALSE);
rootp->removeChild(root);
root->unref();

  root = new SoSeparator;
  rootp->addChild(root);
  root->ref();
  rootp->enableNotify(TRUE);
  rootp->touch(); 
} 
      
TkSceneUpdate::TkSceneUpdate( SoSeparator *root1,SoSeparator *root2)
{
    for(int i=0;i<41;i++){
       for(int j=0;j<12;j++){
          for(int k=0;k<200;k++){ 
             MatrixSeparatorM[i][j][k]=0;
	  }
	  MatrixSeparator[i][j]=0;
       }
       MatrixSeparatorL[i]=0;
    }
    rootp=root1;
    root = root2;
    
    cout <<root<< " -rootpcst-" <<rootp<<endl;
    modulo = 2;//silicon(2)-pixel(1)
    bar_fow =1;//bar(2)-fow(1)
    lay=0;
    loadTkModuleMap();
    theSoSeparator1=0;
}