/******************************************************************************
* FILE: SpinResTracker.cc
* DESCRIPTION:  
*   Integrating Spinor form of T-BMT equation with two snakes
*   using single Spin Resonance
* AUTHOR: Vahid H. Ranjbar
* LAST REVISED: 05/25/2021
******************************************************************************/
#include <iostream>     
#include <complex> 
#include <stdio.h>
#include <stdlib.h>
#include <ctime>
#include <fstream>
#include <mpi.h>

const double PI = acos(-1.);
const double TOL = 1.e-10;


// One turn map propagating spinor Zout from TurnNo to TurnNo+1 performed over 2*Steps integration steps using resonance strengths Res[]
// at spin resonance Ggamma energies K[]
// One turn Map transforming to Interaction Frame before and after snakes
void OneTurnIF( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4]);
// One turn Map transforming to Resonance Frame before and after snakes
void OneTurnRF( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4]);
// One turn Map without transforming (all transformations done in single step)
void OneTurnAN( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4]);


// Transport over single spin resonance
void ResTM( std::complex<double> Res, double K, double Ggamma, double F, double O, std::complex<double> Zout[4]);
// Applies 4th order Magnus Gaussian quadrature
void FourthO( double h, double t, std::complex<double> Res[7], double K[7],double Ggamma, std::complex<double> Zout[4]);
// Calculates value of matrix in interaction frame with Xi= Sum_i Exp[i(Ggamma-K[i])*t]Res[i] with M11=0, M12= j/2 Xi, M21 = j/2 Xi^* , M22 = 0
void A_IFrame(std::complex<double> Res[7], double K[7], double Ggamma, double t,std::complex<double> Zout[4]);
// Calculates the matrix given in Resonance Frame with Xi= Sum_i Exp[i(K[0]-K[i])*t]Res[i] with M11= j*(K[0]-Ggamma)/2, M12= -j/2 Xi
void A_RFrame(std::complex<double> Res[7], double K[7], double Ggamma, double t, std::complex<double> Zout[4]);

// add matrix X + Y = Zout
void add1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4]);
// minus matrix X - Y = Zout
void minus1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4]);
// matrix matrix dot X * Y = Zout
void dot1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4]);
// matrix vector dot X * Y = Zout
void dot2( std::complex<double> X[4], std::complex<double> Y[2], std::complex<double> Zout[2]);
// multiply scaler times matrix X * y = Zout
void mult1( std::complex<double> X[4], std::complex<double> y, std::complex<double> Zout[4]);
// Exponentiate Matrix exp(A) = Zout
void expM( std::complex<double> A[4], std::complex<double> Zout[4]);
// Magnus Omega 2
void OM2(std::complex<double> Rs[7], double K[7], double Ggamma, double t, std::complex<double> OM2[4]);
// Magnus Omega 1
void OM1(std::complex<double> Res[7], double K[7], double Ggamma, double t, double h, std::complex<double> OM1[4]);
// Interaction Frame Exact Integration
void Wag( double h, double t0, std::complex<double> Res[7], double K[7],double Ggamma, std::complex<double> Zout[4]);
// <Sy> Envelope  Estimate function for single resonance
void SAmp(double Ggamma, double K2, double ARes, double SA[1]);


int main (int argc, char *argv[]) 
{
  int rank=0;
  FILE * myfile;
  char filename[50];

 


  
   // fixed inputs
 
   
   const double Cir = 3833.85; //Circumference of RHIC
   const double clight = 299792458; //speed of light
 
  //default values
   int offset=0;
  int Steps = 10;  // integration steps 0 to PI
  int NN = 1; // number of particles distributed evenly over betatron phase space.
  long long int NT =  670000; // number of turns
  int DumpStep = 1; // number of turns between each particle dump
  int nstrobe = 40000; // number of turns used for stroboscopic averaging
  double Ggam0 = 414.8 ;// Initial energy in Ggamma
  double alpha = 3.74118e-6; // acceleration rate d Ggamma/d \theta in RHIC
  double Vtune0 = 29.67; // vertical tune
  double Cv = 2;  // vertical chromaticity
  double eta =0.0001889; // momentum compaction factor in RHIC
  double qs = 8.9e-05; // synchrotron tune in RHIC
  double tau0 = 5.0e-9; // initial time offset in from bucket center in seconds
  double Amp = 1.0; // initial intrinsic resonance multiplicative factor 
  
  double w0R,w1R,w2R,w3R,w4R,w5R,w6R;
  double w0I,w1I,w2I,w3I,w4I,w5I,w6I;
  double k0,k1,k2,k3,k4,k5,k6;
  
   // comment out if not using MPI
  int numprocs;
MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &numprocs);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);
   if (rank == 0) {
    std::cout << "Using " << numprocs << " MPI processes." << std::endl;
   }
   NN = numprocs;
   ////////


  

  // Reading in Resonances strength and energies from file Resonance.in
  std::ifstream configInput("Resonance.in");
  std::string dummy;
  configInput >> dummy >> k0;
  configInput >> dummy >> w0R >> w0I;
  configInput >> dummy >> k1;
  configInput >> dummy >> w1R >> w1I;
  configInput >> dummy >> k2;
  configInput >> dummy >> w2R >> w2I;
  configInput >> dummy >> k3;
  configInput >> dummy >> w3R >> w3I;
  configInput >> dummy >> k4;
  configInput >> dummy >> w4R >> w4I;
  configInput >> dummy >> k5;
  configInput >> dummy >> w5R >> w5I;
  configInput >> dummy >> k6;
  configInput >> dummy >> w6R >> w6I;
  configInput.close();

  // printing out values read to screen 
   
  std::cout << "input resonances are: \n"
	     << "K0= " << k0<< "w0 =" << w0R << " " << w0I << "\n"
     << "K1= " << k1 << "w1 =" << w1R << " " << w1I << "\n"
<< "K2= " << k2 << "w2 =" << w2R << " " << w2I << "\n"
<< "K3= " << k3 << "w3 =" << w3R << " " << w3I << "\n"
<< "K4= " << k4 << "w4 =" << w4R << " " << w4I << "\n"
<< "K5= " << k5 << "w5 =" << w5R << " " << w5I << "\n"
<< "K6= " << k6 << "w6 =" << w6R << " " << w6I << "\n";
 

 // command line inputs
for (int i = 1; i < argc; ++i) {
  
  std::string arg = argv[i];
 
  if(arg=="Ggam0=") Ggam0=strtod(argv[i+1], NULL);
  if(arg=="alpha=") alpha=strtod(argv[i+1], NULL);
  if(arg=="Vtune0=") Vtune0=strtod(argv[i+1], NULL);
  if(arg=="Cv=") Cv=strtod(argv[i+1], NULL);
  if(arg=="qs=") qs=strtod(argv[i+1], NULL);
  if(arg=="eta=") eta=strtod(argv[i+1], NULL);
  if(arg=="Amp=") Amp=strtod(argv[i+1], NULL);
  if(arg=="tau0=") tau0=strtod(argv[i+1], NULL);
  if(arg=="NN=") NN=atoi(argv[i+1]);
  if(arg=="NT=") {NT= (int long long) strtod(argv[i+1],NULL); }
  if(arg=="DumpStep=") DumpStep=atoi(argv[i+1]);
  if(arg=="nstrobe=") nstrobe=atoi(argv[i+1]);
  if(arg=="rank=") rank=atoi(argv[i+1]);
  if(arg=="offset=") offset = atoi(argv[i+1]);
  if(arg=="Steps=") Steps= atoi(argv[i+1]);

 }

 

 
 std::cout << "input variables are: \n"
	     << "Ggam0 = " << Ggam0 << " alpha= " <<alpha << "Vtune0= " << Vtune0 << "\n"
	     << "Cv= " <<Cv << " qs= " <<qs << " eta= " <<eta <<" tau0= " <<tau0 << "\n"
	     << "Amp= " << Amp <<  "\n"
	     << "NN = " << NN <<" NT= " << NT << " DumpStep= " << DumpStep << " nstrobe = " << nstrobe << "\n";
   
      


 

// derived constants
  int Np = NT/DumpStep;
  double z0 = tau0*clight;
  double f0 = clight/Cir;
  double w0 = f0*2*PI;
  double ws = w0*qs;
  
  // setting up complex variables and Identity matrix
  std::complex<double> j,Id[4];
  
  j = std::complex<double> (0.0 , 1.0) ;
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);
 
  
  // offseting rank for multiple runs
  rank = rank + offset;
  
  
  //  #pragma omp parallel private(rank) shared(SSY,DAvg) num_threads(NN)  
    // {
  std::complex<double>  nadd,ni,phase0, phase,ww0[7],ww[7],T[4],Tf[4],T0[4],T1[4],SArray[2],SAout[2];
  std::complex<double>  t11,t12,t21,t22,tf11,tf12,tf21,tf22,u1,d1,uf,df,ut,dt,TP[4];
  double a,c,b,d,af,cf,bf,t0,t1,t2,t3,R11,R12,R13,R21,R22,R23,R31,R32,R33,dp,SY;
  double nx,ny,nz,norm3,nus,nusf,n1,n2,n3,nf1,nf2,nf3,norm,normf,b1,b2,b3,b1f,b2f,b3f,b3fAVG,SYAVG;
  double b2fAVG,b1fAVG,dif3,dif3AVG;
  double nn,K[7],Ggam,GgamF,unit,unitavg;
  int NAmp,kp;
  double NC = NN;
  //rank = omp_get_thread_num();
  NAmp = rank/NN;
  nn = rank-NAmp*NN+1.0;
  ni = (nn-1.0)*2.0*PI*j/NC;
  
  // betatron phase 
   phase = exp(-ni);
     
       // set name of output file based on input parameters
       sprintf(filename,"TBTAmp%.3fTau%.3fQs%.3fCV%.2fQ%.3fR%i.dat", Amp,  tau0*1e9, qs*1e5,Cv,Vtune0,rank);
      
   myfile = fopen (filename, "w+");
   // adding input parameters to header of output file
   fprintf(myfile, "Ggam0=%f  alpha=%f  Vtune0=%f  Cv=%f  qs=%f   eta=%f  tau0=%f ns  rank = %i offset = %i\n ", Ggam0,alpha,Vtune0,Cv,qs,eta,tau0*1e9,rank, offset);
   
   fprintf(myfile, "NT=%lld  DumpStep=%d  Steps=%d  NN=%d  nstrobe=%d   Np=%d \n ", NT,DumpStep,Steps,NN,nstrobe,Np);
 
   fprintf(myfile, "Amp=%f  \n",Amp);
  
   
  
  // Assigning Resonance locations and strengths
  K[0] = k0  - Vtune0; 
  ww0[0] = std::complex<double> ( w0R, w0I );
  ww0[0] = ww0[0]*phase*Amp;
  
  K[1] = k1  - Vtune0; 
  ww0[1] = std::complex<double> ( w1R, w1I );
  ww0[1] = ww0[1]*phase*Amp;

  K[2] = k2  + Vtune0; 
  ww0[2] = std::complex<double> ( w2R, w2I );
  ww0[2] = ww0[2]*phase*Amp;

  K[3] = Vtune0 + k3; 
  ww0[3] = std::complex<double> ( w3R, w3I );
  ww0[3] = ww0[3]*phase*Amp;

  K[4] = Vtune0 + k4; 
  ww0[4] = std::complex<double> ( w4R, w4I );
  ww0[4] = ww0[4]*phase*Amp;

  K[5] = k5 ; 
  ww0[5] = std::complex<double> ( w5R, w5I );

  K[6] = k6; 
  ww0[6] = std::complex<double> ( w6R, w6I );

  // printing resonances used 
 
  for(int ir=0;ir<7;ir++){
     fprintf(myfile, "K=%f  ww= ( %f, %f ) \n",K[ir],std::real(ww0[ir]),std::imag(ww0[ir]));
     std::cout << rank << " " << ir << " K= " << K[ir] << " " << ww0[ir].real() << " " << ww0[ir].imag() << "\n";
   }
    
   
   
  // tracking through one turn to calculate spin closed orbit
       OneTurnAN(ww0,K,Ggam0,0,Steps,T);
       
       
       t11 = T[0];
       t12 = T[1];
       t21 = T[2];
       t22 = T[3];
       a = std::real(t11);
       d = std::imag(t12);
       c = std::real(t12);
       b = std::imag(t11);
       t0 = a;
       t1 = -d;
       t2 = -c;
       t3 = -b;
       R11 = t0*t0+t1*t1-t2*t2-t3*t3;
       R12 = 2*(t1*t2-t0*t3);
       R13 = 2*(t1*t3+t0*t2);

       R21 = 2*(t1*t2+t0*t3);
       R22 = t0*t0-t1*t1+t2*t2-t3*t3;
       R23 = 2*(t2*t3-t0*t1);

       R31 = 2*(t1*t3-t0*t2);
       R32 = 2*(t2*t3+t0*t1);
       R33 = t0*t0-t1*t1-t2*t2+t3*t3;


       nx =  R32-R23;
       ny = -R31+R13;
       nz =  R21-R12;
       norm3 = sqrt(nx*nx + ny*ny + nz*nz);
       nx = nx/norm3;
       ny = ny/norm3;
       nz = nz/norm3;

       nus = acos(std::real(t11))/PI;
       n1 = -std::imag(t21)/std::abs(sin(PI*nus));
       n2 =  std::real(t21)/std::abs(sin(PI*nus));
       n3 = -std::imag(t11)/std::abs(sin(PI*nus));
       norm = sqrt(n1*n1 + n2*n2 + n3*n3);
       u1 = sqrt((n3+1)/2);
       d1 = (n1+j*n2)/sqrt(2*(1+n3));
       for(int i=0;i<4;i++){T0[i]=Id[i];T1[i]=Id[i];}
       b1 = 0;
       b2 = 0;
       b3 = 0;
       GgamF = (Ggam0+alpha*2.0*PI*(NT-1));
       
       
// Doing Stroboscopic Average on Final GgamF
       

for(int k=0;k<nstrobe;k++)
	 {
	   OneTurnAN(ww0,K,GgamF,k,Steps,T);
	 
	   dot1(T,T0,T1);
	   
           for(int i=0;i<4;i++){T0[i]=T1[i];}
	   t11 = T1[0];
	   t12 = T1[1];
	   t21 = T1[2];
	   t22 = T1[3];
	   a = std::real(t11);
	   d = std::imag(t12);
	   c = std::real(t12);
	   b = std::imag(t11);

	   t0 = a;
	   t1 = -d;
	   t2 = -c;
	   t3 = -b;
	   R11 = t0*t0+t1*t1-t2*t2-t3*t3;
	   R12 = 2*(t1*t2-t0*t3);
	   R13 = 2*(t1*t3+t0*t2);

	   R21 = 2*(t1*t2+t0*t3);
	   R22 = t0*t0-t1*t1+t2*t2-t3*t3;
	   R23 = 2*(t2*t3-t0*t1);

	   R31 = 2*(t1*t3-t0*t2);
	   R32 = 2*(t2*t3+t0*t1);
	   R33 = t0*t0-t1*t1-t2*t2+t3*t3;
 	   b1 = b1 + R11*n1 + R21*n2 + R31*n3;
	   b2 = b2 + R12*n1 + R22*n2 + R32*n3;
 	   b3 = b3 + R13*n1 + R23*n2 + R33*n3;
	  
	 }

       b1 = b1/nstrobe;
       b2 = b2/nstrobe;
       b3 = b3/nstrobe;
       norm = sqrt(b1*b1 + b2*b2 + b3*b3);
       b1f = b1/norm;
       b2f = b2/norm;
       b3f = b3/norm;
       std::cout << "Stable Spin for final Ggamma=" << GgamF<<  "  b1="<<b1f << "b2="<<b2f << "b3=" << b3f << "\n";
    
       for(int i=0;i<4;i++){T0[i]=Id[i];T1[i]=Id[i];}
       b1 = 0;
       b2 = 0;
       b3 = 0;
      
      
       // Doing Stroboscopic Average on initial Ggam0

       for(int k=0;k<nstrobe;k++)
	 {
	   OneTurnAN(ww0,K,Ggam0,k,Steps,T);
	   dot1(T,T0,T1);
	   	   
	   
           for(int i=0;i<4;i++){T0[i]=T1[i];}
	   t11 = T1[0];
	   t12 = T1[1];
	   t21 = T1[2];
	   t22 = T1[3];
	   a = std::real(t11);
	   d = std::imag(t12);
	   c = std::real(t12);
	   b = std::imag(t11);

	   t0 = a;
	   t1 = -d;
	   t2 = -c;
	   t3 = -b;
	   R11 = t0*t0+t1*t1-t2*t2-t3*t3;
	   R12 = 2*(t1*t2-t0*t3);
	   R13 = 2*(t1*t3+t0*t2);

	   R21 = 2*(t1*t2+t0*t3);
	   R22 = t0*t0-t1*t1+t2*t2-t3*t3;
	   R23 = 2*(t2*t3-t0*t1);

	   R31 = 2*(t1*t3-t0*t2);
	   R32 = 2*(t2*t3+t0*t1);
	   R33 = t0*t0-t1*t1-t2*t2+t3*t3;
 	   b1 = b1 + R11*n1 + R21*n2 + R31*n3;
	   b2 = b2 + R12*n1 + R22*n2 + R32*n3;
 	   b3 = b3 + R13*n1 + R23*n2 + R33*n3;
	  
	 }
       
       b1 = b1/nstrobe;
       b2 = b2/nstrobe;
       b3 = b3/nstrobe;
       norm = sqrt(b1*b1 + b2*b2 + b3*b3);
       b1 = b1/norm;
       b2 = b2/norm;
       b3 = b3/norm;
       ut = sqrt((b3+1)/2);
       dt = (b1+j*b2)/sqrt(2*(1+b3));
       SArray[0] = ut; SArray[1]=dt;
       std::cout << "Stable Spin for Initial Ggamma =" << Ggam0 << "b1_i="<<b1 << "b2_i="<<b2 << "b3_i=" << b3 << "\n";
       kp=0;
       clock_t begin;
       

  // beging tracking using particles prepared with stroboscopic averaging.
       
        for(long long int k=0;k<NT+1;k++)
	 {
	  
	   
	  
           // Ggamma update     
           Ggam = (Ggam0+alpha*2.0*PI*(k));
	  
          
	   // One Turn Map Update
	   OneTurnAN(ww0,K,Ggam,k,Steps,T);
	  
	    
	   dot2(T,SArray,SAout);
	   SArray[0]=SAout[0];SArray[1]=SAout[1];
	   
	   // calculating vertical spin component after one turn
           SY = std::abs(SArray[0])*std::abs(SArray[0])-std::abs(SArray[1])*std::abs(SArray[1]);      

	   
            if( k % DumpStep == 0){
	      // printing Spin to file
	      kp=rank*Np+k/DumpStep;

	      double wrr,wii,Ares,SA[1];
	      wrr = std::real(ww0[2]);
	      wii = std::imag(ww0[2]);
	      Ares = sqrt(wrr*wrr+wii*wii);
	      
	      SAmp(Ggam,K[2],Ares,SA);
	      //   std::cout << "Ares=" << Ares << "K[2]=" << K[2] << "wrr=" << wrr << "SA=" << SA[0] << "\n";
	      fprintf(myfile, " %lld %f %f %f \n", k,Ggam,SY,SA[0]);
		 	
		 
	    }

	 }
	 
std::cout<< "finnished  final tracking \n";
	

// calculating difference between vertical component of final spin and vertical component of stable spin direction at final Energy
	  dif3 = std::abs(b3f-SY);
	  printf(" %i b3f=%f SY=%f dif3=%f  \n", rank, b3f, SY, dif3);
	  fprintf(myfile, "rank=%i, b3f=%f, SY=%f, dif3=%f \n",rank,b3f,SY,dif3);
	  
	  fclose(myfile);

	  MPI_Barrier(MPI_COMM_WORLD);
          MPI_Finalize();

	
	 return(0);
  
}

void  OneTurnIF( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4])
{

  std::complex<double> Id[4],sig1[4],sig2[4],sig3[4],Zout2[4],Zout3[4];
  std::complex<double> T1S[4],T2S[4],ExpI,ExpF,M1[4],M2[4],MT[4],Mout[4];
  std::complex<double> Mn[4],x1[4],MT1[4],M3[4],M4[4],MT2[4],x2[4],j ;
  j = std::complex<double> (0.0 , 1.0) ;
   double ti,tsnake1,h,t;
   double phi = PI/4.0;
   double Cs = cos(phi);
   double Ss = sin(phi);
   // Setting Up Identity Matrix
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);
  // Setting up Pauli Spin Matricies
  sig1[0] = std::complex<double> (0.0,0.0);
  sig1[1] = std::complex<double> (1.0,0.0);
  sig1[2] = std::complex<double> (1.0,0.0);
  sig1[3] = std::complex<double> (0.0,0.0);

  sig2[0] = std::complex<double> (0.0,0.0);
  sig2[1] = -j;
  sig2[2] =  j;
  sig2[3] = std::complex<double> (0.0, 0.0);

  sig3[0] = std::complex<double> (1.0,0.0);
  sig3[1] = std::complex<double> (0.0,0.0);
  sig3[2] = std::complex<double> (0.0,0.0);
  sig3[3] = std::complex<double> (-1.0,0.0);


  // Setting up Matrix for Snake 1 T1S
  mult1(sig1,-j*Cs,Zout);
  mult1(sig2,j*Ss,Zout2);
  add1(Zout,Zout2,T1S);
  
  // Setting up Matrix for Snake 2 T2S
   mult1(sig1,-j*Cs,Zout);
   mult1(sig2,-j*Ss,Zout2);
   add1(Zout,Zout2,T2S);

   // setting starting theta value (ti)
   ti = TurnNo*2.0*PI;
   // setting theta value of first snake
   tsnake1 = ti+PI;
   
   // Initializing transport matrix to identity matrix
   MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
   

   // Setting up matrix for transformation to interaction Frame applied at beginning of lattice  
   ExpI = exp(j*Ggamma*ti/2.0);
   M1[0] = ExpI;
   M1[1] = std::complex<double> (0.0,0.0);
   M1[2] = std::complex<double> (0.0,0.0);
   M1[3] = std::conj(ExpI) ;

   // Setting up matrix for transformation out of interaction Frame applied before first snake
   ExpF = exp(j*Ggamma*tsnake1/2.0);
   M2[0] =  std::conj(ExpF);
   M2[1] = std::complex<double>(0.0,0.0);
   M2[2] = std::complex<double>(0.0,0.0);
   M2[3] =  ExpF;

  // Setting integration step size

   h = PI/Steps;
  
   for(int i=1;i<=Steps;i++){
     // looping through theta steps from start to first snake
     t = ti+h*(i-1);

     // Calling T-BMT Integrator
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix	   
     dot1(Mn,MT,Mout);
      MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }

// loading MT into new matrix x1 and setting MT back to Identity matrix
     x1[0] = MT[0]; x1[1] = MT[1]; x1[2] = MT[2]; x1[3] = MT[3];
     MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
     
// applying transformation into and out of the interaction frame:  M2.x1.M1 = MT  
    dot1(M2,x1,Zout);
    dot1(Zout,M1,MT1);
   
  // Setting up matrix for transformation to interaction Frame applied at exit of first snake
   ExpI = exp(j*Ggamma*tsnake1/2.0);
   M3[0] = ExpI;
   M3[1] = std::complex<double> (0.0,0.0);
   M3[2] = std::complex<double> (0.0,0.0);
   M3[3] = std::conj(ExpI);

   // Setting up matrix for transformation out of interaction Frame applied at entrance of 2nd snake
   ExpF = exp(j*Ggamma*(tsnake1+PI)/2.0);
   M4[0] = std::conj(ExpF);
   M4[1] = std::complex<double>(0.0,0.0);
   M4[2] = std::complex<double>(0.0,0.0);
   M4[3] =  ExpF;


   for(int i=1;i<=Steps;i++){
      // looping through theta steps from exit of first snake to 2nd snake and end of lattice
     t = tsnake1+h*(i-1);

     // Calling T-BMT Integrator
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix
     dot1(Mn,MT,Mout);
     MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }
   // loading MT into new matrix x1
   x2[0] = MT[0]; x2[1] = MT[1]; x2[2] = MT[2]; x2[3] = MT[3];
   
   // applying transformation into and out of the interaction frame:  M4.x2.M3 = MT2
   dot1(M4,x2,Zout);
   dot1(Zout,M3,MT2);

   // creating one turn matrix: T1S.MT2.T2S.MT1 = Zout
   dot1(T1S,MT2,Zout);
   dot1(Zout,T2S,Zout2);
   dot1(Zout2,MT1,Zout);

   if(Zout[0]!=Zout[0]){
   std::cout << "Zout =" << Zout[0] << " t= " << t << "\n";
   std::cout << "Mn = " << Mn[0] << " " << Mn[1] << "\n";
   exit(-1);
     }
  

}



void  OneTurnRF( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4])
{

  std::complex<double> Id[4],sig1[4],sig2[4],sig3[4],Zout2[4],Zout3[4];
  std::complex<double> T1S[4],T2S[4],ExpI,ExpF,M1[4],M2[4],MT[4],Mout[4];
  std::complex<double> Mn[4],x1[4],MT1[4],M3[4],M4[4],MT2[4],x2[4],j ;
  j = std::complex<double> (0.0 , 1.0) ;
   double ti,tsnake1,h,t;
   double phi = PI/4.0;
   double Cs = cos(phi);
   double Ss = sin(phi);
   // Setting Up Identity Matrix
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);
  // Setting up Pauli Spin Matricies
  sig1[0] = std::complex<double> (0.0,0.0);
  sig1[1] = std::complex<double> (1.0,0.0);
  sig1[2] = std::complex<double> (1.0,0.0);
  sig1[3] = std::complex<double> (0.0,0.0);

  sig2[0] = std::complex<double> (0.0,0.0);
  sig2[1] = -j;
  sig2[2] =  j;
  sig2[3] = std::complex<double> (0.0, 0.0);

  sig3[0] = std::complex<double> (1.0,0.0);
  sig3[1] = std::complex<double> (0.0,0.0);
  sig3[2] = std::complex<double> (0.0,0.0);
  sig3[3] = std::complex<double> (-1.0,0.0);


  // Setting up Matrix for Snake 1 T1S
  mult1(sig1,-j*Cs,Zout);
  mult1(sig2,j*Ss,Zout2);
  add1(Zout,Zout2,T1S);
  
  // Setting up Matrix for Snake 2 T2S
   mult1(sig1,-j*Cs,Zout);
   mult1(sig2,-j*Ss,Zout2);
   add1(Zout,Zout2,T2S);

   // setting starting theta value (ti)
   ti = TurnNo*2.0*PI;
   // setting theta value of first snake
   tsnake1 = ti+PI;
   
   // Initializing transport matrix to identity matrix
   MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
   

   // Setting up matrix for transformation to Resonance Frame applied at beginning of lattice  
   ExpI = exp(j*K[2]*ti/2.0);
   M1[0] = ExpI;
   M1[1] = std::complex<double> (0.0,0.0);
   M1[2] = std::complex<double> (0.0,0.0);
   M1[3] = std::conj(ExpI) ;

   // Setting up matrix for transformation out of Resonance Frame applied before first snake
   ExpF = exp(j*K[2]*tsnake1/2.0);
   M2[0] =  std::conj(ExpF);
   M2[1] = std::complex<double>(0.0,0.0);
   M2[2] = std::complex<double>(0.0,0.0);
   M2[3] =  ExpF;

  // Setting integration step size

   h = PI/Steps;
  
   for(int i=1;i<=Steps;i++){
     // looping through theta steps from start to first snake
     t = ti+h*(i-1);

     // Calling T-BMT Integrator
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix	   
     dot1(Mn,MT,Mout);
      MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }

// loading MT into new matrix x1 and setting MT back to Identity matrix
     x1[0] = MT[0]; x1[1] = MT[1]; x1[2] = MT[2]; x1[3] = MT[3];
     MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
     
// applying transformation into and out of the interaction frame:  M2.x1.M1 = MT  
    dot1(M2,x1,Zout);
    dot1(Zout,M1,MT1);
   
  // Setting up matrix for transformation to Resonance Frame applied at exit of first snake
   ExpI = exp(j*K[2]*tsnake1/2.0);
   M3[0] = ExpI;
   M3[1] = std::complex<double> (0.0,0.0);
   M3[2] = std::complex<double> (0.0,0.0);
   M3[3] = std::conj(ExpI);

   // Setting up matrix for transformation out of Resonance Frame applied at entrance of 2nd snake
   ExpF = exp(j*K[2]*(tsnake1+PI)/2.0);
   M4[0] = std::conj(ExpF);
   M4[1] = std::complex<double>(0.0,0.0);
   M4[2] = std::complex<double>(0.0,0.0);
   M4[3] =  ExpF;


   for(int i=1;i<=Steps;i++){
      // looping through theta steps from exit of first snake to 2nd snake and end of lattice
     t = tsnake1+h*(i-1);

     // Calling T-BMT Integrator
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix
     dot1(Mn,MT,Mout);
     MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }
   // loading MT into new matrix x1
   x2[0] = MT[0]; x2[1] = MT[1]; x2[2] = MT[2]; x2[3] = MT[3];
   
   // applying transformation into and out of the interaction frame:  M4.x2.M3 = MT2
   dot1(M4,x2,Zout);
   dot1(Zout,M3,MT2);

   // creating one turn matrix: T1S.MT2.T2S.MT1 = Zout
   dot1(T1S,MT2,Zout);
   dot1(Zout,T2S,Zout2);
   dot1(Zout2,MT1,Zout);
  

}


void  OneTurnAN( std::complex<double> Res[7], double K[7], double Ggamma, int TurnNo, int Steps, std::complex<double> Zout[4])
{

  std::complex<double> Id[4],sig1[4],sig2[4],sig3[4],Zout2[4],Zout3[4];
  std::complex<double> T1S[4],T2S[4],ExpI,ExpF,M1[4],M2[4],MT[4],Mout[4];
  std::complex<double> Mn[4],x1[4],MT1[4],M3[4],M4[4],MT2[4],x2[4],A0[4],AN[4],j ;
  j = std::complex<double> (0.0 , 1.0) ;
   double ti,tsnake1,h,t;
   double phi = PI/4.0;
   double Cs = cos(phi);
   double Ss = sin(phi);
   // Setting Up Identity Matrix
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);
  // Setting up Pauli Spin Matricies
  sig1[0] = std::complex<double> (0.0,0.0);
  sig1[1] = std::complex<double> (1.0,0.0);
  sig1[2] = std::complex<double> (1.0,0.0);
  sig1[3] = std::complex<double> (0.0,0.0);

  sig2[0] = std::complex<double> (0.0,0.0);
  sig2[1] = -j;
  sig2[2] =  j;
  sig2[3] = std::complex<double> (0.0, 0.0);

  sig3[0] = std::complex<double> (1.0,0.0);
  sig3[1] = std::complex<double> (0.0,0.0);
  sig3[2] = std::complex<double> (0.0,0.0);
  sig3[3] = std::complex<double> (-1.0,0.0);


  // Setting up Matrix for Snake 1 T1S
  mult1(sig1,-j*Cs,Zout);
  mult1(sig2,j*Ss,Zout2);
  add1(Zout,Zout2,T1S);
  
  // Setting up Matrix for Snake 2 T2S
   mult1(sig1,-j*Cs,Zout);
   mult1(sig2,-j*Ss,Zout2);
   add1(Zout,Zout2,T2S);

   // setting starting theta value (ti)
   ti = TurnNo*2.0*PI;
   // setting theta value of first snake
   tsnake1 = ti+PI;
   
   // Initializing transport matrix to identity matrix
   MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
   

   // Setting up matrix for transformation to Resonance Frame applied at beginning of lattice  
   ExpI = exp(j*K[2]*ti/2.0);
   M1[0] = ExpI;
   M1[1] = std::complex<double> (0.0,0.0);
   M1[2] = std::complex<double> (0.0,0.0);
   M1[3] = std::conj(ExpI) ;

   // Setting up matrix for transformation out of Resonance Frame applied before first snake
   ExpF = exp(j*K[2]*tsnake1/2.0);
   M2[0] =  std::conj(ExpF);
   M2[1] = std::complex<double>(0.0,0.0);
   M2[2] = std::complex<double>(0.0,0.0);
   M2[3] =  ExpF;

  // Setting integration step size

   h = PI/Steps;
   /*
   // Setting up Single Resonance Kernel A0
   A0[0] = (K[2]-Ggamma)*j/2.0;
   A0[1] = Res[2]*j/2.0;
   A0[2] = std::conj(Res[2])*j/2.0;
   A0[3] = -(K[2]-Ggamma)*j/2.0;
   */
   for(int i=1;i<=Steps;i++){
     // looping through theta steps from start to first snake
     t = ti+h*(i-1);
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix	   
     dot1(Mn,MT,Mout);
      MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }

// loading MT into new matrix x1 and setting MT back to Identity matrix
     x1[0] = MT[0]; x1[1] = MT[1]; x1[2] = MT[2]; x1[3] = MT[3];
     MT[0] = Id[0]; MT[1]=Id[1]; MT[2]=Id[2]; MT[3]=Id[3];
     
// applying transformation into and out of the interaction frame:  M2.x1.M1 = MT  
    dot1(M2,x1,Zout);
    dot1(Zout,M1,MT1);
   
  // Setting up matrix for transformation to Resonance Frame applied at exit of first snake
   ExpI = exp(j*K[2]*tsnake1/2.0);
   M3[0] = ExpI;
   M3[1] = std::complex<double> (0.0,0.0);
   M3[2] = std::complex<double> (0.0,0.0);
   M3[3] = std::conj(ExpI);

   // Setting up matrix for transformation out of Resonance Frame applied at entrance of 2nd snake
   ExpF = exp(j*K[2]*(tsnake1+PI)/2.0);
   M4[0] = std::conj(ExpF);
   M4[1] = std::complex<double>(0.0,0.0);
   M4[2] = std::complex<double>(0.0,0.0);
   M4[3] =  ExpF;


   for(int i=1;i<=Steps;i++){
      // looping through theta steps from exit of first snake to 2nd snake and end of lattice
     t = tsnake1+h*(i-1);
     // multiplying A0 kernel by theta 
     FourthO(h,t,Res,K,Ggamma,Mn);
     // Cumulating matrix dot product into MT matrix
     dot1(Mn,MT,Mout);
     MT[0] = Mout[0]; MT[1]=Mout[1]; MT[2]=Mout[2]; MT[3]=Mout[3]; 
   }
   // loading MT into new matrix x1
   x2[0] = MT[0]; x2[1] = MT[1]; x2[2] = MT[2]; x2[3] = MT[3];
   
   // applying transformation into and out of the interaction frame:  M4.x2.M3 = MT2
   dot1(M4,x2,Zout);
   dot1(Zout,M3,MT2);

   // creating one turn matrix: T1S.MT2.T2S.MT1 = Zout
   dot1(T1S,MT2,Zout);
   dot1(Zout,T2S,Zout2);
   dot1(Zout2,MT1,Zout);
  

}







void FourthO( double h, double t, std::complex<double> Res[7], double K[7],double Ggamma, std::complex<double> Zout[4])
{  std::complex<double> Id[4],sig1[4],sig2[4],sig3[4],Zout2[4];
   std::complex<double> A1[4],A2[4],cross[4],d[4];
   std::complex<double> a[4],gi,g2,j;
   j= std::complex<double> (0.0 , 1.0);
   double c1,c2,g,a2,b,ab;
   // Setting Up Identity Matrix
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);
  // Setting up Pauli Spin Matricies
  sig1[0] = std::complex<double> (0.0,0.0);
  sig1[1] = std::complex<double> (1.0,0.0);
  sig1[2] = std::complex<double> (1.0,0.0);
  sig1[3] = std::complex<double> (0.0,0.0);

  sig2[0] = std::complex<double> (0.0,0.0);
  sig2[1] = -j;
  sig2[2] =  j;
  sig2[3] = std::complex<double> (0.0, 0.0);

  sig3[0] = std::complex<double> (1.0,0.0);
  sig3[1] = std::complex<double> (0.0,0.0);
  sig3[2] = std::complex<double> (0.0,0.0);
  sig3[3] = std::complex<double> (-1.0,0.0);

  // weights for Gaussian quadrature 
   c1 = 0.5-sqrt(3)/6.0;
   c2 = 0.5+sqrt(3)/6.0;

   // Calculating A1=A(t+c1*h) and A2=A(t+c2*h) matricies for Magnus integration
  
   /*
   
    A_RFrame(Res,K,Ggamma,(t+c1*h),A1);
    A_RFrame(Res,K,Ggamma,(t+c2*h),A2);

   
    // A_IFrame(Res,K,Ggamma,(t+c1*h),A1);
    //A_IFrame(Res,K,Ggamma,(t+c2*h),A2);

   
   //    std::cout << "A1 =" << A1[0] << " " << A1[1] << "\n";
   //  std::cout << "A2 =" << A2[0] << " " << A2[1] << "\n";
  

   // calculating [A1,A2] = A1.A2 - A2.A1 =cross
   dot1(A1,A2,Zout);
   dot1(A2,A1,Zout2);
   minus1(Zout,Zout2,cross);
   
   // calculating h/2(A1 + A2) 
   mult1(A1,h/2,Zout);
   mult1(A2,h/2,Zout2);
   add1(Zout,Zout2,Zout);
   // Calculating d = h/2(A1 + A2) - h*h*sqrt(3)/12*[A1,A2]
   mult1(cross,h*h*sqrt(3)/12.0,Zout2);
   minus1(Zout,Zout2,d);
   */
   d[0] = (K[2]-Ggamma)*j*h/2.0;
   d[1] = Res[2]*j*h/2.0;
   

   // calculating exp(d) = Zout matrix exponential 
   gi = d[0];g2=d[1];
   g = std::imag(gi); a2 = std::real(g2);b=std::imag(g2);
   ab = sqrt(g*g+a2*a2+b*b);
   //  std::cout << "ab =" << ab << "\n";
   
   mult1(sig3,g,Zout);
   mult1(sig1,b,Zout2);
   add1(Zout,Zout2,Zout);
   mult1(sig2,a2,Zout2);

   add1(Zout,Zout2,a);
   mult1(Id,cos(ab),Zout);
   mult1(a,j*sin(ab)/ab,Zout2);
   add1(Zout,Zout2,Zout);
   
}



void A_IFrame(std::complex<double> Res[7], double K[7], double Ggamma, double t, std::complex<double> Zout[4])
{
  std::complex<double> Kvec[7],Xi,CXi;
  std::complex<double> j;
  j = std::complex<double> (0.0 , 1.0);
 
  Kvec[0] = exp(j*t*(Ggamma-K[0]));
  Kvec[1] = exp(j*t*(Ggamma-K[1]));
  Kvec[2] = exp(j*t*(Ggamma-K[2]));
  Kvec[3] = exp(j*t*(Ggamma-K[3]));
  Kvec[4] = exp(j*t*(Ggamma-K[4]));
  Kvec[5] = exp(j*t*(Ggamma-K[5]));
  Kvec[6] = exp(j*t*(Ggamma-K[6]));
  
 Xi = Kvec[0]*Res[0]+Kvec[1]*Res[1]+Kvec[2]*Res[2]+Kvec[3]*Res[3]+Kvec[4]*Res[4]+Kvec[5]*Res[5]+Kvec[6]*Res[6];

 //std::cout << "Xi=" << Xi << "Kvec[0]=" << Kvec[0] << "Res[0]=" << Res[0] << "\n";
	 CXi = std::conj(Xi); 

		Zout[0]= std::complex<double>(0.0,0.0);
		Zout[1] = Xi*j/2.0;
		Zout[2] = CXi*j/2.0;
		Zout[3] = std::complex<double>(0.0,0.0);

}


void A_RFrame(std::complex<double> Res[7], double K[7], double Ggamma, double t, std::complex<double> Zout[4])
{
  std::complex<double> Kvec[7],Xi,CXi;
  std::complex<double> j;
  j = std::complex<double> (0.0 , 1.0);
 
  Kvec[0] = exp(j*t*(K[2]-K[0]));
  Kvec[1] = exp(j*t*(K[2]-K[1]));
  Kvec[2] = exp(j*t*(K[2]-K[2]));
  Kvec[3] = exp(j*t*(K[2]-K[3]));
  Kvec[4] = exp(j*t*(K[2]-K[4]));
  Kvec[5] = exp(j*t*(K[2]-K[5]));
  Kvec[6] = exp(j*t*(K[2]-K[6]));
  
 Xi = Kvec[0]*Res[0]+Kvec[1]*Res[1]+Kvec[2]*Res[2]+Kvec[3]*Res[3]+Kvec[4]*Res[4]+Kvec[5]*Res[5]+Kvec[6]*Res[6];
 ;
	 CXi = std::conj(Xi); 

	        Zout[0]= j*(K[2]-Ggamma)/2.0;
		Zout[1] = Xi*j/2.0;
		Zout[2] = CXi*j/2.0;
		Zout[3] = -j*(K[2]-Ggamma)/2.0;

		
}


void add1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4])
{
  Zout[0] = X[0] + Y[0];
  Zout[1] = X[1] + Y[1];
  Zout[2] = X[2] + Y[2];
  Zout[3] = X[3] + Y[3];
  //std::cout << "Zout in add =" << Zout[0] << " " << Zout[1] << " " << Zout[2] << " " << Zout[3] << " \n";
}


void minus1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4])
{
  Zout[0] = X[0] - Y[0];
  Zout[1] = X[1] - Y[1];
  Zout[2] = X[2] - Y[2];
  Zout[3] = X[3] - Y[3];
}


void dot1( std::complex<double> X[4], std::complex<double> Y[4], std::complex<double> Zout[4])
{
  //  std::cout << "1 Zout2 = " << Zout[2] << "X2=" << X[2] << "Y0=" << Y[0] << "X3=" << X[3] << "Y2=" <<Y[2] << "\n";
  Zout[0] = X[0]*Y[0]+X[1]*Y[2];
  Zout[1] = X[0]*Y[1]+X[1]*Y[3];
  Zout[2] = X[2]*Y[0]+X[3]*Y[2];
  Zout[3] = X[2]*Y[1]+X[3]*Y[3];
  // std::cout << "2 Zout2 = " << Zout[2] << "X2=" << X[2] << "Y0=" << Y[0] << "X3=" << X[3] << "Y2=" <<Y[2] << "\n";
}


void dot2( std::complex<double> X[4], std::complex<double> Y[2], std::complex<double> Zout[2])
{
  //  std::cout << "1 Zout2 = " << Zout[2] << "X2=" << X[2] << "Y0=" << Y[0] << "X3=" << X[3] << "Y2=" <<Y[2] << "\n";
  Zout[0] = X[0]*Y[0]+X[1]*Y[1];
  Zout[1] = X[2]*Y[0]+X[3]*Y[1];
  
  // std::cout << "2 Zout2 = " << Zout[2] << "X2=" << X[2] << "Y0=" << Y[0] << "X3=" << X[3] << "Y2=" <<Y[2] << "\n";
}

void mult1( std::complex<double> *X, std::complex<double> y, std::complex<double> Zout[4])
{
  // std::cout << "Zout in mult before =" << Zout[0] << " " << Zout[1] << " " << Zout[2] << " " << Zout[3] << " \n";
  Zout[0] = X[0]*y;
  Zout[1] = X[1]*y;
  Zout[2] = X[2]*y;
  Zout[3] = X[3]*y;
  //std::cout << "Zout in mult =" << Zout[0] << " " << Zout[1] << " " << Zout[2] << " " << Zout[3] << " \n";
  
}



void expM( std::complex<double> A[4], std::complex<double> Zout[4])
{  std::complex<double> Id[4],sig1[4],sig2[4],sig3[4],Zout2[4];
   std::complex<double> A1[4],A2[4],cross[4];
   std::complex<double> a[4],gi,g2,j,ab,g,a2,b;
   j= std::complex<double> (0.0 , 1.0);
  Id[0] = std::complex<double>(1.0, 0.0); 
  Id[1] = std::complex<double>(0.0,0.0);
  Id[2] = std::complex<double>(0.0,0.0);
  Id[3] = std::complex<double>(1.0,0.0);

  sig1[0] = std::complex<double> (0.0,0.0);
  sig1[1] = std::complex<double> (1.0,0.0);
  sig1[2] = std::complex<double> (1.0,0.0);
  sig1[3] = std::complex<double> (0.0,0.0);

  sig2[0] = std::complex<double> (0.0,0.0);
  sig2[1] = -j;
  sig2[2] =  j;
  sig2[3] = std::complex<double> (0.0, 0.0);

  sig3[0] = std::complex<double> (1.0,0.0);
  sig3[1] = std::complex<double> (0.0,0.0);
  sig3[2] = std::complex<double> (0.0,0.0);
  sig3[3] = std::complex<double> (-1.0,0.0);
  
   gi = A[0];g2=A[1];
   if(gi + g2 != 0.0){
   g = std::imag(gi); a2 = std::real(g2);b=std::imag(g2);
   ab = sqrt(g*g+a2*a2+b*b);
   
   mult1(sig3,g,Zout);
   mult1(sig1,b,Zout2);
   add1(Zout,Zout2,Zout);
   mult1(sig2,a2,Zout2);

   add1(Zout,Zout2,a);
   mult1(Id,cos(ab),Zout);
   mult1(a,j*sin(ab)/ab,Zout2);
   add1(Zout,Zout2,Zout);}else {
     std::cout << "doing Identity in expM \n";
     Zout[0]=Id[0];Zout[1]=Id[1];Zout[2]=Id[2];Zout[3]=Id[3];}


}

void SAmp(double Ggamma, double K2, double ARes, double SA[1])
{
  double delta = K2 - Ggamma;
  double lambda = sqrt(delta*delta+ARes*ARes);
  double b = ARes*sin(PI*lambda/2.0)/lambda;
  //  std::cout << "b=" << b << "ARes=" << ARes << "Lamb=" << lambda << "\n";
  SA[0] = 1-8*b*b*(1-b*b);
  //  std::cout << "SA[0]=" << SA[0] << "\n";
}