//////////////functions////////////////
///calc emi////
int calcEmi(int evtNum,double x[128*160],double px[128*160],double y[128*160],double py[128*160],double Sigma[4][4]){
 for(int i=0;i<4;++i){
   for(int j=0;j<4;++j){
       Sigma[i][j]=0;
   }
 }
 for(int i=0;i<evtNum;++i){
  Sigma[0][0]=Sigma[0][0]+x[i]*x[i];
  Sigma[0][1]=Sigma[0][1]+x[i]*px[i];
  Sigma[0][2]=Sigma[0][2]+x[i]*y[i];
  Sigma[0][3]=Sigma[0][3]+x[i]*py[i];

  Sigma[1][0]=Sigma[1][0]+px[i]*x[i];
  Sigma[1][1]=Sigma[1][1]+px[i]*px[i];
  Sigma[1][2]=Sigma[1][2]+px[i]*y[i];
  Sigma[1][3]=Sigma[1][3]+px[i]*py[i];

  Sigma[2][0]=Sigma[2][0]+y[i]*x[i];
  Sigma[2][1]=Sigma[2][1]+y[i]*px[i];
  Sigma[2][2]=Sigma[2][2]+y[i]*y[i];
  Sigma[2][3]=Sigma[2][3]+y[i]*py[i];

  Sigma[3][0]=Sigma[3][0]+py[i]*x[i];
  Sigma[3][1]=Sigma[3][1]+py[i]*px[i];
  Sigma[3][2]=Sigma[3][2]+py[i]*y[i];
  Sigma[3][3]=Sigma[3][3]+py[i]*py[i];
 }

// printf("In Sigma[i][j]----\n");
 for(int i=0;i<4;++i){
   for(int j=0;j<4;++j){
    Sigma[i][j]=Sigma[i][j]/(double)evtNum;
//    printf("%e ",Sigma[i][j]);
   }
//   printf("\n");
 }

 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 FtoF4(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;
}

int Inverse4(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;
}
int FtoV4(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 ellipseTheta(double A, double B, double C){
 double ans;
  if(C!=A){
    ans=0.5*atan(2*B/(double)(C-A));
  }else{
    printf("ellipse ERROE\n");getchar();
  }
 return ans;
}
double ellipseMinor(double A, double B, double C, double KK){
 double ans;
 ans = 1/(double)(A+2*B*KK+C*KK*KK);
 return ans;
}
     
void thinkLine(){
 int bug=0;
 char iname[256];
 int i,j,k,idmy;
 double pi=TMath::Pi();
///////////////////////////
  char Name[256],sdmy[256];

  double ax=-2.16;double ay=-0.81;double a1=0.575;double a2=0.2;

FILE *ofileL;
double xL,yL,pyL,pxL;
double dtt=0;
int evtNum=10;
ofileL=fopen("inInjTwissL_2021miniSol_line4.txt","w");
 for(int i=0;i<evtNum;++i){
	 xL=-0.02+0.04*i/(double)evtNum;
	 yL=xL*a1;
	 pxL=xL*ax;
	 pyL=yL*ay;
    fprintf(ofileL,"%e,%e,%e,%e,%e,%e\n",xL*1E2,yL*1E2,pxL,pyL,dtt,dtt);//cm, rad
}
fclose(ofileL);


 double xl[128],yl[128],pxl[128],pyl[128];
 for(int i=0;i<evtNum;++i){
	 xl[i]=-0.02+0.04*i/(double)evtNum;
	 yl[i]=xl[i]*a1;
	 pxl[i]=xl[i]*ax;
	 pyl[i]=yl[i]*ay;
 }
 TGraph *gr_xl,*gr_yl,*gr_xyl,*gr_pxpyl;
 gr_xl=new TGraph(evtNum,xl,pxl);
 gr_yl=new TGraph(evtNum,yl,pyl);
 gr_xyl=new TGraph(evtNum,xl,yl);
 gr_pxpyl=new TGraph(evtNum,pxl,pyl);
//////////////
gROOT->SetStyle("Plain");
gROOT->ForceStyle();
gStyle->SetOptStat(0);
gStyle->SetCanvasBorderMode(0);
gStyle->SetPadColor(0);
gStyle->SetTitleFillColor(0);
gStyle->SetStatColor(0);

int color=0;

 TCanvas *c2 = new TCanvas("c2","Acceptance c2",200,100,600,600);
 c2->Divide(2,2);
///
 c2->cd(1);
 c2->cd(1)->SetLeftMargin(0.13);
 TH2F* frame2_1 = new TH2F("frame2_1"," ",10, -2E-2, 2E-2, 10, -4E-2, 4E-2);
 frame2_1->SetTitle("");
 frame2_1->GetXaxis()->SetLabelSize(0.04);
 frame2_1->GetXaxis()->SetTitleSize(0.05);
 frame2_1->GetYaxis()->SetLabelSize(0.04);
 frame2_1->GetYaxis()->SetTitleSize(0.05);
 frame2_1->GetXaxis()->SetTitle("X (m)");
 frame2_1->GetYaxis()->SetTitle("X' (rad)");
 frame2_1->GetYaxis()->SetTitleOffset(1.3);
 frame2_1->SetStats(0);
 frame2_1->Draw();

 TLegend *leg2_1; 
 leg2_1=new TLegend(0.7,0.7,0.9,0.9);
 leg2_1 -> SetTextSize(0.04);
 leg2_1->SetFillColor(0);


   gr_xl->SetLineColor(2);
   gr_xl->SetMarkerStyle(20);
   gr_xl->SetMarkerColor(2);
   gr_xl->Draw("lp");


c2->cd(1)->SetGridx();
c2->cd(1)->SetGridy();

 c2->cd(2);
 c2->cd(2)->SetLeftMargin(0.13);
 TH2F* frame2_2 = new TH2F("frame2_2"," ",10, -2E-2, 2E-2, 10, -2E-2, 2E-2);
 frame2_2->SetTitle("");
 frame2_2->GetXaxis()->SetLabelSize(0.04);
 frame2_2->GetXaxis()->SetTitleSize(0.05);
 frame2_2->GetYaxis()->SetLabelSize(0.04);
 frame2_2->GetYaxis()->SetTitleSize(0.05);
 frame2_2->GetXaxis()->SetTitle("Y (m)");
 frame2_2->GetYaxis()->SetTitle("Y' (rad)");
 frame2_2->GetYaxis()->SetTitleOffset(1.3);
 frame2_2->SetStats(0);
 frame2_2->Draw();

   gr_yl->SetLineColor(2);
   gr_yl->SetMarkerStyle(20);
   gr_yl->SetMarkerColor(2);
   gr_yl->Draw("lp");

c2->cd(2)->SetGridx();
c2->cd(2)->SetGridy();

///
///
 c2->cd(3);
 c2->cd(3)->SetLeftMargin(0.13);
 TH2F* frame2_3 = new TH2F("frame2_3"," ",10, -4E-2,4E-2, 10, -4E-2, 4E-2);
 frame2_3->SetTitle("");
 frame2_3->GetXaxis()->SetLabelSize(0.04);
 frame2_3->GetXaxis()->SetTitleSize(0.05);
 frame2_3->GetYaxis()->SetLabelSize(0.04);
 frame2_3->GetYaxis()->SetTitleSize(0.05);
 frame2_3->GetXaxis()->SetTitle("X (m)");
 frame2_3->GetYaxis()->SetTitle("Y (m)");
 frame2_3->GetYaxis()->SetTitleOffset(1.3);
 frame2_3->SetStats(0);
 frame2_3->Draw();

   gr_xyl->SetLineColor(2);
   gr_xyl->SetMarkerStyle(20);
   gr_xyl->SetMarkerColor(2);
   gr_xyl->Draw("lp");


c2->cd(3)->SetGridx();
c2->cd(3)->SetGridy();

 c2->cd(4);
 c2->cd(4)->SetLeftMargin(0.13);
 TH2F* frame2_4 = new TH2F("frame2_4"," ",10, -4E-2, 4E-2, 10, -4E-2, 4E-2);
 frame2_4->SetTitle("");
 frame2_4->GetXaxis()->SetLabelSize(0.04);
 frame2_4->GetXaxis()->SetTitleSize(0.05);
 frame2_4->GetYaxis()->SetLabelSize(0.04);
 frame2_4->GetYaxis()->SetTitleSize(0.05); 
 frame2_4->GetXaxis()->SetTitle("X' (rad)"); 
 frame2_4->GetYaxis()->SetTitle("Y' (rad)"); 
 frame2_4->GetYaxis()->SetTitleOffset(1.3);
 frame2_4->SetStats(0);
 frame2_4->Draw();


   gr_pxpyl->SetLineColor(2);
   gr_pxpyl->SetMarkerStyle(20);
   gr_pxpyl->SetMarkerColor(2);
   gr_pxpyl->Draw("lp");

c2->cd(4)->SetGridx();
c2->cd(4)->SetGridy();



}