program pd_integration

implicit NONE
integer i, n_steps,n_gyro
DOUBLE PRECISION t,dt
DOUBLE PRECISION,  DIMENSION (:), ALLOCATABLE :: time      
DOUBLE PRECISION,  DIMENSION (:,:), ALLOCATABLE :: p 
DOUBLE PRECISION,  DIMENSION (:), ALLOCATABLE :: ps 
DOUBLE PRECISION,  DIMENSION (:,:), ALLOCATABLE :: pe 
DOUBLE PRECISION,  DIMENSION (:), ALLOCATABLE :: pes 
DOUBLE PRECISION,external ::  gyro


! Initialise data fields:

n_gyro=10
n_steps=500
dt=DBLE(n_gyro)*6.28d0/DBLE(n_steps)
print*,"number of steps:",n_steps 
print*,"dt: ",dt
ALLOCATE(time(n_steps),p(6,n_steps) ,ps(6)) ! 6 phase space variables


time=0.0d0
p=0.0d0
ps=0.0d0


do i=1,n_steps
  time(i)=DBLE(i)/DBLE(n_steps)*DBLE(n_gyro)
enddo

p(4,1)=1.5d0! vx(0)=1

do i=2,n_steps
t=time(i)

! choose integrator and problem to integrate:


! gyromotion: 

!call rk4(p(:,i-1),t,dt,ps)
!p(:,i)=p(:,i-1)+ps(:)

!call rk3(p(:,i-1),t,dt,ps)
!p(:,i)=p(:,i-1)+ps(:)

!call euler(p(:,i-1),t,dt,ps)
!p(:,i)=p(:,i-1)+ps(:)
 
!call euler_back(p(:,i-1),t,dt,ps)
!p(:,i)=ps(:)

!call euler_semi(p(:,i-1),t,dt,ps)
!p(:,i)=ps(:)


! harmonic oszillator:

!call euler_feder(p(:,i-1),t,dt,ps)
!p(:,i)=p(:,i-1)+ps(:)

call euler_semi_feder(p(:,i-1),t,dt,ps)
p(:,i)=ps(:)

!call euler_back_feder(p(:,i-1),t,dt,ps)
!p(:,i)=ps(:)

!call rk4_feder(p(:,i-1),t,dt,ps)
!p(:,i)=p(:,i-1)+ps(:)




enddo




! Output of data:

open(10,file="out_data_es")
do i= 1,n_steps
   write(10,*),i,time(i),p(1,i),p(2,i),p(3,i),p(4,i),p(5,i),p(6,i) 
   !write(10,*),i,time(i),p(1,i),p(2,i),p(4,i),p(5,i)
   !write(10,*),i,time(i),p(1,i),p(2,i),p(3,i),pe(1,i),pe(2,i),pe(3,i) 
enddo
close(10)

deallocate(time,p,ps)
end program pd_integration



subroutine euler(pp,t,dt,pps)
 
   implicit none
   
    DOUBLE PRECISION dt,t
    DOUBLE PRECISION pp(6),pps(6),k(6)
       
    
    Call gyro(t,pp,k)
    
    pps=k*dt
    
   
  end subroutine euler


subroutine euler_semi(pp,t,dt,pps)
 
   implicit none
   
    DOUBLE PRECISION dt,t
    DOUBLE PRECISION pp(6),pps(6),k(6)
       
    pps(1:3)= pp(1:3)
    Call gyro(t,pp,k) 
    pps(4:6)=pp(4:6)+k(4:6)*dt
   
    Call gyro(t,pps,k)
    pps(1:3)=pp(1:3)+k(1:3)*dt
   
   
  end subroutine euler_semi


subroutine rk4(pp,t,dt,pps)
 
   implicit none
    DOUBLE PRECISION y
    DOUBLE PRECISION k1,k2,k3,k4,dt,dt_2,t,dt_3,dt_6
    DOUBLE PRECISION f
    DOUBLE PRECISION pp(6),pps(6),k(6),z(6)
  
      dt_2=dt/2.0d0
      dt_3=dt/3.0d0
      dt_6=dt/6.0d0
      
      Call gyro(t,pp,k)
      pps=k*dt_6
      z=pp+dt_2*k
     
      Call gyro(t+dt_2,z,k)
      pps= pps+ k*dt_3
      z=pp+dt_2*k

      Call gyro(t+dt_2,z,k)
      pps= pps+ k*dt_3

      z=pp+dt*k

      Call gyro(t+dt,z,k)
      pps= pps+ k*dt_6
      
    end subroutine rk4
   
subroutine rk3(pp,t,dt,pps)
 
   implicit none
    DOUBLE PRECISION y
    DOUBLE PRECISION k2,k3,k4,dt,dt_2,t,dt_23,dt_6
    DOUBLE PRECISION f
    DOUBLE PRECISION pp(6),pps(6),k(6),z(6),k1(6)
  
      dt_2=dt/2.0d0
      dt_23=2.0d0*dt/3.0d0
      dt_6=dt/6.0d0
      
      Call gyro(t,pp,k)
      k1=k
      pps=k*dt_6
      z=pp+dt_2*k
     
      Call gyro(t+dt_2,z,k)
      pps= pps+ k*dt_23
      z=pp+2.0d0*dt*k-dt*k1

      

      Call gyro(t+dt,z,k)
      pps= pps+ k*dt_6
      
    end subroutine rk3


 subroutine gyro(t,pin,pout)
     implicit none
    DOUBLE PRECISION t, pin(6),pout(6)
    double precision Bz,Bx,By,Ev,sign

    sign=1.0d0
    Ev=0.0d0

    pout(1)=pin(4)  ! 4=vx,5=vy,6=vz,1=x,2=y,3=z
    pout(2)=pin(5)
    pout(3)=pin(6)
    
    pout(4)=sign*(Ev+pin(5)*Bz(pin(1),pin(2),pin(3)))
    pout(5)=sign*(-Bz(pin(1),pin(2),pin(3))*pin(4))
    pout(6)=pin(6)
    
  end subroutine gyro
  
  
  double precision function Bz(x,y,z)
    
    double precision x,y,z
    
     Bz=1.0d0
    !Bz=exp(-x**2)+exp(-y**2)
    !Bz=2.0d0*(sin(x)**2+sin(y)**2)*0.01d0*exp((x**2+y**2)/10)
   
end function Bz



 subroutine euler_back(pp,t,dt,pps)
 
   implicit none
    DOUBLE PRECISION pin(6),pout(6)
     double precision Bz,Bx,By,Ev
    DOUBLE PRECISION dt,t,NN,NNB,NNdt,NNBdt
    DOUBLE PRECISION pp(6),pps(6)

    Ev=0.0d0
    pin=pp
    NN=1.0d0/(1.0d0+dt**2*Bz(pin(1),pin(2),pin(3))**2)
    NNB=dt*Bz(pin(1),pin(2),pin(3))*NN
    NNdt=dt*NN
    NNBdt=dt**2*Bz(pin(1),pin(2),pin(3))*NN  
    
!   print*,NN,NNB,NNdt,NNBdt
!   read*,dt
    pout(1)=pin(1)+NNdt*pin(4)+NNBdt*pin(5)
    pout(2)=pin(2)-NNBdt*pin(4)+NNdt*pin(5)
    pout(3)=pin(6)
    
    pout(4)=NN*pin(4)+NNB*pin(5)
    pout(5)=-NNB*pin(4)+NN*pin(5)+dt*Ev
    pout(6)=pin(6)

    pps=pout
   
  end subroutine euler_back



 

subroutine euler_feder(pp,t,dt,pps)
 
   implicit none
   
    DOUBLE PRECISION dt,t
    DOUBLE PRECISION pp(6),pps(6),k(6)
       
    
    Call feder(t,pp,k)
    
    pps=k*dt
    
   
  end subroutine euler_feder



subroutine euler_semi_feder(pp,t,dt,pps)
 
   implicit none
   
    DOUBLE PRECISION dt,t
    DOUBLE PRECISION pp(6),pps(6),k(6)
       
   !  print*, "start:", pp(1:6)
    Call feder(t,pps,k)
    pps(1:3)= pp(1:3)
    pps(4:6)=pp(4:6)+k(4:6)*dt
    ! print*, pps(1:6)
    
    Call feder(t,pps,k)
    
    pps(1:3)=pp(1:3)+k(1:3)*dt
  ! print*, pps(1:6)
  ! read*,t
  end subroutine euler_semi_feder

    subroutine euler_back_feder(pp,t,dt,pps)
 
   implicit none
   
    DOUBLE PRECISION dt,t,D
    DOUBLE PRECISION pp(6),pps(6),k(6)
     D=1.0d0 !Federkonstante  
   
    
    
    pps(4:4)= (pp(4:4)- dt*D*pp(1))/(1.0d0+dt**2*D)
    pps(1:1)= pp(1:1) + dt* pps(4:4)

  end subroutine euler_back_feder
 

subroutine rk4_feder(pp,t,dt,pps)
 
   implicit none
    DOUBLE PRECISION y
    DOUBLE PRECISION k1,k2,k3,k4,dt,dt_2,t,dt_3,dt_6
    DOUBLE PRECISION f
    DOUBLE PRECISION pp(6),pps(6),k(6),z(6)
  
      dt_2=dt/2.0d0
      dt_3=dt/3.0d0
      dt_6=dt/6.0d0
      
      Call feder(t,pp,k)
      pps=k*dt_6
      z=pp+dt_2*k
     
      Call feder(t+dt_2,z,k)
      pps= pps+ k*dt_3
      z=pp+dt_2*k

      Call feder(t+dt_2,z,k)
      pps= pps+ k*dt_3

      z=pp+dt*k

      Call feder(t+dt,z,k)
      pps= pps+ k*dt_6
      
    end subroutine rk4_feder

 subroutine feder(t,pin,pout)
     implicit none
    DOUBLE PRECISION t, pin(6),pout(6)
    double precision Bz,Bx,By,D
    D =1.0d0 ! Federkonstante
   
    pout(1)=pin(4)  ! 4=vx,5=vy,6=vz,1=x,2=y,3=z
    pout(2)=pin(5)
    pout(3)=pin(6)
    
    pout(4)=-D* pin(1) ! Bewegung in x-Richtung
    pout(5)=pin(5)
    pout(6)=pin(6)
    
  end subroutine feder




