//
// $Id: changeGeometry.cpp,v 1.2 2003/01/23 17:47:22 melanson Exp $
//
// File: readGeometry.cpp
// Purpose: 
// Created: 21-FEB-1998   John Hobbs
//
// $Revision: 1.2 $
//
//

// Include files
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>

#include "CLHEP/Random/Randomize.h"
#include "CLHEP/Random/RandGauss.h"
#include "d0om/d0_Ref.hpp"
#include <d0_geometry/base/BaseGeometry.hpp>
#include <geometry_system/components/CartesianCoordinate.hpp>
#include "silicon_geometry/base/SiBaseGeometry.hpp"

using namespace std;
using namespace dgs;

// The move routines
void do_smt();
void do_smt_barrels();
void do_smt_layers();
void do_smt_tilt_layers();
void do_smt_ladders();
void do_smt_random_ladders();
void do_cft_barrel();
//void do_cft();
//void do_cft_ribbons();
//void do_cft_random_ribbons();
//void do_cal_cc()


string infile;
string fbase;
int    fcount=1;

// A trivial helper class
class SmtInfo {
public:
  SmtInfo(): d_barrel(-1), d_layer(-1), d_ladder(-1), d_posn(0) {}
  SmtInfo(int barrel, int layer=-1, int ladder=-1, GeometryElement *posn=0):
    d_barrel(barrel), d_layer(layer), d_ladder(ladder), d_posn(posn)
      { if( posn ) d_oldposn = posn->get_position(); }
  void update() { if( d_posn ) d_newposn = d_posn->get_position(); }
  int d_barrel,d_layer,d_ladder;
  GeometryElement *d_posn;
  GeometryXform d_oldposn,d_newposn;
};

// Formatted print of the helper class
ostream& operator<<(ostream& os, SmtInfo& me)
{
  GeometryXform::_ndecimal=4;
  os << "(Barrel,Layer,Ladder) = (" << me.d_barrel << "," << me.d_layer 
     << "," << me.d_ladder << ")" << endl
     << "      " << me.d_oldposn << endl
     << "      " << me.d_newposn;
  return os;
}

// Generate the output file name
string outfile() 
{
  stringstream fname;
  fname << fbase << "-test" << setfill('0') << setw(3) << fcount++ << ".evpack";
  cout << "Writing to " << fname.str() << endl;
  return string(fname.str());
}

// Generate a gaussian random number with sigma.  Limit to 2sigma effects.
double deltaRandom(const double sigma)
{
  double delta=sigma;
  while( delta != 0.0 && abs(delta)<=2.0*sigma ) delta = RandGauss::shoot(0.0,sigma);
  return delta;
}


// ---------------------------------------------------------------------------
int main(int argc, char **argv)
{
  // Initialize d0om and handle file names.
  if( argc != 2 ) {
    cout << "Usage: changeGeometry infile_geometry_file" << endl;
    exit(1);
  }
  infile = string(argv[1]);
  fbase = infile.substr(0,infile.find("."));
  int slash = fbase.rfind('/')+1;
  fbase = fbase.substr(slash,fbase.size()-slash);
  cout << "Input file: " << infile << ", base name: " << fbase << endl;
  d0om_init("d0Reader");

  // SMT related moves
  do_smt_layers();
  do_smt_barrels();
  do_smt();
  do_smt_tilt_layers();
  do_smt_ladders();
  do_smt_random_ladders();


  //
  return 0;
}
// ---------------------------------------------------------------------------
// 


// Rotate and shift barrels/layers 3/5 and 4/1
void do_smt_layers() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Get the laddders for later comparison and the original positions
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  vector<SiLayer*> layers;
  vector<const SiLadder*> ladders;
  vector<CartesianCoordinate> oldposns,newposns;
  CartesianCoordinate center(0.0,0.0,0.0);
  for( int ib=1 ; ib<=6 ; ib++ ) {
    SiBarrel *sib = smt->get_mutable_center(1)->get_mutable_barrel(ib);
    if( ib==3 || ib==4 ) {
      if( ib==3 ) layers.push_back(sib->get_mutable_layer(5));
      else layers.push_back(sib->get_mutable_layer(1));
      ladders.push_back(layers[layers.size()-1]->get_ladder(1));
      oldposns.push_back(ladders[ladders.size()-1]->local_to_global(center));
    }
    const SiLayer *sil = sib->get_layer(1);
    const SiLadder *silad = sil->get_ladder(1);
    ladders.push_back(silad);
    oldposns.push_back(silad->local_to_global(center));
  }

  // Rotate the SMT by 10 microns/10 cm = 0.0001 rads about z-axis and shift
  // them along z
  GeometryXform rot(0.0,0.0,0.0,0.0,0.0,0.0003);
  for( int ib=0 ; ib<layers.size() ; ib++ ) layers[ib]->move(rot);
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

  // Compare the old and current positions
  cout << " Barrels/Layers 3/5 & 4/1 rotated and shifted" << endl;
  for( int il=0 ; il<ladders.size() ; il++ ) {
    newposns.push_back(ladders[il]->local_to_global(center));
    cout << "       " 
	 << oldposns[il].phi() << " " << newposns[il].phi() << " dphi="
	 << newposns[il].phi()-oldposns[il].phi() << endl;
  }

}
// 

// Rotate and shift barrels 2 and 4
void do_smt_barrels() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Get the laddders for later comparison and the original positions
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  vector<SiBarrel*> barrels;
  vector<const SiLadder*> ladders;
  vector<CartesianCoordinate> oldposns,newposns;
  CartesianCoordinate center(0.0,0.0,0.0);
  for( int ib=1 ; ib<=6 ; ib++ ) {
    SiBarrel *sib = smt->get_mutable_center(1)->get_mutable_barrel(ib);
    if( ib==2 || ib==4 ) barrels.push_back(sib);
    const SiLayer *sil = sib->get_layer(1);
    const SiLadder *silad = sil->get_ladder(1);
    ladders.push_back(silad);
    oldposns.push_back(silad->local_to_global(center));
  }

  // Rotate the SMT by 10 microns/10 cm = 0.0001 rads about z-axis and shift
  // them along z
  GeometryXform rot(0.0,0.0,0.01,0.0,0.0,0.0001);
  for( int ib=0 ; ib<barrels.size() ; ib++ ) barrels[ib]->move(rot);
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

  // Compare the old and current positions
  cout << " Barrels 2 & 4 rotated and shifted" << endl;
  for( int il=0 ; il<ladders.size() ; il++ ) {
    newposns.push_back(ladders[il]->local_to_global(center));
    cout << "      "
	 << oldposns[il].phi() << " " << newposns[il].phi() << " dphi="
	 << newposns[il].phi()-oldposns[il].phi() << ", " 
	 << oldposns[il].z() << " " << newposns[il].z() << " dz=" 
	 << oldposns[il].z() - newposns[il].z() << endl;
  }

}
// 

// Rotate the entire SMT
void do_smt() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Get the laddders for later comparison and the original positions
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  vector<const SiLadder*> ladders;
  vector<CartesianCoordinate> oldposns,newposns;
  CartesianCoordinate center(0.0,0.0,0.0);
  for( int ib=1 ; ib<=6 ; ib++ ) {
    const SiBarrel *sib = smt->get_center(1)->get_barrel(ib);
    const SiLayer *sil = sib->get_layer(1);
    const SiLadder *silad = sil->get_ladder(1);
    ladders.push_back(silad);
    oldposns.push_back(silad->local_to_global(center));
  }

  // Rotate the SMT by 20 microns/10 cm = 0.0002 rads about z-axis
  GeometryXform rot(0.0,0.0,0.0,0.0,0.0,0.0002);
  smt->move(rot);
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

  // Compare the old and current positions
  cout << " SMT rotated " << endl;
  for( int il=0 ; il<ladders.size() ; il++ ) {
    newposns.push_back(ladders[il]->local_to_global(center));
    cout << "      "
	 << oldposns[il].phi() << " " << newposns[il].phi() << " dphi="
	 << newposns[il].phi()-oldposns[il].phi() << endl;
  }

}

// Rotate and shift barrels/layers 3/5 and 4/1
void do_smt_tilt_layers() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Get the laddders for later comparison and the original positions
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  vector<SiLayer*> layers;
  vector<SmtInfo> posns;
  for( int ib=1 ; ib<=6 ; ib++ ) {
    SiBarrel *sib = smt->get_mutable_center(1)->get_mutable_barrel(ib);
    if( ib==3 || ib==4 ) {
      int il=5;
      if( ib==4 ) il=1;
      SiLayer* layer=sib->get_mutable_layer(il);
      SiLadder* ladder=layer->get_mutable_ladder(1);
      layers.push_back(layer);
      posns.push_back(SmtInfo(ib,il,-1,layer));
      posns.push_back(SmtInfo(ib,il,1,ladder));
    }
    else {
      SiLayer *sil = sib->get_mutable_layer(1);
      SiLadder *silad = sil->get_mutable_ladder(1);
      posns.push_back(SmtInfo(ib,1,1,silad));
    }
  }

  // Rotate the layers by 0.001 about the global x-axis
  GeometryXform rot(0.0,0.0,0.0,0.0,0.001,0.0);
  for( int ib=0 ; ib<layers.size() ; ib++ ) layers[ib]->move(rot);
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

  // Compare the old and current positions
  cout << " Tilt Barrels/Layers 3/5 & 4/1 rotated about global x-axis" << endl;
  for( int il=0 ; il<posns.size() ; il++ ) {
    posns[il].update();
    cout << posns[il] << endl;
  }

}
// 

// Rotate barrel/layer/ladder 4/4/3 by 0.00045 about its local y
// Shift  barrel/layer/ladder 3/2/1 by 0.0015 along its local x
// Shift  barrel/layer/ladder 5/3/4 by 0.0045 along its local z and 0.00100 in y
void do_smt_ladders() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Get the laddders for later comparison and the original positions
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  vector<SmtInfo> ladders;
  for( int ib=1 ; ib<=6 ; ib++ ) {
    GeometryXform delta;
    int ilayer = -1, ilad = -1;
    switch( ib ) {
    case 3:
      ilayer=2;
      ilad=1;
      delta=GeometryXform(0.0,0.0,0.0,0.0,0.0054,0.0,
			  GeometryXform::xyz);
      break;
    case 4:
      ilayer=4;
      ilad=3;
      delta=GeometryXform(0.0015,0.0,0.0,0.0,0.0,0.0);
      break;
    case 5:
      ilayer=3;
      ilad=4;
      delta=GeometryXform(0.0,0.001,0.0045,0.0,0.0,0.0);
      break;
    default:
      break;
    }
    if( ilayer != - 1) {
      SiBarrel* sib = smt->get_mutable_center(1)->get_mutable_barrel(ib);
      SiLayer* sil = sib->get_mutable_layer(ilayer);
      SiLadder* silad = sil->get_mutable_ladder(ilad);
      ladders.push_back(SmtInfo(ib,ilayer,ilad,silad));
      silad->move(delta);
      ladders[ladders.size()-1].update();
    }
  }

  // Write out the geometry
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

  // Compare the old and current positions
  cout << " Ladder specific motion" << endl;
  for( int i=0 ; i<ladders.size() ; i++ ) cout << ladders[i] << endl;

}
// 

void do_smt_random_ladders() {
    
  d0_Ref<BaseGeometry> base = BaseGeometry::readGeometry(infile,"EVPACK");
  if( base.is_null() ) throw "Fatal error getting geometry";

  // Range of random motion about each of the six GX coordinates. Truncated
  // at two sigma.
  const double dxSigma=0.0010;  // Local x
  const double dySigma=0.0070;  // Local y (radius)
  const double dzSigma=0.0045;  // Local z
  const double daSigma=0.00125;
  const double dbSigma=0.00025;
  const double dgSigma=0.0;

  // Get the laddders for later comparison and the original positions
  cout << " Random ladder-by-ladder motion" << endl;
  d0_Ref<SiBaseGeometry> smt = base->get_mutable_silicon();
  int nbarrels = smt->get_center(1)->get_barrel_count();
  for( int ib=1 ; ib<=nbarrels ; ib++ ) {
    SiBarrel *barrel = smt->get_mutable_center(1)->get_mutable_barrel(ib);
    int nlayers = barrel->get_layer_count();
    for( int il=1 ; il<=nlayers ; il++ ) {
      SiLayer *layer = barrel->get_mutable_layer(il);
      int nladders = layer->get_ladder_count();
      for( int ilad=1 ; ilad<=nladders ; ilad++ ) {
	SiLadder *ladder=layer->get_mutable_ladder(ilad);
	double dx = deltaRandom(dxSigma);
	double dy = deltaRandom(dySigma);
	double dz = deltaRandom(dzSigma);
	double da = deltaRandom(daSigma);
	double db = deltaRandom(dbSigma);
	double dg = deltaRandom(dgSigma);
	ladder->move(GeometryXform(dx,dy,dz,da,db,dg,GeometryXform::xyz));
	cout << setw(3) << ib << setw(3) << il << setw(3) << ilad << "  :  "
	     << setw(8) << dx << " " << setw(8) << dy << " " << setw(8) << dz << " " 
	     << setw(8) << da << " " << setw(8) << db << " " << setw(8) << dg 
	     << " (xyz convention)" << endl;
      }
    }
  }

  // Write out the geometry
  BaseGeometry::writeGeometry(base,outfile(),"EVPACK");

}