int Inverse(double A[4][4],double B[4][4]){
int i,j,k;
double tmp_a[4][4];
double inv_a[4][4];
double buf;
int n=4;

for(i=0;i<n;i++){
  for(j=0;j<n;j++){
    tmp_a[i][j]=A[i][j];
  }
}
for(i=0;i<n;i++){
  for(j=0;j<n;j++){
    inv_a[i][j]=(i==j)?1.0:0.0;
  }
}
for(i=0;i<n;i++){
  buf=1/tmp_a[i][i];
  for(j=0;j<n;j++){
    tmp_a[i][j]*=buf;
    inv_a[i][j]*=buf;
  }
  for(j=0;j<n;j++){
    if(i!=j){
      buf=tmp_a[j][i];
      for(k=0;k<n;k++){
        tmp_a[j][k]-=tmp_a[i][k]*buf;
        inv_a[j][k]-=inv_a[i][k]*buf;
      }
    }
  }
}
for(i=0;i<n;i++){
  for(j=0;j<n;j++){
    B[i][j]=inv_a[i][j];
  }
}
 return 1;
}
Double_t fitf(Double_t *x, Double_t *par){
   Double_t result=0;

   result=par[0]*x[0]+par[1];

   return result;

}
//void inpManHsol_Line2(){ 
void inpManLsol2022(){ 
 int local_cartesian=0;
 int bug=0;

 int i,j,k;
 char fname[2][256];
 double posx[20][50*4],posy[20][50*4],posz[20][50*4],radius[20][50*4];
 double Vx[20][50*4],Vy[20][50*4],Vz[20][50*4],dl[20][50*4],dh[20][50*4];
 double dVx[20][50*4],dVy[20][50*4],dVz[20][50*4];
 int step[20][50*4],step0[50*4];
 double posx0[20],posy0[20],posz0[20],radius0[20];
 double Vx0[20],Vy0[20],Vz0[20];
 int st0[20];
 double pi=TMath::Pi();
 double cV00;

 double cdx[20][50*4],cdy[20][50*4],cdz[20][50*4];
 double cVx[20][50*4],cVy[20][50*4],cVz[20][50*4];
 double cAngx[20][50*4],cAngy[20][50*4],cAngz[20][50*4],cAngx2[20][50*4];
 double cAngxx[20][50*4],pitch[20][50*4];
 double cnx[20][3],cny[20][3],cnz[20][3];//unit vectors at check point [pointID][x,y,z]
 double nx,ny,nz,nAng;
 int cn,fnm[2];
int loop=1;//==jj0
//int loop=10;//==jj0
int read_i;

TPolyMarker3D *dist,*tr0;
  tr0=new TPolyMarker3D(256*2);
  dist=new TPolyMarker3D(256*80);
TPolyLine3D *trj_line;
  trj_line=new TPolyLine3D(256*2);
 

////////////////////setZaxis/
 FILE *infile;
 char fname0[256],sdmy[256],lineread[512];
 double readx,ready,readz,readVx,readVy,readVz,readvv;
 double x[16],y[16],z[16],vx[16],vy[16],vz[16],vx00[16],vy00[16],vz00[16];
 double s[16];
 double x0,y0,z0,vx0,vy0,vz0;
 int idmy,linenum,st;
 x0=8.97162380e+01;
 y0=1.47834590e+02;
 z0=4.86711590e+01;
 vx0=-2.55452100e+10;
 vy0=-1.20964000e+10;
 vz0=-9.96566000e+08;
 printf("aa\n"); 
 //sprintf(fname0,"base_2021Jan.txt");
 sprintf(fname0,"nindex2_base.txt");
 infile=fopen(fname0,"r");
 printf("fname0=%s\n",fname0);
 fgets(lineread,1024,infile);
 printf("%s",lineread);
 //getchar();
 fscanf(infile,"    %d",&linenum);
 printf("lnum=%d \n",linenum);//getchar();
 for(i=0;i<9;++i){
    fgets(lineread,1024,infile);
    printf("%s",lineread);//getchar();
  }
 i=0;
 int kk=0;
 while( (fscanf(infile,"%d   %lf       %lf       %lf       %lf      %lf       %lf    \n",
                         //&st,&readx,&ready,&readz,&readVx,&readVy,&readVz)==7) && i<linenum && ready<140.  ){
                         &st,&readx,&ready,&readz,&readVx,&readVy,&readVz)==7)/* && i<11*/  ){
     readvv=sqrt(pow(readVx,2)+pow(readVy,2)+pow(readVz,2));
     if(kk<256*2){
         //tr0->SetPoint(i,readx,ready,readz);
         tr0->SetPoint(kk,readz,readx,ready);
         //trj_line->SetPoint(i,readx,ready,readz);
         trj_line->SetPoint(kk,readz,readx,ready);
     }
     if(i<11){
       x[i]=readx;
       y[i]=ready;
       z[i]=readz;
       vx00[i]=readVx;
       vy00[i]=readVy;
       vz00[i]=readVz;

       if(i==0){
        vx0=readVx;
        vy0=readVy;
        vz0=readVz;
        x0=readx;
        y0=ready;
        z0=readz;
       }
       vx[i]=readVx/(double)readvv;
       vy[i]=readVy/(double)readvv;
       vz[i]=readVz/(double)readvv;
      printf("i=%d\n",i);
    }
    if(i%1==0)kk++;
    i++;
     
 }
 fclose(infile);
 int numi=11;
 double rnx[16],rny[16],rnz[16],rr[16],ttt0,pp1;
 for(int i=0;i<numi;++i){
   rnx[i]=x[i]-x0; 
   rny[i]=y[i]-y0; 
   rnz[i]=z[i]-z0; 
   rr[i]=sqrt(pow(rnx[i],2)+pow(rny[i],2)+pow(rnz[i],2));
   rnx[i]=rnx[i]/(double)rr[i];
   rny[i]=rny[i]/(double)rr[i];
   rnz[i]=rnz[i]/(double)rr[i];
   ttt0=rnx[i]*vx[i]+rny[i]*vy[i]+rnz[i]*vz[i];
   pp1=vx[5]*vx[i]+vy[5]*vy[i]+vz[5]*vz[i];
   printf("i=%d y=%lf rnx=%e %e %e vx=%e %e %e cos=%e pp1=%e rr=%e\n",i,y[i],rnx[i],rny[i],rnz[i],vx[i],vy[i],vz[i],ttt0,pp1,rr[i]);
 }
 double sx=(x[5]-x0)/(double)rr[5];
 double sy=(y[5]-y0)/(double)rr[5];
 double sz=(z[5]-z0)/(double)rr[5];
 double svx=(vx00[5]-vx0)/(double)rr[5];
 double svy=(vy00[5]-vy0)/(double)rr[5];
 double svz=(vz00[5]-vz0)/(double)rr[5];
 double range=-0.2;
 range=0.;
 double xx,yy,zz,dr,vxx,vyy,vzz;
 for(int i=0;i<numi;++i){
     dr=-1*range+range*i*2/(double)numi;
     xx=dr*sx+x0;
     yy=dr*sy+y0;
     zz=dr*sz+z0;
     vxx=dr*svx+vx0;
     vyy=dr*svy+vy0;
     vzz=dr*svz+vz0;
 }
////////////////////setZaxis/

double cV0[20],cVzx0[20],cVxy0[20];
double Minv[4][4];
       cV0[0]=sqrt(pow(vx0,2)+pow(vy0,2)+pow(vz0,2));
       cVzx0[0]=sqrt(pow(vx0,2)+pow(vz0,2));
       if(local_cartesian!=1){
        cnz[0][0]=vx0/(double)cV0[0];
        cnz[0][1]=vy0/(double)cV0[0];
        cnz[0][2]=vz0/(double)cV0[0];
       }else{
        cnz[0][0]=vx0/(double)cVzx0[0];
        cnz[0][1]=0;
        cnz[0][2]=vz0/(double)cVzx0[0];
       }
       
       cnx[0][0]=vz0/(double)cVzx0[0];
       cnx[0][1]=0;
       cnx[0][2]=-1*vx0/(double)cVzx0[0];
       if(bug==1){
        cny[0][0]=cnx[0][1]*cnz[0][2]-cnx[0][2]*cnz[0][1];
        cny[0][1]=cnx[0][2]*cnz[0][0]-cnx[0][0]*cnz[0][2];
        cny[0][2]=cnx[0][0]*cnz[0][1]-cnx[0][1]*cnz[0][0];
       }else{//tadasii
        cny[0][0]=cnz[0][1]*cnx[0][2]-cnz[0][2]*cnx[0][1];
        cny[0][1]=cnz[0][2]*cnx[0][0]-cnz[0][0]*cnx[0][2];
        cny[0][2]=cnz[0][0]*cnx[0][1]-cnz[0][1]*cnx[0][0];
       }
       printf("cnx[%d]=%lf %lf %lf\n",0,cnx[0][0],cnx[0][1],cnx[0][2]);
       printf("cny[%d]=%lf %lf %lf\n",0,cny[0][0],cny[0][1],cny[0][2]);
       printf("cnz[%d]=%lf %lf %lf:%lf\n",0,cnz[0][0],cnz[0][1],cnz[0][2],sqrt(pow(cny[0][0],2)+pow(cny[0][1],2)+pow(cny[0][2],2)));

//one by one
int picki=0;
for(picki=0;picki<1;++picki){
  double M[4][4]={{cnx[picki][0],cnx[picki][1],cnx[picki][2],0},
                {cny[picki][0],cny[picki][1],cny[picki][2],0},
                {cnz[picki][0],cnz[picki][1],cnz[picki][2],0},
                {0,0,0,1}};
  double Mt[4][4]={{-1*cnx[picki][0],-1*cnx[picki][1],-1*cnx[picki][2],0},
                {-1*cny[picki][0],-1*cny[picki][1],-1*cny[picki][2],0},
                {-1*cnz[picki][0],-1*cnz[picki][1],-1*cnz[picki][2],0},
                {0,0,0,1}};
  Inverse(M, Minv);
       printf("M=%lf %lf %lf %lf\n",M[0][0],M[0][1],M[0][2],M[0][3]);
       printf("M=%lf %lf %lf %lf\n",M[1][0],M[1][1],M[1][2],M[1][3]);
       printf("M=%lf %lf %lf %lf\n",M[2][0],M[2][1],M[2][2],M[2][3]);
       printf("M=%lf %lf %lf %lf\n",M[3][0],M[3][1],M[3][2],M[3][3]);
printf("----------------------------\n");
       printf("Minv=%lf %lf %lf %lf\n",Minv[0][0],Minv[0][1],Minv[0][2],Minv[0][3]);
       printf("Minv=%lf %lf %lf %lf\n",Minv[1][0],Minv[1][1],Minv[1][2],Minv[1][3]);
       printf("Minv=%lf %lf %lf %lf\n",Minv[2][0],Minv[2][1],Minv[2][2],Minv[2][3]);
       printf("Minv=%lf %lf %lf %lf\n",Minv[3][0],Minv[3][1],Minv[3][2],Minv[3][3]);
printf("----------------------------\n");
       printf("Mt=%lf %lf %lf %lf\n",Mt[0][0],Mt[0][1],Mt[0][2],Mt[0][3]);
       printf("Mt=%lf %lf %lf %lf\n",Mt[1][0],Mt[1][1],Mt[1][2],Mt[1][3]);
       printf("Mt=%lf %lf %lf %lf\n",Mt[2][0],Mt[2][1],Mt[2][2],Mt[2][3]);
       printf("Mt=%lf %lf %lf %lf\n",Mt[3][0],Mt[3][1],Mt[3][2],Mt[3][3]);
//  getchar();
//for(int i=0;i<4;++i){
// for(int j=0;j<4;++j){
//    Minv[i][j]=M[i][j];
// }
//}
////////////////////
}//end for (picki)

//

/////New Injection TRY//////////////
 FILE *inTRY;
 char iname[256];
 sprintf(iname,"inInjTwissL_2021miniSol_line4.txt");
 //sprintf(iname,"test_2021Sep27_XY.txt");
 inTRY=fopen(iname,"r");
 double tdx[100*2],tdy[100*2],tpx[100*2],tpy[100*2];
 double tdz[100*2],tt[100*2];
 double tdx0,tdy0,tpx0,tpy0;
 double tdz0,tt0;
 int inum=0;
 int select=0;
 while( fscanf(inTRY,"%lf,%lf,%lf,%lf,%lf,%lf",&tdx0,&tdy0,&tpx0,&tpy0,&tdz0,&tt0)==6){
      tdx[inum]=tdx0;
      tdy[inum]=tdy0;
      tpx[inum]=tpx0;
      tdx[inum]=tdx0;
      tpy[inum]=tpy0;
      //tdz[inum]=tdz0-0.6;//cm
      tdz[inum]=tdz0;//cm
      tt[inum]=tt0;
      inum++;
     //cm rad 
     printf("tdx0=%lf tdy0=%lf tpx0=%lf tpy0=%lf\n",tdx0,tdy0,tpx0,tpy0);
 }
 TGraph *gr_txx,*gr_tyy,*gr_txy,*gr_tpxpy,*gr_zt;
 gr_txx=new TGraph(inum,tdx,tpx);
 gr_tyy=new TGraph(inum,tdy,tpy);
 gr_txy=new TGraph(inum,tdx,tdy);
 gr_tpxpy=new TGraph(inum,tpx,tpy);
 gr_zt=new TGraph(inum,tdz,tt);
/////New Injection TRY END//////////////
///////Graph

gROOT->SetStyle("Plain");
gROOT->ForceStyle();
gStyle->SetOptStat(0);
gStyle->SetCanvasBorderMode(0);
gStyle->SetPadColor(0);
gStyle->SetTitleFillColor(0);
gStyle->SetStatColor(0);

double xxp[20][2];
double yyp[20][2];
double xy[20][2];
double xpyp[20][2];

 char com[20][128];
   sprintf(com[0],"y=%d cm",fnm[0]);
   sprintf(com[1],"y=%d cm",fnm[1]);
 int color=0;
 char fc[32];
 TCanvas *c16 = new TCanvas("c16","test_1",300,100,800,800);
 c16->Divide(3,2);
 TH2F *frame16[6];
 for(int i=0;i<6;++i){
//  lcom[i].SetTextSize(0.07);
  sprintf(fc,"frame16_%d",i);
  if(y0>110 && i<3)frame16[i] = new TH2F(fc," ",10, -6, 6, 10, -0.06,0.06);
  if(y0<110 && i<3)frame16[i] = new TH2F(fc," ",10, -6, 6, 10, -0.06,0.06);
  if(i==3)frame16[i] = new TH2F(fc," ",10, -6, 6, 10, -6,6);
  if(i==4)frame16[i] = new TH2F(fc," ",10, -0.05, 0.05, 10, -0.05,0.05);
  if(i==5)frame16[i] = new TH2F(fc," ",10, -5, 5, 10, -5,5);
  frame16[i]->SetTitle("");
  frame16[i]->GetXaxis()->SetLabelSize(0.04);
  frame16[i]->GetXaxis()->SetTitleSize(0.05);
  frame16[i]->GetYaxis()->SetLabelSize(0.04);
  frame16[i]->GetYaxis()->SetTitleSize(0.06);
  if(i==0){
   frame16[0]->GetXaxis()->SetTitle("X[cm]");
   frame16[0]->GetYaxis()->SetTitle("#deltaV_{X}/V");
  }
  if(i==1){
   frame16[1]->GetXaxis()->SetTitle("Y[cm]");
   frame16[1]->GetYaxis()->SetTitle("#deltaV_{Y}/V");
  }
  if(i==2){
   frame16[2]->GetXaxis()->SetTitle("Z[cm]");
   frame16[2]->GetYaxis()->SetTitle("#deltaV_{Z}/V");
  }
  if(i==3){
   frame16[3]->GetXaxis()->SetTitle("X[cm]");
   frame16[3]->GetYaxis()->SetTitle("Y[cm]");
  }
  if(i==4){
   frame16[4]->GetXaxis()->SetTitle("#deltaV_{X}/V");
   frame16[4]->GetYaxis()->SetTitle("#deltaV_{Y}/V");
  }
  frame16[i]->GetYaxis()->SetTitleOffset(1.5);
  frame16[i]->SetStats(0);

 }

 c16->cd(1);
 c16->SetLeftMargin(0.16);
 c16->SetRightMargin(0.05);
 frame16[0]->Draw();
printf("ppp\n");
int fit_frag=1;
 double par0=1;
  TF1 *funcxx[20];
  char ff[20];

   gr_txx->SetMarkerStyle(9);
   gr_txx->SetMarkerColor(2);
   gr_txx->SetLineColor(2);
   gr_txx->Draw("p");
 c16->SetGridx();
 c16->SetGridy();
//lcom[0].DrawLatex(0,ymax0[0][0]*0.9,com[0]);


printf("ppp\n");
 c16->cd(2);
 c16->SetLeftMargin(0.16);
 c16->SetRightMargin(0.05);
 frame16[1]->Draw();
   gr_tyy->SetMarkerStyle(7);
   gr_tyy->SetMarkerColor(2);
   gr_tyy->SetLineColor(2);
   gr_tyy->Draw("p");
 c16->SetGridx();
 c16->SetGridy();
 
printf("ppp\n");
 c16->cd(3);
 c16->SetLeftMargin(0.16);
 c16->SetRightMargin(0.05);
 frame16[2]->Draw();
color=0;
 c16->SetGridx();c16->SetGridy();
//lcom[2].DrawLatex(0,ymax0[2]*0.9,com[2]);


 c16->cd(4);
 c16->SetLeftMargin(0.16);
 c16->SetRightMargin(0.05);
 frame16[3]->Draw();

   gr_txy->SetMarkerStyle(7);
   gr_txy->SetMarkerColor(2);
   gr_txy->SetLineColor(2);
   gr_txy->Draw("p");
 c16->SetGridx();
 c16->SetGridy();

 c16->cd(5);
 c16->SetLeftMargin(0.16);
 c16->SetRightMargin(0.05);
 frame16[4]->Draw();

   gr_tpxpy->SetMarkerStyle(7);
   gr_tpxpy->SetMarkerColor(2);
   gr_tpxpy->SetLineColor(2);
   gr_tpxpy->Draw("p");
 c16->SetGridx();
 c16->SetGridy();

/////////////////////////////////////////////////////
///////////create input//////////
double rz,rvz;
double rrx[100*2],rry[100*2],rrvx[100*2],rrvy[100*2],rrz[100*2],rrvz[100*2];
rz=0;rvz=0;
FILE *ofileT;
char onameS[256];
sprintf(onameS,"InputTest_Lsol_XY_line4.txt");// 
ofileT=fopen(onameS,"w");
double fac=0.;
for(int i=0;i<inum;++i){

   rz=tdz[i]*fac;
   if(fac==0)tt[i]=0;//no Tkin dispersion
   if(fac!=0)tt[i]=tt[i]/(double)fac;// momentum dispersion
     xx=rz*sx+x0;
     yy=rz*sy+y0;
     zz=rz*sz+z0;
     dist->SetPoint(i,zz,xx,yy); 
     vxx=rz*svx+vx0;
     vyy=rz*svy+vy0;
     vzz=rz*svz+vz0;
   rrx[i]=tdx[i]*Minv[0][0]+tdy[i]*Minv[0][1]+0*Minv[0][2];
   rry[i]=tdx[i]*Minv[1][0]+tdy[i]*Minv[1][1]+0*Minv[1][2];
   rrz[i]=tdx[i]*Minv[2][0]+tdy[i]*Minv[2][1]+0*Minv[2][2];
//       printf("%d,%9.8e,%9.8e,%9.8e\n",i,tdx[i],tdy[i],tdz[i]);

   rrx[i]=rrx[i]+xx;
   rry[i]=rry[i]+yy;
   rrz[i]=rrz[i]+zz;
   //dist->SetPoint(i,rrz[i],rrx[i],rry[i]); 
       printf("%d,%9.8e,%9.8e,%9.8e\n",i,rrx[i],rry[i],rrz[i]);
 
   rrvx[i]=(tpx[i]*Minv[0][0]+tpy[i]*Minv[0][1]+0*Minv[0][2])*cV0[0];
   rrvy[i]=(tpx[i]*Minv[1][0]+tpy[i]*Minv[1][1]+0*Minv[1][2])*cV0[0];
   rrvz[i]=(tpx[i]*Minv[2][0]+tpy[i]*Minv[2][1]+0*Minv[2][2])*cV0[0];

   rrvx[i]=rrvx[i]+vxx;
   rrvy[i]=rrvy[i]+vyy;
   rrvz[i]=rrvz[i]+vzz;

       fprintf(ofileT,"%d,%d,%d,%9.8e,%9.8e,%9.8e,",i,0,0,rrx[i],rry[i],rrz[i]);
       fprintf(ofileT,"%9.8e,%9.8e,%9.8e,%9.8e,%9.8e,",rrvx[i],rrvy[i],rrvz[i],tt[i],0.);
       fprintf(ofileT,"%d,%9.8e,%9.8e,%9.8e,",0,x0,y0,z0);
       fprintf(ofileT,"%9.8e,%9.8e,%9.8e\n",vx0,vy0,vz0);
}
fclose(ofileT);

 TGraph *gr_yxcomp[2];
 TGraph *gr_yzcomp[2];
 TGraph *gr_yxpcomp[2];
 TGraph *gr_yypcomp[2];
 TGraph *gr_yzpcomp[2];
///////

  double tmpV2[3][100];
  for(int pp=0;pp<100;++pp){
     tmpV2[0][pp]=rrvx[pp]/(double)cV0[0];
     tmpV2[1][pp]=rrvy[pp]/(double)cV0[0];
     tmpV2[2][pp]=rrvz[pp]/(double)cV0[0];
   }
   gr_yxcomp[1]= new TGraph(100,rry,rrx);
   gr_yzcomp[1]= new TGraph(100,rry,rrz);
   gr_yxpcomp[1]= new TGraph(100,rry,tmpV2[0]);
   gr_yypcomp[1]= new TGraph(100,rry,tmpV2[1]);
   gr_yzpcomp[1]= new TGraph(100,rry,tmpV2[2]);

   gr_yxcomp[0]= new TGraph(20,posy[0],posx[0]);
   gr_yzcomp[0]= new TGraph(20,posy[0],posz[0]);
   gr_yxpcomp[0]= new TGraph(20,posy[0],tmpV2[0]);
   gr_yypcomp[0]= new TGraph(20,posy[0],tmpV2[1]);
   gr_yzpcomp[0]= new TGraph(20,posy[0],tmpV2[2]);

 TCanvas *c18 = new TCanvas("c18","test2",300,100,800,800);
 c18->Divide(3,2);
 TH2F *frame18[6];
 for(int i=0;i<6;++i){
//  lcom[i].SetTextSize(0.07);
  sprintf(fc,"frame18_%d",i);
  if(i==0)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, 80,100);//yx
  if(i==1)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, 40,60);//yz
  if(i==2)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, -1,1.0);//ypx
  if(i==3)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, -0.5,0.5);//ypy
  if(i==4)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, -0.2,0.5);//yyypz
  if(i==5)frame18[i] = new TH2F(fc," ",10, 130, 160, 10, -1,1);//yyypz
  frame18[i]->SetTitle("");
  frame18[i]->GetXaxis()->SetLabelSize(0.04);
  frame18[i]->GetXaxis()->SetTitleSize(0.05);
  frame18[i]->GetYaxis()->SetLabelSize(0.04);
  frame18[i]->GetYaxis()->SetTitleSize(0.06);
  if(i==0){
   frame18[0]->GetXaxis()->SetTitle("y[cm]");
   frame18[0]->GetYaxis()->SetTitle("x[cm]");
  }
  if(i==1){
   frame18[1]->GetXaxis()->SetTitle("y[cm]");
   frame18[1]->GetYaxis()->SetTitle("z[cm]");
  }
  if(i==2){
   frame18[2]->GetXaxis()->SetTitle("y[cm]");
   frame18[2]->GetYaxis()->SetTitle("vx[rad]");
  }
  if(i==3){
   frame18[3]->GetXaxis()->SetTitle("y[cm]");
   frame18[3]->GetYaxis()->SetTitle("vy[rad]");
  }
  if(i==4){
   frame18[4]->GetXaxis()->SetTitle("y[cm]");
   frame18[4]->GetYaxis()->SetTitle("vz[rad]");
  }
  frame18[i]->GetYaxis()->SetTitleOffset(1.5);
  frame18[i]->SetStats(0);

 }

 c18->cd(1);
 c18->SetLeftMargin(0.16);
 c18->SetRightMargin(0.05);
 frame18[0]->Draw();
printf("ppp\n");
 for(int i=0;i<2;++i){
   color=i+1;
   gr_yxcomp[i]->SetMarkerStyle(7);
   gr_yxcomp[i]->SetMarkerSize(1);
   gr_yxcomp[i]->SetMarkerColor(color);
   gr_yxcomp[i]->SetLineColor(color);
   gr_yxcomp[i]->Draw("p");
}


 c18->cd(2);
 c18->SetLeftMargin(0.16);
 c18->SetRightMargin(0.05);
 frame18[1]->Draw();
  for(int i=0;i<2;++i){
   color=i+1;
   gr_yzcomp[i]->SetMarkerStyle(7);
   gr_yzcomp[i]->SetMarkerSize(1);
   gr_yzcomp[i]->SetMarkerColor(color);
   gr_yzcomp[i]->SetLineColor(color);
   gr_yzcomp[i]->Draw("p");
  }
 c18->SetGridx();
 c18->SetGridy();
 
 c18->cd(3);
 c18->SetLeftMargin(0.16);
 c18->SetRightMargin(0.05);
 frame18[2]->Draw();
color=0;
for(int i=0;i<2;++i){
  color=i+1;
   gr_yxpcomp[i]->SetMarkerStyle(7);
   gr_yxpcomp[i]->SetMarkerSize(1);
   gr_yxpcomp[i]->SetMarkerColor(color);
   gr_yxpcomp[i]->SetLineColor(color);
   gr_yxpcomp[i]->Draw("p");
}
 c18->SetGridx();c18->SetGridy();


 c18->cd(4);
 c18->SetLeftMargin(0.16);
 c18->SetRightMargin(0.05);
 frame18[3]->Draw();

for(int i=0;i<2;++i){
 color=i+1;
   gr_yypcomp[i]->SetMarkerStyle(7);
   gr_yypcomp[i]->SetMarkerSize(1);
   gr_yypcomp[i]->SetMarkerColor(color);
   gr_yypcomp[i]->SetLineColor(color);
   gr_yypcomp[i]->Draw("p");
}
 c18->SetGridx();
 c18->SetGridy();

 c18->cd(5);
 c18->SetLeftMargin(0.16);
 c18->SetRightMargin(0.05);
 frame18[4]->Draw();

for(int i=0;i<2;++i){
 color=i+1;
   gr_yzpcomp[i]->SetMarkerStyle(7);
   gr_yzpcomp[i]->SetMarkerSize(1);
   gr_yzpcomp[i]->SetMarkerColor(color);
   gr_yzpcomp[i]->SetLineColor(color);
   gr_yzpcomp[i]->Draw("p");
}
 c18->SetGridx();
 c18->SetGridy();

/////////////////////////////////////////////////////
 TCanvas *cz = new TCanvas("cz","z-t",300,100,800,800);
 TH2F *frame;
  frame = new TH2F("frame"," ",10, -.2, .2, 10, -1E-2,1E-2);//yyypz
  frame->SetTitle("");
  frame->GetXaxis()->SetLabelSize(0.04);
  frame->GetXaxis()->SetTitleSize(0.05);
  frame->GetYaxis()->SetLabelSize(0.04);
  frame->GetYaxis()->SetTitleSize(0.06);
  frame->Draw("same");

  gr_zt->SetMarkerStyle(7);
  gr_zt->Draw("p");//cm rad




TCanvas *c3 = new TCanvas("c3","3-D track",200,100,700,800);

 TH3F *frame3;
   frame3 = new TH3F("frame3","test",10, -1+z0, 1+z0,10, -1+x0, 1+x0, 10,-1+y0,1+y0);
   //frame3 = new TH3F("frame3","test",10, -80, 80,10, -80, 80, 10,-0,200);
   frame3->GetXaxis()->SetLabelSize(0.04);
   frame3->GetXaxis()->SetTitleSize(0.04);
   frame3->GetYaxis()->SetLabelSize(0.04);
   frame3->GetYaxis()->SetTitleSize(0.04);
   frame3->GetZaxis()->SetLabelSize(0.04);
   frame3->GetZaxis()->SetTitleSize(0.04);
   frame3->GetXaxis()->SetTitle("x");
   frame3->GetYaxis()->SetTitle("y");
   frame3->GetZaxis()->SetTitle("z");
   frame3->GetXaxis()->SetTitleOffset(1.8);
   frame3->GetYaxis()->SetTitleOffset(1.5);
   frame3->GetZaxis()->SetTitleOffset(1.5);
   frame3->SetStats(0);
   frame3->Draw();


   c3->SetLeftMargin(0.15);
   c3->SetRightMargin(0.1);
   c3->SetBottomMargin(0.1);
   c3->Draw();

   tr0->SetMarkerStyle(20);
   tr0->SetMarkerColor(3);
//   tr0->Draw("same");
   trj_line->SetLineColor(2);
   trj_line->Draw("same");

   dist->SetMarkerStyle(4);
   dist->SetMarkerColor(4);
   dist->Draw("same");
//   tr0->Draw("same");

}



int FtoV(double A[4][4], double V[4],double VV[4]){
 int i,j;
 for(i=0;i<4;++i){
   VV[i]=0;  
   for(j=0;j<4;++j){
     VV[i]=VV[i]+A[i][j]*V[j];
   }
 }
 return 1;
}
int TransT4(double A[4][4], double B[4][4]){
 int i,j;
 for(i=0;i<4;++i){
  for(j=0;j<4;++j){
    B[j][i]=A[i][j];
  }
 }
 return 1;
}

double trace(double A[2][2]){
 double ans;
 ans=A[0][0]+A[1][1];
 return ans;
}
double det(double A[2][2]){
 double ans;
 ans=A[0][0]*A[1][1]-A[0][1]*A[1][0];
 return ans;
}
int TtoT(double A[2][2],double B[2][2],double C[2][2]){
 for(int i=0;i<2;++i){
    for(int j=0;j<2;++j){
       C[i][j]=0;
       for(int k=0;k<2;++k){
          C[i][j]=C[i][j]+A[i][k]*B[k][j];
       }
       printf("%lf ",C[i][j]);
    }
   printf("\n");
 }
 return 1;
}
int SumTwo(double A[2][2], double B[2][2],double C[2][2]){
 for(int i=0;i<2;++i){
    for(int j=0;j<2;++j){
       C[i][j]=0;
       C[i][j]=A[i][j]+B[i][j];
    }
 }
 return 1;
}
int DiffTwo(double A[2][2], double B[2][2],double C[2][2]){
 for(int i=0;i<2;++i){
    for(int j=0;j<2;++j){
       C[i][j]=0;
       C[i][j]=A[i][j]-B[i][j];
    }
 }
 return 1;
}
int FtoF(double A[4][4], double B[4][4],double C[4][4]){
 printf("FtoF-A*B---\n");

 printf("---A-------------\n");
 printf("%lf	%lf	%lf	%lf\n",A[0][0],A[0][1],A[0][2],A[0][3]);
 printf("%lf	%lf	%lf	%lf\n",A[1][0],A[1][1],A[1][2],A[1][3]);
 printf("%lf	%lf	%lf	%lf\n",A[2][0],A[2][1],A[2][2],A[2][3]);
 printf("%lf	%lf	%lf	%lf\n",A[3][0],A[3][1],A[3][2],A[3][3]);
 printf("---B-------------\n");
 printf("%lf	%lf	%lf	%lf\n",B[0][0],B[0][1],B[0][2],B[0][3]);
 printf("%lf	%lf	%lf	%lf\n",B[1][0],B[1][1],B[1][2],B[1][3]);
 printf("%lf	%lf	%lf	%lf\n",B[2][0],B[2][1],B[2][2],B[2][3]);
 printf("%lf	%lf	%lf	%lf\n",B[3][0],B[3][1],B[3][2],B[3][3]);

 
 printf("---A*B-------------\n");
 for(int i=0;i<4;++i){
    for(int j=0;j<4;++j){
       C[i][j]=0;
       for(int k=0;k<4;++k){
          C[i][j]=C[i][j]+A[i][k]*B[k][j];
       }
       printf("%lf ",C[i][j]);
    }
   printf("\n");
 }
 return 1;
}