#include <stdio.h>
double f1(int id,double t,double x[3],double v[3])
{
        return v[id];
}

double f2(int id,double t,double x[3],double v[3],double B[3])
{
  double ans[3];
 ans[0]=v[1]*B[2]-v[2]*B[1];
 ans[1]=v[2]*B[0]-v[0]*B[2];
 ans[2]=v[0]*B[1]-v[1]*B[0];
        return ans[id];
}
double fg(int id,double t,double x[3],double v[3],double g)
{
  double ans[3];
  double R=sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2));
 ans[0]=-1*x[0]*g/(double)R;
 ans[1]=-1*x[1]*g/(double)R;
 ans[2]=-1*x[2]*g/(double)R;
// printf("ans=%lf %lf %lf\n",ans[0],ans[1],ans[2]);
        return ans[id];
}

void Circle(){
     int imax=100E3;
     double pi=TMath::Pi();
     double x0=33.3*1E-2;//
     double y0=0.0;
     double z0=0.*1E-2;//m
     FILE *ofile,*infile;
     char oname[256];
     char iname[256];
     char sdmy[256];
     double ldmy;
     int idmy;

     sprintf(oname,"testCircle.txt"); 
     ofile=fopen(oname,"w"); 

     double t=0;
     double x[3],v[3],dt;
     double k1[6][2],k2[6][2],k3[6][2],k4[6][2];
     double g=9.8;//[m/s^2]
     double v0=sqrt(x0*g);//[m/sec]
      printf("v0=%lf\n",v0);

     x[0]=x0;                  //
     x[1]=y0;                  //
     x[2]=z0;                  //
     v[0]=0;                  //
     v[1]=0;                  //
     v[2]=v0;                  //

     dt=1E-4;                //

     double xx[128*100],zz[128*100],yy[128*100],px[128*100],py[128*100],pz[128*100],p0;
     double vv;
     double tg[128*100];
     int ig=0;
     double x1[3],x2[3],v1[3],v2[3],x3[3],v3[3];
//////////Runge-Kutta
     for(int ii=0;ii<imax;ii++) {
           k1[0][0]=dt*f1(0,t,x,v);
           k1[0][1]=dt*fg(0,t,x,v,g);
           k1[1][0]=dt*f1(1,t,x,v);
           k1[1][1]=dt*fg(1,t,x,v,g);
           k1[2][0]=dt*f1(2,t,x,v);
           k1[2][1]=dt*fg(2,t,x,v,g);

           for(int i=0;i<3;++i)x1[i]=x[i]+k1[i][0]/2.0;
           for(int i=0;i<3;++i)v1[i]=v[i]+k1[i][1]/2.0;
           for(int i=0;i<3;++i)k2[i][0]=dt*f1(i,t+dt/2.0,x1,v1);
           for(int i=0;i<3;++i)k2[i][1]=dt*fg(i,t+dt/2.0,x1,v1,g);

           for(int i=0;i<3;++i)x2[i]=x[i]+k2[i][0]/2.0;
           for(int i=0;i<3;++i)v2[i]=v[i]+k2[i][1]/2.0;
           for(int i=0;i<3;++i)k3[i][0]=dt*f1(i,t+dt/2.0,x2,v2);
           for(int i=0;i<3;++i)k3[i][1]=dt*fg(i,t+dt/2.0,x2,v2,g);

           for(int i=0;i<3;++i)x3[i]=x[i]+k3[i][0];
           for(int i=0;i<3;++i)v3[i]=v[i]+k3[i][1];
           for(int i=0;i<3;++i)k4[i][0]=dt*f1(i,t+dt,x3,v3);
           for(int i=0;i<3;++i)k4[i][1]=dt*fg(i,t+dt,x3,v3,g);

           for(int i=0;i<3;++i)x[i]=x[i]+(k1[i][0]+2.0*k2[i][0]+2.0*k3[i][0]+k4[i][0])/6.0;
           for(int i=0;i<3;++i)v[i]=v[i]+(k1[i][1]+2.0*k2[i][1]+2.0*k3[i][1]+k4[i][1])/6.0;

           if( ii%100==0 && ig<4*2000){  //for graph 
             xx[ig]=x[0];
             yy[ig]=x[1];
             zz[ig]=x[2];
             vv=sqrt(pow(v[0],2)+pow(v[1],2)+pow(v[2],2));
             tg[ig]=t;
             px[ig]=(v[0]/vv);
             py[ig]=(v[1]/vv);
             pz[ig]=(v[2]/vv);
             if( x[1]>0.95){
                   printf("pos y too big %lf\n",x[1]);
                         break;
             }
             printf("%e,%e,%e,%e,%e,%e,%e\n",t,xx[ig],yy[ig],zz[ig],px[ig],py[ig],pz[ig]);
             ig++;
             if(ii>=0)fprintf(ofile,"%e,%e,%e,%e,%e,%e,%e\n",t,x[0],x[1],x[2],v[0],v[1],v[2]);
           }
           t=t+dt;
     }//END(for imax)
     fclose(ofile);



///////Graph
gROOT->SetStyle("Plain");
gROOT->ForceStyle();
gStyle->SetOptStat(0);
gStyle->SetCanvasBorderMode(0);
gStyle->SetPadColor(0);
gStyle->SetTitleFillColor(0);
gStyle->SetStatColor(0);
gStyle->SetCanvasColor(0);
gStyle->SetFrameLineColor(1);

TGraph *gr_xz,*gr_xy;
TGraph *gr_tpx,*gr_tpy,*gr_tpz;
 gr_xz = new TGraph(ig,xx,zz);
 gr_xy = new TGraph(ig,xx,yy);

 gr_tpx = new TGraph(ig,tg,px);
 gr_tpy = new TGraph(ig,tg,py);
 gr_tpz = new TGraph(ig,tg,pz);

 TCanvas *c = new TCanvas("c","check x-z",200,100,500,500);
 c->SetLeftMargin(0.13);
 TH2F* frame = new TH2F("frame"," ",10, -0.4, 0.4, 100, -0.4, 0.4);
 frame->SetTitle("");
 frame->GetXaxis()->SetLabelSize(0.04);
 frame->GetXaxis()->SetTitleSize(0.05);
 frame->GetYaxis()->SetLabelSize(0.04);
 frame->GetYaxis()->SetTitleSize(0.05);
 frame->GetXaxis()->SetTitle("posx[m]");
 frame->GetYaxis()->SetTitle("posz[m]");
 frame->GetYaxis()->SetTitleOffset(1.2);
 frame->SetStats(0);
 frame->Draw();
   gr_xz->SetMarkerStyle(7);
   gr_xz->SetMarkerColor(1);
   gr_xz->SetLineWidth(1);
   gr_xz->Draw("pl");
 c->SetGrid();

 TCanvas *cxy = new TCanvas("cxy","check x-y",200,100,500,500);
 cxy->SetLeftMargin(0.13);
 TH2F* framexy = new TH2F("framexy"," ",10, -0.4, 0.4, 100, -0.1, 0.4);
 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("posx[m]");
 framexy->GetYaxis()->SetTitle("posy[m]");
 framexy->GetYaxis()->SetTitleOffset(1.2);
 framexy->SetStats(0);
 framexy->Draw();
   gr_xy->SetMarkerStyle(7);
   gr_xy->SetMarkerColor(1);
   gr_xy->SetLineWidth(1);
   gr_xy->Draw("pl");
 cxy->SetGrid();



 TCanvas *cSpin = new TCanvas("cVel","check velosity time",200,100,900,500);
 cSpin->SetLeftMargin(0.1);
 cSpin->SetRightMargin(0.1);
 TH2F* frameS = new TH2F("frameS"," ",10, 0., 10, 100, -1, 1);
 frameS->SetTitle("");
 frameS->GetXaxis()->SetLabelSize(0.04);
 frameS->GetXaxis()->SetTitleSize(0.05);
 frameS->GetYaxis()->SetLabelSize(0.04);
 frameS->GetYaxis()->SetTitleSize(0.05);
 frameS->GetXaxis()->SetTitle("[sec]");
 frameS->GetYaxis()->SetTitle("V-vector");
 frameS->GetYaxis()->SetTitleOffset(0.9);
 frameS->SetStats(0);
 frameS->Draw();

   gr_tpx->SetMarkerStyle(7);
   gr_tpx->SetMarkerColor(2);
   gr_tpx->SetLineWidth(1);
   gr_tpx->SetLineColor(2);
   gr_tpx->Draw("pl");

   gr_tpy->SetMarkerStyle(9);
   gr_tpy->SetMarkerColor(8);
   gr_tpy->SetLineWidth(2);
   gr_tpy->SetLineColor(8);
  gr_tpy->Draw("pl");

   gr_tpz->SetMarkerStyle(7);
   gr_tpz->SetMarkerColor(kMagenta+2);
   gr_tpz->SetLineWidth(1);
   gr_tpz->SetLineColor(kMagenta+2);
   gr_tpz->Draw("pl");

 cSpin->SetGrid();
}