! programa para integrar numericamente una orbita utilizando
! el metodo de Bulirsch - Stoer. Acepta hasta 50 objetos con
! o sin masa.
!
! Se asume que existe un objeto de masa 1 en el origen de 
! coordenadas. Las masas se expresan en fraccion de la masa
! del cuerpo central. Los objetos sin masa se indican con 
! fraccion 0.
!
! Los datos de entrada se leen del archivo IBS.IN en el 
! siguiente orden:
!
! cg y vtol      cte. Gauss y error del integrador
! T0 y Tf        instante inicial y final en DJ
! step y isal    paso de integracion y nro. de pasos para impresion
! ncuer          nro. de cuerpos en total
! nombre y rm    nombre (a12) y masa (frac. masa central)
! x,y,z          coord. eclipticas en UA
! vx,vy,vz       vel. eclipticas en UA/dia
! ...
!
! El program almacena las componentes de posicion y velocidad
! en la matriz X. Si las componentes de posicion son (x,y,z)
! y las de velocidad (vx,vy,vz), el orden de almacenamiento es:
!
! X(1)		x_1
! X(2)		vx_1
! X(3)		y_1
! X(4)		vy_1
! X(5)		z_1
! X(6)		vz_1
! X(7)		x_2
! ...
!
! El programa guarda informacion en un archivo de salida 
! denominado IBS.OUT cada ISAL pasos de integracion
!
! rgh - Septiembre 2008 (Curso de Mecanica)
!
!
program ibs
implicit none
  integer, parameter ::d=selected_real_kind(15,200)
  integer::isal,ncuer
  integer::i,j,jit,nec
  real(d)::cg,pi,eps,dir
  real(d)::vtol,t0,tf,step,step0
  real(d),dimension(300)::x
  real(d),dimension(50)::rm
  character(12),dimension(50)::nom
  real(d)::phi,rr
  real(d),dimension(3)::vec

  pi=4._d*atan(1._d)
  eps=23.43929111_d/180._d*pi
!  cg=0.01720209895_d

! lee archivo de entrada
!
  open(7,file='ibs.in',status='old')

! primero lee constantes
!
  read(7,*)cg,vtol
  read(7,*)t0,tf
  read(7,*)step,isal
  read(7,*)ncuer

! numero de ecuaciones
!
  nec=ncuer*6

! en que direccion temporal integrara?
!
  if(tf > t0)then
    dir=1._d
    step0=step
  else
    dir=-1._d
    step0=-step
  endif

! lee datos para cada objeto y guarda las
! coordenadas y velocidades en el orden
! que requiere el integrador (x,vx,y,vy,z,vz)
!
  j=0
  do i=1,ncuer
    read(7,*)nom(i),rm(i)
    read(7,*)x(j+1),x(j+3),x(j+5)
    read(7,*)x(j+2),x(j+4),x(j+6)
    j=j+6
    if(j >= 300)then
      write(*,'(/a)')'IBS acepta 50 objetos maximo.'
      stop
    endif
  enddo
  close(7)

! este es el cuerpo principal del integrador
!

!**************************************************
! archivos de salida que se deben modificar segun 
! la informacion que se desea guardar.
!
! aqui se va a guardar la informacion para cada objeto
! en un archivo diferente
!**************************************************
!
  open(7,file='ibs-1.out')
  open(8,file='ibs-2.out')
!  open(9,file='ibs.out')
  
!**************************************************
! hasta aqui. Recuerde cerrar los archivos antes de 
! salir!
!**************************************************

  jit=-1
  do

! si corresponde, guarda informacion en el
! archivo de salida
!
    if((jit == isal).or.(jit == -1))then

!**************************************************
! aqui es donde se obtiene la informacion de salida
! asi que es la parte a modificar si es necesario
!**************************************************
!

! saco tiempo a pantalla
!
      write(*,*)t0


! guardo (x,y,z) y (vx,vy,vz) para los dos 
! primeros objetos
!
      write(7,*)t0,nom(1),x(1),x(3),x(5),x(2),x(4),x(6)
      write(8,*)t0,nom(2),x(7),x(9),x(11),x(8),x(10),x(12)

! usando la posicion del objeto con masa, 
! calcula la posicion relativa de la particula
! para un sistema de coordenadas en rotacion
!
!      rr=sqrt(x(7)**2+x(9)**2+x(11)**2)
!      phi=atan2(x(9),x(7))-atan2(x(3),x(1))
!      if(phi < 0._d)phi=phi+2._d*pi
!      vec(1)=rr*cos(phi)
!      vec(2)=rr*sin(phi)
!      vec(3)=0._d
!      write(9,*),t0,nom(2),vec,rr,phi*180._d/pi

!**************************************************
! de aqui en adelante no modificar
!**************************************************
!     
      jit=0
    endif

    jit=jit+1

! si el ultimo paso lleva a un instante posterior
! al pedido, lo ajusta
!
    if((dir*t0+step0) > (dir*tf))step0=tf-t0

! llama a la subrutina que hace la integracion
!
    call stoer(nec,step0,vtol,t0,rm,cg,x)

    if((dir*t0) >= (dir*tf))exit
  enddo

!**************************************************
! se cierran los archivos de salida 
!**************************************************
!
  close(7)
  close(8)
!  close(9)

! guarda archivo de reinicio de la integracion
!
  open(7,file='ibs.rst')
  write(7,'(2g23.15)')cg,vtol
  write(7,*)t0,tf
  write(7,'(g23.15,i6)')step,isal
  write(7,'(i4)')ncuer

  j=0
  do i=1,ncuer
    write(7,'(a12,g23.15)')nom(i),rm(i)
    write(7,'(3g23.15)')x(j+1),x(j+3),x(j+5)
    write(7,'(3g23.15)')x(j+2),x(j+4),x(j+6)
    j=j+6
  enddo
  close(7)

end program ibs


!
!***********************************************************************
!
! subrutina que realiza un paso de integracion del sistema de
! ecuaciones utilizando el metodo de Bulirsch-Stoer.
!
! m: numero de ecuaciones de primer orden (m=nro.objetos * 6)
! ha: paso de integracion
! tol: tolerancia en el resultado
! rm: masas (frac. masa central)
! cg: constante de Gauss
! ya: vectores iniciales en el orden (x,xdot,y,ydot,z,zdot).
!    En la salida se obtiene la solucion del sistema.
! ta: instante para el que se da la solucion.
!
! El maximo numero de iteraciones permitidas es de 50.
!
! Esta subrutina llama a la subrutina EXTRAP que realiza las
! extrapolaciones y a FORCE que calcula la fuerza
!
subroutine stoer(m,ha,tol,ta,rm,cg,ya)
implicit none
  integer, parameter ::d=selected_real_kind(15,200)
  integer, parameter::np=4,ns=4,maxit=10,maxext=6
  integer,intent(in)::m
  real(d),intent(in)::ha,tol,cg
  real(d),dimension(50),intent(in)::rm
  real(d),intent(inout)::ta
  real(d),dimension(300),intent(inout)::ya
  integer::nstep,np2,n,i,i0,k,ip2,jf,jb,j,div
  real(d)::h,err,yy,dy,derr
  real(d),dimension(300)::y
  real(d),dimension(50,300)::v

  nstep=2**ns
  np2=2**np
  h=ha
  n=1

! itera maxit veces
!
  do i0=0,maxit
    i=i0+1
    err=-10._d

! extrapola los vectores
!
    call extrap(m,n,h,ta,cg,rm,ya,y)
    if(i == 1)then
      v(1,1:m)=y(1:m)
    else
      do k=1,m
        yy=y(k)
        v(i,k)=yy
        ip2=np2
        jf=i0
        if(jf > maxext)jf=maxext
        do jb=1,jf
          j=i-jb
          div=ip2-1
          dy=(yy-v(j,k))/dble(div)
          yy=yy+dy
          v(j,k)=yy
          ip2=nstep*ip2
        enddo
        derr=dabs(dy)
        if(derr > err)err=derr
      enddo
    endif
   
! si esta por debajo de la tolerancia sale
!
    if((err <= tol).and.(err /= -10._d))exit

! parte el intervalo en dos
!
    h=h/2._d
    n=2*n
  enddo

  if(i0 > maxit)then

! si entra aqui se requieren mas iteraciones
!
    write(*,'(/1x,a,i2,a,d10.3,a,d10.3)')'Iteraciones: ', maxit, &
      ' Error: ',err,' Tolerancia: ',tol
  endif

! finaliza el paso de integracion
!
  ta=ta+ha
  ya(1:m)=v(j,1:m)
  return
end subroutine stoer

!
!**********************************************************************
!
!     esta rutina hace las extrapolaciones
!     y llama a force que son las ecuaciones diferenciales
!     escritas como un sistema de primer orden
!
!     m: nro. de objetos * 6
!     n: fraccion de intervalo considerado en cada iteracion
!     h: paso de integracion total / n
!     ta: instante para el que se dan los vectores
!     cg: constante de Gauss
!     rm: masas (frac. masa central)
!     ya: vectores de posicion y velocidad en el orden (x, xdot, y,
!         ydot, z, zdot)
!     y: vectores de posicion y velocidad extrapolados
!
subroutine extrap(m,n,h,ta,cg,rm,ya,y)
implicit none
  integer, parameter ::d=selected_real_kind(15,200)
  integer,intent(in)::m,n
  real(d),intent(in)::h,ta,cg
  real(d),dimension(300),intent(in)::ya
  real(d),dimension(50),intent(in)::rm
  real(d),dimension(300),intent(out)::y
  integer::j
  real(d),dimension(300)::y0,dy
  real::h2,t

  h2=h/2.d0
  y0=ya 

!calcula para un instante medio entre ta y ta+h
!
  t=ta+h2
  call force(m,cg,rm,y0,dy)
  y=y0+h2*dy

! si no es la primera iteracion cubre el intervalo total
! calculando para instantes separados h/2
!
  if(n /= 1)then
    do j=2,n
      t=t+h2
      call force(m,cg,rm,y,dy)
      y0=y0+h*dy
      t=t+h2
      call force(m,cg,rm,y0,dy)
      y=y+h*dy
    enddo
  endif

! calcula para el instante final
!
  t=t+h2
  call force(m,cg,rm,y,dy)
  y0=y0+h*dy

! obtiene la estimacion extrapolada
!
  call force(m,cg,rm,y0,dy)
  y=(y0+y+h2*dy)/2._d

  return
end subroutine extrap

!
!**********************************************************************
!
!     Subrutina que calcula las fuerzas keplerianas y las perturbaciones
!     Internamente esta dise#ada para aceptar hasta 50 objetos, limite
!     impuesto por el array de reciprocas de las masas rm.
!
!     Acepta que los objetos sin masa ocupen la misma posicion en el
!     espacio para posibilitar condiciones iniciales correspondientes a
!     una colision, por ejemplo
!
!     m: nro. de cuerpos * 6
!     cg: cte. de Gauss
!     rm: masas (frac. masa central)
!     y: vectores de posicion y velocidad en el orden x, xdot, y, ydot,
!        z, zdot
!     dy: velocidades y aceleraciones
!
subroutine force(m,cg,rm,y,dy)
implicit none
  integer, parameter ::d=selected_real_kind(15,200)
  integer,intent(in)::m
  real(d),intent(in)::cg
  real(d),dimension(50),intent(in)::rm
  real(d),dimension(300),intent(in)::y
  real(d),dimension(300),intent(out)::dy
  integer::ncorp,i,j,nx,ny,nz,jx,jy,jz,mx,my,mz
  real(d),dimension(50)::r
  real(d),dimension(50,50)::ro
  real(d)::cx,cy,cz,w,hx,hy,hz,cdx,cdy,cdz
  real(d)::fac,fx,fy,fz,amm

  ncorp=m/6

! define distancias al sol y distancias mutuas
!
  do i=1,ncorp
    nx= 6*i-5
    ny= nx+2
    nz= ny+2

! calcula r^-3 para cada objeto
!
    cx=y(nx)
    cy=y(ny)
    cz=y(nz)
    w=dsqrt(cx**2+cy**2+cz**2)
    r(i)=1._d/w**3
    do j=i+1,ncorp
      jx=6*j-5
      jy=jx+2
      jz=jy+2

! calcula (r-r0)^-3 para cada par de objetos
!
      hx=y(jx)
      hy=y(jy)
      hz=y(jz)
      cdx=hx-cx
      cdy=hy-cy
      cdz=hz-cz
      w=dsqrt(cdx**2+cdy**2+cdz**2)
!      ro(i,j)=1._d/w**3
!
! para posibilitar integraciones con las
! particulas en la misma posicion inicial
! (formacion de familias, por ejemplo)
!
      if(w == 0._d)then
        ro(i,j)=0._d
      else 
        ro(i,j)=1._d/w**3
      endif
      ro(j,i)=ro(i,j)
    enddo
  enddo

! calcula fuerzas keplerianas
!
  do i=1,ncorp
    nx=i*6-5
    ny=nx+2
    nz=ny+2
    if(rm(i) == 0._d)then
      fac=-r(i)
    else
      fac=-(rm(i)+1.d0)*r(i)
    endif
    fx=fac*y(nx)
    fy=fac*y(ny)
    fz=fac*y(nz)

! calcula fuerzas perturbadoras
!
    do j=1,ncorp
      if(j /= i)then
        mx=j*6-5
        my=mx+2
        mz=my+2
        cdx=y(mx)-y(nx)
        cdy=y(my)-y(ny)
        cdz=y(mz)-y(nz)
        if(rm(j) == 0._d)then
          amm=0._d
        else
          amm=rm(j)
        endif
        fx=fx+amm*(cdx*ro(i,j)-y(mx)*r(j))
        fy=fy+amm*(cdy*ro(i,j)-y(my)*r(j))
        fz=fz+amm*(cdz*ro(i,j)-y(mz)*r(j))
      endif
    enddo

! velocidades y aceleraciones
!
    dy(nx)=y(nx+1)
    dy(nx+1)=cg**2*fx
    dy(ny)=y(ny+1)
    dy(ny+1)=cg**2*fy
    dy(nz)=y(nz+1)
    dy(nz+1)=cg**2*fz
  enddo
  return
end subroutine force