#include <TRandom.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#define N0 1028*100
int FtoV4(double A[4][4], double V[4],double VV[4]){
 int i,j;
 int nn=4;int mm=4;
 for(i=0;i<nn;++i){
   VV[i]=0;
   for(j=0;j<mm;++j){
     VV[i]=VV[i]+A[i][j]*V[j];
   }
 }
 return 1;
}


int normalQ(double K1, double Len, double Qmat[4][4]){
  double a1,a2,a3;
  double b1,b2,b3;
/*
// text book, etc
 if(K1>0){
   a1=cos(sqrt(K1)*Len);
   a2=sin(sqrt(K1)*Len)/(double)sqrt(K1);
   a3=-1*sqrt(K1)*sin(sqrt(K1)*Len);
   b1=cosh(sqrt(K1)*Len);
   b2=sinh(sqrt(K1)*Len)/(double)sqrt(K1);
   b3=sqrt(K1)*sinh(sqrt(K1)*Len);
 }else{
   K1=K1*-1;
   b1=cos(sqrt(K1)*Len);
   b2=sin(sqrt(K1)*Len)/(double)sqrt(K1);
   b3=-1*sqrt(K1)*sin(sqrt(K1)*Len);
   a1=cosh(sqrt(K1)*Len);
   a2=sinh(sqrt(K1)*Len)/(double)sqrt(K1);
   a3=sqrt(K1)*sinh(sqrt(K1)*Len);
 }
*/
//SAD integral-K
 if(K1>0){
  a1=cos(sqrt(K1*Len));
  a2=sin(sqrt(K1*Len))*sqrt(Len/(double)K1);
  a3=-1*sin(sqrt(K1*Len))*sqrt(K1/(double)Len);
  b1=cosh(sqrt(K1*Len));
  b2=sinh(sqrt(K1*Len))*sqrt(Len/(double)K1);
  b3=sinh(sqrt(K1*Len))*sqrt(K1/(double)Len);
 }else{
  K1=K1*-1;
  b1=cos(sqrt(K1*Len));
  b2=sin(sqrt(K1*Len))*sqrt(Len/(double)K1);
  b3=-1*sin(sqrt(K1*Len))*sqrt(K1/(double)Len);
  a1=cosh(sqrt(K1*Len));
  a2=sinh(sqrt(K1*Len))*sqrt(Len/(double)K1);
  a3=sinh(sqrt(K1*Len))*sqrt(K1/(double)Len);
 }


 Qmat[0][0]=a1;Qmat[0][1]=a2;Qmat[0][2]=0;Qmat[0][3]=0;
 Qmat[1][0]=a3;Qmat[1][1]=a1;Qmat[1][2]=0;Qmat[1][3]=0;
 Qmat[2][0]=0;Qmat[2][1]=0;Qmat[2][2]=b1;Qmat[2][3]=b2;
 Qmat[3][0]=0;Qmat[3][1]=0;Qmat[3][2]=b3;Qmat[3][3]=b1;
/*
 printf("matrix normal---\n");
 printf("%lf    %lf     %lf     %lf\n",Qmat[0][0],Qmat[0][1],Qmat[0][2],Qmat[0][3]);
 printf("%lf    %lf     %lf     %lf\n",Qmat[1][0],Qmat[1][1],Qmat[1][2],Qmat[1][3]);
 printf("%lf    %lf     %lf     %lf\n",Qmat[2][0],Qmat[2][1],Qmat[2][2],Qmat[2][3]);
 printf("%lf    %lf     %lf     %lf\n",Qmat[3][0],Qmat[3][1],Qmat[3][2],Qmat[3][3]);
 getchar();
*/
 return 1;
}
int skewQ(double K1, double Len, double theta,double SQmat[4][4]){
  double a1,a2,a3;
  double b1,b2,b3;
  double pi=TMath::Pi();
// text book, etc
/*
 if(K1>0){
   a1=cos(sqrt(K1)*Len);
   a2=sin(sqrt(K1)*Len)/(double)sqrt(K1);
   a3=-1*sqrt(K1)*sin(sqrt(K1)*Len);
   b1=cosh(sqrt(K1)*Len);
   b2=sinh(sqrt(K1)*Len)/(double)sqrt(K1);
   b3=sqrt(K1)*sinh(sqrt(K1)*Len);
 }else{
   K1=K1*-1;
   b1=cos(sqrt(K1)*Len);
   b2=sin(sqrt(K1)*Len)/(double)sqrt(K1);
   b3=-1*sqrt(K1)*sin(sqrt(K1)*Len);
   a1=cosh(sqrt(K1)*Len);
   a2=sinh(sqrt(K1)*Len)/(double)sqrt(K1);
   a3=sqrt(K1)*sinh(sqrt(K1)*Len);
 }
*/
//SAD integral-K
 if(K1>0){
  a1=cos(sqrt(K1*Len));
  a2=sin(sqrt(K1*Len))*sqrt(Len/(double)K1);
  a3=-1*sin(sqrt(K1*Len))*sqrt(K1/(double)Len);
  b1=cosh(sqrt(K1*Len));
  b2=sinh(sqrt(K1*Len))*sqrt(Len/(double)K1);
  b3=sinh(sqrt(K1*Len))*sqrt(K1/(double)Len);
 }else{
  K1=K1*-1;
  b1=cos(sqrt(K1*Len));
  b2=sin(sqrt(K1*Len))*sqrt(Len/(double)K1);
  b3=-1*sin(sqrt(K1*Len))*sqrt(K1/(double)Len);
  a1=cosh(sqrt(K1*Len));
  a2=sinh(sqrt(K1*Len))*sqrt(Len/(double)K1);
  a3=sinh(sqrt(K1*Len))*sqrt(K1/(double)Len);
 }


 theta=pi+theta*pi/(double)180;
 double cos_t=cos(theta);
 double sin_t=sin(theta);
 SQmat[0][0]=a1*cos_t*cos_t+b1*sin_t*sin_t;
 SQmat[0][1]=a2*cos_t*cos_t+b2*sin_t*sin_t;
 SQmat[0][2]=cos_t*sin_t*(b1-a1);
 SQmat[0][3]=cos_t*sin_t*(b2-a2);

 SQmat[1][0]=a3*cos_t*cos_t+b3*sin_t*sin_t;
 SQmat[1][1]=a1*cos_t*cos_t+b1*sin_t*sin_t;
 SQmat[1][2]=cos_t*sin_t*(b3-a3);
 SQmat[1][3]=cos_t*sin_t*(b1-a1);

 SQmat[2][0]=cos_t*sin_t*(b1-a1);
 SQmat[2][1]=cos_t*sin_t*(b2-a2);
 SQmat[2][2]=a1*sin_t*sin_t+b1*cos_t*cos_t;
 SQmat[2][3]=a2*sin_t*sin_t+b2*cos_t*cos_t;

 SQmat[3][0]=cos_t*sin_t*(b3-a3);
 SQmat[3][1]=cos_t*sin_t*(b1-a1);
 SQmat[3][2]=a3*sin_t*sin_t+b3*cos_t*cos_t;
 SQmat[3][3]=a1*sin_t*sin_t+b1*cos_t*cos_t;
/*
 printf("SQmatrix---\n");
 printf("%lf	%lf	%lf	%lf\n",SQmat[0][0],SQmat[0][1],SQmat[0][2],SQmat[0][3]);
 printf("%lf	%lf	%lf	%lf\n",SQmat[1][0],SQmat[1][1],SQmat[1][2],SQmat[1][3]);
 printf("%lf	%lf	%lf	%lf\n",SQmat[2][0],SQmat[2][1],SQmat[2][2],SQmat[2][3]);
 printf("%lf	%lf	%lf	%lf\n",SQmat[3][0],SQmat[3][1],SQmat[3][2],SQmat[3][3]);
 getchar(); 
*/
 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 FtoF4(double A[4][4], double B[4][4],double C[4][4]){

 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];
       }
    }
 }
 return 1;
}

int determinant(double aa[4][4],double *det){
*det=1.0;
double buf;
int n=4;  //配列の次数
int i,j,k;

double a[4][4];
for(i=0;i<n;i++){
 for(j=0;j<n;j++){
    a[i][j]=aa[i][j];
 }
}
//三角行列を作成
for(i=0;i<n;i++){
 for(j=0;j<n;j++){
  if(i<j){
   buf=a[j][i]/a[i][i];
   for(k=0;k<n;k++){
   a[j][k]-=a[i][k]*buf;
   }
  }
 }
}
//対角部分の積
for(i=0;i<n;i++){
 (*det)*=a[i][i];
}

printf("inner det=%e\n",*det);
 return 1;
}
void readSADMateach2021_20220628(int now){ 
 double pi=TMath::Pi();
///
//////////////////////SAD///////////
 double MAT[32][4][4],MAT_inv[4][4];
 FILE *infile;
 double readD,readD1,readD2,readD3;
 double readD4,readD5,readD6,readD7;
 char iname[256],dmyc,sdmy[256];
 int typ=0;
 int id=5;
 //if(id==5)sprintf(iname,"nakayama/TransferMatrix_ALL_Oct25_16_1.txt");
 if(id==5)sprintf(iname,"testTransferMatrix_ALL_Oct25_16_1.txt");
 if(id==4)sprintf(iname,"TransferMatrix_ALL_test_2021June10_h.txt");
 if(id==3)sprintf(iname,"TransferMatrix_ALL_test_2021Aug7_7h.txt");
// if(id==3)sprintf(iname,"TransferMatrix_ALL_test_2021Aug7_6h.txt");
 if(id==2)sprintf(iname,"TransferMatrix_ALL_test_MayJune_2_h.txt");
 if(id==0)sprintf(iname,"TransferMatrix_ALL_test_MayJune_0_h.txt");
 if(id==1){
	 //sprintf(iname,"TransferMatrix_ALL_test_MayJune_1_h.txt");
	 sprintf(iname,"TransferMatrix_ALL_test_June10_2h.txt");//mag 0-23
	 //sprintf(iname,"TransferMatrix_ALL_test_June10_2_org_h.txt");
	 typ=1;
 }
 infile=fopen(iname,"r");
 printf("now is %s\n",iname);
 //MAT
 int mag=1;
 if(typ==1)mag=25;
 if(typ!=1)mag=25;
 if(id==3)mag=25;
 if(id==4)mag=27;
 if(id==5)mag=27;
 if(now>(mag))now=mag;
 for(int k=0;k<mag;++k){
	if(k>0){fscanf(infile,"%c",&dmyc);printf("%c0\n",dmyc);}
   for(int i=0;i<4;++i){
	if(i>0){fscanf(infile,"%c",&dmyc);printf("%c1\n",dmyc);}
	fscanf(infile,"%c",&dmyc);printf("%c1\n",dmyc);
	fscanf(infile,"%c",&dmyc);printf("%c2\n",dmyc);
        fscanf(infile,"%lf,%lf,%lf,%lf",&readD,&readD1,&readD2,&readD3);
	printf("*%lf %lf %lf %lf*\n",readD,readD1,readD2,readD3);
        fscanf(infile,"%c",&dmyc);printf("%c\n",dmyc);
        fscanf(infile,"%c",&dmyc);printf("%c\n",dmyc);
	MAT[k][i][0]=readD;MAT[k][i][1]=readD1;MAT[k][i][2]=readD2;MAT[k][i][3]=readD3;
	printf("*%lf %lf %lf %lf*\n",MAT[k][i][0],MAT[k][i][1],MAT[k][i][2],MAT[k][i][3]);
//	printf("--------\n");
   } 
   printf("------MAT k=%d END-----\n",k);//getchar();
 }
 printf("------MAT END-----\n");//getchar();
   fscanf(infile,"%c",&dmyc);printf("%c -1\n",dmyc);
//Twiss READ---------------
 for(int k=0;k<mag;++k){
   fscanf(infile,"%c",&dmyc);printf("%c0\n",dmyc);
   fscanf(infile,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",&readD,&readD1,&readD2,&readD3,&readD4,&readD5,&readD6,&readD7);
   printf("readD=%3.2f %3.2f %3.2f %3.2f: %3.2f %3.2f %3.2f %3.2f:DETR=%lf\n",readD,readD1,readD2,readD3,readD4,readD5,readD6,readD7,readD4*readD7-readD5*readD6);
   fscanf(infile,"%c",&dmyc);printf("%c1\n",dmyc);
   fscanf(infile,"%c",&dmyc);printf("%c2\n",dmyc);//getchar();
 }
 fclose(infile);

 for(int k=0;k<mag;++k){
   printf("k=%d-------\n",k);
   for(int i=0;i<4;++i){
	printf("%lf %lf %lf %lf\n",MAT[k][i][0],MAT[k][i][1],MAT[k][i][2],MAT[k][i][3]);
   }
   printf("--------\n");//getchar();
 }
		   
   double test[32][4][4];
   FtoF4(MAT[1],MAT[0],test[0]);
   FtoF4(MAT[2],test[0],test[1]);
   FtoF4(MAT[3],test[1],test[2]);
   FtoF4(MAT[4],test[2],test[3]);
   FtoF4(MAT[5],test[3],test[4]);
   FtoF4(MAT[6],test[4],test[5]);
   FtoF4(MAT[7],test[5],test[6]);
   FtoF4(MAT[8],test[6],test[7]);
   FtoF4(MAT[9],test[7],test[8]);
   FtoF4(MAT[10],test[8],test[9]);
   FtoF4(MAT[11],test[9],test[10]);
   FtoF4(MAT[12],test[10],test[11]);
   FtoF4(MAT[13],test[11],test[12]);
   FtoF4(MAT[14],test[12],test[13]);
   FtoF4(MAT[15],test[13],test[14]);
   FtoF4(MAT[16],test[14],test[15]);
   FtoF4(MAT[17],test[15],test[16]);
   FtoF4(MAT[18],test[16],test[17]);
   FtoF4(MAT[19],test[17],test[18]);
   FtoF4(MAT[20],test[18],test[19]);
   FtoF4(MAT[21],test[19],test[20]);
   FtoF4(MAT[22],test[20],test[21]);
   FtoF4(MAT[23],test[21],test[22]);
   if(mag>24)FtoF4(MAT[24],test[22],test[23]);
   if(mag>25)FtoF4(MAT[25],test[23],test[24]); 
    
   
   printf("----------MAt*MAT*------------\n");
   for(int i=0;i<4;++i){
	   for(int j=0;j<4;++j){
		   printf("%lf ",test[now][i][j]);
	   }
	   printf("\n");
   }
   printf("----------MAT[mag_tot] ^^^--> $$$------------\n");
   for(int i=0;i<4;++i){
	   for(int j=0;j<4;++j){
		   printf("%lf ",MAT[mag-1][i][j]);
	   }
	   printf("\n");
   }
TH1D *h_x,*h_y;
h_x=new TH1D("h_x","",100,-0.1,0.1);//m
h_y=new TH1D("h_y","",100,-0.1,0.1);//m
   
double x0[N0],y0[N0],px0[N0],py0[N0];
double x1[N0],y1[N0],px1[N0],py1[N0];
 double VecIn[4],VecOut[4];
 double epsx0=1.13E-7*1.5;
 double epsy0=1.03E-7*1.5;
 double beta0=10;
 double gamma0=1/(double)beta0;//alpha=0
 double sigma[4]={sqrt(epsx0*beta0),sqrt(epsx0*gamma0),sqrt(epsy0*beta0),sqrt(epsy0*gamma0)};
 int evtNum=1E4;
 for(int i=0;i<evtNum;++i){
    x0[i]=gRandom->Gaus(0,sigma[0]);
    px0[i]=gRandom->Gaus(0,sigma[1]);
    y0[i]=gRandom->Gaus(0,sigma[2]);
    py0[i]=gRandom->Gaus(0,sigma[3]);

    VecIn[0]=x0[i];
    VecIn[1]=px0[i];
    VecIn[2]=y0[i];
    VecIn[3]=py0[i];
    FtoV4(test[now],VecIn,VecOut);//uptp22 _0,_2 uptp24 _1
    //FtoV4(MAT[mag-1],VecIn,VecOut);//^^^$$$
    x1[i]=VecOut[0]; //--> output for monitor
    px1[i]=VecOut[1]; //--> output for monitor
    y1[i]=VecOut[2]; //--> output for monitor
    py1[i]=VecOut[3]; //--> output for monitor
    h_x->Fill(x1[i]);
    h_y->Fill(y1[i]);
 }
 double rmsx,rmsy;
 rmsx=h_x->GetRMS();
 rmsy=h_y->GetRMS();
 printf("rmsx=%lf rmsy=%lf\n",rmsx,rmsy);
 FILE *ofile;
 char oname[256];
 if(id==5)sprintf(oname,"outRMS_Oct25_16_1.txt");
 if(id!=5)sprintf(oname,"outRMS_Aug7_7.txt");
 //sprintf(oname,"outRMS_Aug7_6.txt");
 ofile=fopen(oname,"a");
 fprintf(ofile,"%d,%e,%e\n",now,rmsx,rmsy);
 //////Graph
gROOT->SetStyle("Plain");
gROOT->ForceStyle();
gStyle->SetOptStat(0);
gStyle->SetCanvasBorderMode(0);
gStyle->SetPadColor(0);
gStyle->SetTitleFillColor(0);
gStyle->SetStatColor(0);

 gStyle->SetCanvasBorderMode(0);//remove yellow line
 gStyle->SetCanvasColor(0);
 gStyle->SetFrameLineColor(1);
 gStyle->SetPadColor(0);
 gStyle->SetTitleFillColor(0);
 gStyle->SetStatColor(0);

 TGraph *gr_xxp[2],*gr_yyp[2];
 TGraph *gr_xy[2],*gr_xpyp[2];

 gr_xxp[0]=new TGraph(evtNum,x0,px0);
 gr_yyp[0]=new TGraph(evtNum,y0,py0);
 gr_xy[0]=new TGraph(evtNum,x0,y0);
 gr_xpyp[0]=new TGraph(evtNum,px0,py0);
 gr_xxp[1]=new TGraph(evtNum,x1,px1);
 gr_yyp[1]=new TGraph(evtNum,y1,py1);
 gr_xy[1]=new TGraph(evtNum,x1,y1);
 gr_xpyp[1]=new TGraph(evtNum,px1,py1);

 TCanvas *cxy = new TCanvas("cxy","check ",200,100,800,800);
 cxy->Divide(2,2);
 cxy->cd(1);
 cxy->cd(1)->SetLeftMargin(0.1);
// cxy->SetRightMargin(0.1);
 TH2F* framexy = new TH2F("framet"," ",10, -1E-1, 1E-1, 100, -1E-1, 1E-1);
 framexy->SetTitle("");
 framexy->GetXaxis()->SetLabelSize(0.04);
 framexy->GetXaxis()->SetTitleSize(0.05);
 framexy->GetYaxis()->SetLabelSize(0.04);
 framexy->GetYaxis()->SetTitleSize(0.05);
 framexy->GetXaxis()->SetTitle("x[m]");
 framexy->GetYaxis()->SetTitle("x'[rad]");
 framexy->GetYaxis()->SetTitleOffset(1.0);
 framexy->SetStats(0);
 framexy->Draw();

 gr_xxp[0]->SetMarkerStyle(6);
 gr_xxp[0]->SetMarkerColor(1);
 gr_xxp[0]->Draw("p");

 gr_xxp[1]->SetMarkerStyle(6);
 gr_xxp[1]->SetMarkerColor(2);
 gr_xxp[1]->Draw("p");
 cxy->cd(1)->SetGrid();

 cxy->cd(2);
 cxy->cd(2)->SetLeftMargin(0.1);
// cxy->SetRightMargin(0.1);
 TH2F* framexy2 = new TH2F("frame2"," ",10, -1E-1, 1E-1, 100, -1E-1, 1E-1);
 framexy2->SetTitle("");
 framexy2->GetXaxis()->SetLabelSize(0.04);
 framexy2->GetXaxis()->SetTitleSize(0.05);
 framexy2->GetYaxis()->SetLabelSize(0.04);
 framexy2->GetYaxis()->SetTitleSize(0.05);
 framexy2->GetXaxis()->SetTitle("y[m]");
 framexy2->GetYaxis()->SetTitle("y'[rad]");
 framexy2->GetYaxis()->SetTitleOffset(1.0);
 framexy2->SetStats(0);
 framexy2->Draw();

 gr_yyp[0]->SetMarkerStyle(6);
 gr_yyp[0]->SetMarkerColor(1);
 gr_yyp[0]->Draw("p");

 gr_yyp[1]->SetMarkerStyle(6);
 gr_yyp[1]->SetMarkerColor(2);
 gr_yyp[1]->Draw("p");
 cxy->cd(2)->SetGrid();

 cxy->cd(3);
 cxy->cd(3)->SetLeftMargin(0.15);
// cxy->SetRightMargin(0.1);
 //TH2F* framexy3 = new TH2F("frame3"," ",10, -.4E-1, .4E-1, 100, -.4E-1, .4E-1);
 //TH2F* framexy3 = new TH2F("frame3"," ",10, -1E-1, 1E-1, 100, -1E-1, 1E-1);
 TH2F* framexy3 = new TH2F("frame3"," ",10, -2E-2, 2E-2, 100, -2E-2, 2E-2);
 framexy3->SetTitle("");
 framexy3->GetXaxis()->SetLabelSize(0.04);
 framexy3->GetXaxis()->SetTitleSize(0.05);
 framexy3->GetYaxis()->SetLabelSize(0.04);
 framexy3->GetYaxis()->SetTitleSize(0.05);
 framexy3->GetXaxis()->SetTitle("x[m]");
 framexy3->GetYaxis()->SetTitle("y[m]");
 framexy3->GetYaxis()->SetTitleOffset(1.5);
 framexy3->SetStats(0);
 framexy3->Draw();

 gr_xy[0]->SetMarkerStyle(6);
 gr_xy[0]->SetMarkerColor(1);
 gr_xy[0]->Draw("p");

 gr_xy[1]->SetMarkerStyle(6);
 gr_xy[1]->SetMarkerColor(2);
 gr_xy[1]->Draw("p");
 cxy->cd(3)->SetGrid();

 cxy->cd(4);
 cxy->cd(4)->SetLeftMargin(0.1);
// cxy->SetRightMargin(0.1);
 TH2F* framexy4 = new TH2F("frame4"," ",10, -1E-1, 1E-1, 100, -1E-1, 1E-1);
 framexy4->SetTitle("");
 framexy4->GetXaxis()->SetLabelSize(0.04);
 framexy4->GetXaxis()->SetTitleSize(0.05);
 framexy4->GetYaxis()->SetLabelSize(0.04);
 framexy4->GetYaxis()->SetTitleSize(0.05);
 framexy4->GetXaxis()->SetTitle("x'[rad]");
 framexy4->GetYaxis()->SetTitle("y'[rad]");
 framexy4->GetYaxis()->SetTitleOffset(1.0);
 framexy4->SetStats(0);
 framexy4->Draw();

 gr_xpyp[0]->SetMarkerStyle(6);
 gr_xpyp[0]->SetMarkerColor(1);
 gr_xpyp[0]->Draw("p");

 gr_xpyp[1]->SetMarkerStyle(6);
 gr_xpyp[1]->SetMarkerColor(2);
 gr_xpyp[1]->Draw("p");
 cxy->cd(4)->SetGrid();
/*
 char gname[256];
 sprintf(gname,"MayJune_%d_%d.gif",id,now);
 cxy->SaveAs(gname);
 */
}