TVector3 s[7]; // seven snouts
TVector3 b[3];  // three attachment blocks

Double_t rblock = 34*25.4; // mm - estimate
Double_t rsnout = (4*12-6)*25.4; // mm - estimate

Double_t phi_block[3] = {40, 160, -80}; // degrees x is east; y is north
Double_t phi_snout[7] = {10, 70, 125, 160, 195, -110, -50};
Double_t initial_length[7];
Double_t length[7];

Double_t deltaZ = 200; // 20 cm


void initialize() {  // sets nominal positions and lengths
  int i;
  for( i=0; i<7 ; i++ ) phi_snout[i] *= (TMath::Pi()/180);
  for( i=0; i<3 ; i++ ) phi_block[i] *= (TMath::Pi()/180);

  for( i=0 ; i<3 ; i++ ) {
    b[i].SetZ( 0.0 );
    b[i].SetX( rblock*TMath::Cos( phi_block[i] ) );
    b[i].SetY( rblock*TMath::Sin( phi_block[i] ) );
  }

  for( i=0 ; i<7 ; i++ ) {
    s[i].SetZ( deltaZ );
    s[i].SetX(rsnout*TMath::Cos( phi_snout[i] ) );
    s[i].SetY(rsnout*TMath::Sin( phi_snout[i] ) );
  }


  TVector3 bms;  // block minus snout
  printf("Initial lengths [mm]\n");
  for( i=0 ; i<7 ; i++ ) {
    if( i==0 || i==1 ) {
      bms = s[i] -b[0];
    } else if( i==2 || i==3 || i==4 ) {
      bms = s[i] - b[1];
    } else if( i==5 || i==6 ) {
      bms = s[i] - b[2];
    } else {
      printf("Something bad happened.\n");
    }
    length[i] = bms.Mag();
    initial_length[i] = length[i];
    printf("%d\t%f\n", i, length[i]);
  }

}

void bump(Double_t dx, Double_t dy, Double_t dz, Double_t yaw=0, Double_t pitch=0, Double_t roll=0) {
  TVector3 bumped_block[3];
  TVector3 bump(dx, dy, dz);
  for( int i=0 ; i<3 ; i++ ) {
    bumped_block[i] = b[i]+bump;
  }

    for( i=0 ; i<7 ; i++ ) {
    if( i==0 || i==1 ) {
      bms = s[i] -bumped_block[0];
    } else if( i==2 || i==3 || i==4 ) {
      bms = s[i] - bumped_block[1];
    } else if( i==5 || i==6 ) {
      bms = s[i] - bumped_block[2];
    } else {
      printf("Something bad happened.\n");
    }

    length[i] = bms.Mag();
    printf("%d\t%f\t%f\t%f\n", i, length[i] - initial_length[i], length[i], initial_length[i]);
  }



}