program pd_integration

implicit NONE

INTEGER, Parameter:: wp=8 ! set working precision to double precision
REAL(KIND=wp), parameter :: pi      = 3.1415926535897932384626433_wp
integer i, n_steps,n_gyro,test
REAL(KIND=wp) t,dt,estart
REAL(KIND=wp),  DIMENSION (:), ALLOCATABLE :: time      
REAL(KIND=wp),  DIMENSION (:,:), ALLOCATABLE :: p 
REAL(KIND=wp),  DIMENSION (:), ALLOCATABLE :: ps 
REAL(KIND=wp),external ::  gyro

! Initialise time step and length of integration

n_gyro=100
n_steps=10000

dt=DBLE(n_gyro)*2.0d0*pi/DBLE(n_steps)
!dt=DBLE(n_gyro)/DBLE(n_steps-1)
print*,"number of steps:",n_steps 
print*,"dt: ",dt

! store all variables for all time steps for demonstration purposes:
ALLOCATE(time(n_steps),p(6,n_steps) ,ps(6)) ! 6 phase space variables:3 spatial, 3 velocity space

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

! Initial conditions

time(1)=0.0d0
p(4,1)=1.0d0! vx(0)=1
p(6,1)=1.0d0! vz(0)=1

estart= (p(4,1)**2+p(5,1)**2+p(6,1)**2) ! initial energy

do i=2,n_steps,1
   time(i)=time(i-1)+dt
   
   !time(i)=time(i-1)+dt*2.0d0*pi
   t=time(i)
   
   
   ! choose integrator and problem to integrate:
   
   
   ! gyromotion: 
   
   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(:) ! save state-vector
   
   !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(:)
   
   
   
   
   ! 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="euler")
!do i= n_steps-1000,n_steps
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) ,(p(4,i)**2+p(5,i)**2+p(6,i)**2)/estart
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) ! calculate forces on particle
   
  pps=k*dt ! extrapolate to new state vector
    
end subroutine euler

subroutine gyro(t,pin,pout)! calculate forces on particle
  implicit none
  DOUBLE PRECISION t, pin(6),pout(6)
  double precision Bz,Bx,By,Ev,sign
  
  sign=1.0d0
  !Ev=0.001d0 ! set electric field
  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)=0.0d0
  
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 


