program opttraj

  include 'params.f90'
  integer :: n  ! number of points describing the trajectory
  real,dimension (:),allocatable:: dt,x,y,ax,ay,vx,vy,x1,y1,x2,y2
  !
  !  dt = array of time intervals in between points of the trajectory
  !  x,y,vx,vy,ax,ay  positions speeds and accelerations on the trajectory
  !  x1 y1 x2 y2 inside and outside of the curve
  !  
  
  
  
  real :: dtold,axold,ayold   ! to be saved during Monte Carlo annealing
  integer :: i,nyes,nbut,nt,test,nyesc,nyesmc,nref,nmc,n1,mloop,k

  real :: R,w,Vmax2,ymin,vv,t,told,an,at
  
  ! nyes number of accepted good moves
  ! nbut number of accepted bad moves (due to Metropolis method)
  ! nt total number of moves
  ! test does the intended move satisfy constraints ?
  ! nyesc number of moves satisfying constraints
  ! nyesmc number of moves accepted by the Metropolis method
  
  
  
  nbut=0  
  nyesc=0 
  nyesmc=0 
  nref=0
  nmc=0

  
  
  ! initialize trajectory
  R=(R1+R2)/2
  Vmax2=sqrt(0.5*g*R)
  w=Vmax2/R
  n1=3.1415/2/w/dti   
  
  n=3*n1         ! number of points describing the initial trajectory
  !, as two orthogonal straight lines joined by an arc circle
  
  allocate(x(n))
  allocate(y(n))
  allocate(dt(n))
  allocate(ax(n))
  allocate(ay(n))
  allocate(vx(n))
  allocate(vy(n))
  allocate(x1(n))
  allocate(y1(n))
  allocate(x2(n))
  allocate(y2(n))
  
  vv=R/(n1*dti)
  ymin=-(n1-1)*dti*vmax2
  vx(1)=0.0
  vy(1)=vmax2
  do i=1,n1
     t=(i-1)*dti
     dt(i)=dti
     x(i)=R
     y(i)=ymin+t*vmax2       ! straight line
     ax(i)=0.0
     ay(i)=0.0
     x1(i)=R1
     y1(i)=ymin+t*vmax2
     x2(i)=R2
     y2(i)=ymin+t*vmax2
  enddo

  
  DO i=n1,2*n1
     t=(i-n1)*dti
     dt(i)=dti
     x(i)=R*cos(w*t)
     y(i)=R*sin(w*t)
     ax(i)=-w*R*w*cos(w*t)   ! arc circle
     ay(i)=-w*R*w*sin(w*t)
     x1(i)=R1*cos(w*t)
     y1(i)=R1*sin(w*t)
     x2(i)=R2*cos(w*t)
     y2(i)=R2*sin(w*t)
  ENDDO
  do i=2*n1,n
     t=(i-2*n1)*dti
     dt(i)=dti
     x(i)=-t*w*R
     y(i)=R                 ! straight line
     ax(i)=0.0
     ay(i)=0.0
     x1(i)=-t*R1*w
     y1(i)=R1
     x2(i)=-t*R2*w
     y2(i)=R2
  enddo
  xn=x(n)                  ! last points of the curve : used when
  ! optimizing trajectory to keep them fixed 
  yn=y(n)
  do mloop=1,maxmloop
     t=0.0
     told=1e12
     do k=1+maxmc*(mloop-1),maxmc*(mloop)    ! succession of
        ! annealing steps 
        ! 
1       continue 
        i=(n+1)*ran1(idum)! random point to be changed
        if(i.eq.0) goto 1   
        axold=ax(i)
        ayold=ay(i)
        dtold=dt(i)
        xold=x(1) 
        yold=y(1)         ! saving the trajectory
        vxold=vx(1)
        vyold=vy(1)
        told=temps(x,y,vx,vy,ax,ay,n,dt,xn,yn) 
        
        nmc=nmc+1
        CALL montecarlo(dt,x,y,vx,vy,ax,ay,i,k,n,tper,idum) 
        t=temps(x,y,vx,vy,ax,ay,n,dt,xn,yn)
        call contraintes(xn,yn,x,y,vx,vy,ax,ay,i,k,n,test)
        
        IF (test.EQ.1) THEN
           
           w=min(1.,exp(-(t-told)/tper))    ! tper is the fictious temperature
        
           nyesc=nyesc+1!moves satisfying constraints
           if (ran1(idum).gt.w) then ! Metropolis step refused
              nref=nref+1
              ax(i)=axold
              ay(i)=ayold
              dt(i)=dtold
              x(1)=xold
              y(1)=yold
              vx(1)=vxold
              vy(1)=vyold
           else
              if(t.gt.told) then 
                 nbut=nbut+1       
              else
                 nyesmc=nyesmc+1
              endif
           ENDIF
        else
           ax(i)=axold   ! trial move violates constraints
           ay(i)=ayold
           x(1)=xold
           y(1)=yold
           vx(1)=vxold
           vy(1)=vyold
           dt(i)=dtold
        endif
        if(mod(nmc,ipasinf).eq.0) then
           write(6,*) '--------------------------------------'
           write(6,*) 'Annealing loop                        ',mloop
           write(6,*) 'iteration in loop                     ',k
           write(6,*) 'actual trajectory time                ',told
           write(6,*) 'fictious temperature                  ',tper
           write(6,*) 'number of mc steps                    ',nmc
           write(6,*) 'number of steps satisfying constraints',nyesc
           write(6,*) 'number of accepted steps              ',nyesmc
           write(6,*) 'number of accepted defavourable steps ',nbut
           write(6,*) 'number of refused steps               ',nref
           if(nref.ne.0) write(6,*) 'ratio of refused to accepted',nbut/(nref*1.0)
           write(6,*) 
           write(6,*)
           write(6,*)
           open(unit=11,file='trajectory',status='unknown')
           open(unit=12,file='speed',status='unknown')
           open(unit=13,file='acceleration',status='unknown')
           DO i=1,n
              
              an=accelerationn(x,y,vx,vy,ax,ay,i,n)
              at=accelerationt(x,y,vx,vy,ax,ay,i,n)
                
              write(11,100) x(i),y(i),x1(i),y1(i),x2(i),y2(i) !
              ! saving trajectory
              write(12,101) i,sqrt(vx(i)*vx(i)+vy(i)*vy(i))! 100 n° de la carte
              write(13,101) i,an,at,sqrt(an*an+at*at),ax(i),ay(i)
100           format(f15.8,f15.8,f15.8,f15.8,f15.8,f15.8,f15.8)
101           format(i5,f15.8,f15.8,f15.8,f15.8,f15.8,f15.8,f15.8)
           ENDDO
           close(11)
           close(12)
           close(13)
        endif
     ENDdo
  enddo
end program opttraj





FUNCTION accelerationn(x,y,vx,vy,ax,ay,i,n) result(an) 
include 'params.f90'
! normal  acceleration
  INTEGER, INTENT (IN) :: i,n
  real,dimension(n):: x,y,ax,ay,vx,vy
  real :: nx,ny,tx,ty,v
 
  
     
  v=sqrt(vx(i)*vx(i)+vy(i)*vy(i))  
  
  tx=vx(i)/v    ! tangent vector at t
  ty=vy(i)/v    
  
     
  nx=-ty   !  normal vector at t
  ny=tx
     
  an=(ax(i)*nx+ay(i)*ny)   ! projection of the acceleration on the normal vector
   

end FUNCTION accelerationn
  
FUNCTION accelerationt(x,y,vx,vy,ax,ay,i,n) result(at)
include 'params.f90'
  INTEGER, INTENT (IN) :: i,n
  real,dimension(n):: x,y,ax,ay,vx,vy
  real :: tx,ty,v
  

  v=sqrt(vx(i)*vx(i)+vy(i)*vy(i))
  tx=vx(i)/v    ! tangent vector at t
  ty=vy(i)/v    
  
 
  at=ax(i)*tx+ay(i)*ty   ! projection of the acceleration on the tangent vector
     

end FUNCTION accelerationt
  
    
FUNCTION temps(x,y,vx,vy,ax,ay,n,dt,xn,yn) result(t) ! total
  ! trajectory time
include 'params.f90'
  INTEGER, INTENT (INOUT) :: n
  real,dimension(n):: x,y,vx,vy,ax,ay,dt
  real :: t,vvx,vvy,xx,yy
  integer :: i
  t=0.0
  vvx=vx(1)
  vvy=vy(1)
  xx=x(1)
  yy=y(1)
  DO i=2,n
     t=t+dt(i)
     vvx=vvx+ax(i)*dt(i)
     vvy=vvy+ay(i)*dt(i)
     xx=xx+vvx*dt(i)
     yy=yy+vvy*dt(i)
     vx(i)=vvx
     vy(i)=vvy
     x(i)=xx
     y(i)=yy
 ENDDO
 
 return
end FUNCTION temps

subroutine montecarlo(dt,x,y,vx,vy,ax,ay,ik,k,n,temp,idum)
include 'params.f90'
  INTEGER, INTENT(IN) :: n,k,ik
  real,dimension(n) :: dt,ax,ay,x,y,vx,vy
  real,intent(out) :: temp
  real :: facrer,tper,tpert,tpert2,p
  integer :: j

  temp=temp0*(((k+1)*1.0)**(-0.3))  ! annealing law as a function of
  ! iteration number k
  facrer=(16./(1.-sqrt(temp)))**(1./6.)   
  facrer=facrer-(16./(1.+sqrt(temp)))**(1./6.)   
  
  tper=tper0*facrer
  tpert=tper*0.3   ! for the time intervals
  tpert2=tper*0.03 ! for the first point of the trajectory
  

  j=3*ran1(idum)  ! which coordinate to be changed
  IF (j==1)THEN
     ax(ik)=ax(ik)+tper*1.0*(ran1(idum)-0.5)
  ENDIF
  IF (j==2)THEN
     ay(ik)=ay(ik)+tper*1.0*(ran1(idum)-0.5)
  ENDIF
  IF (j==0)THEN
     p=dt(ik)+tpert*1.0*(ran1(idum)-0.5)
     if(p.gt.0.0) dt(ik)=p ! reject negative time intervals
  endif
  if(ik.eq.1) then  
     IF (j==0)THEN
        x(1)=x(1)+tpert2*1.0*(ran1(idum)-0.5)
     ENDIF
     IF (j==1)THEN
        vx(1)=vx(1)+tpert2*1.0*(ran1(idum)-0.5) ! changing the first
        ! points of the trajectory
     ENDIF
     IF (j==2)THEN
        vy(1)=vy(1)+tpert2*1.0*(ran1(idum)-0.5)
     ENDIF
  endif
end subroutine montecarlo

subroutine contraintes(xn,yn,x,y,vx,vy,ax,ay,inn,k,n,t1) ! check if
  ! moves satisfies constraints 
include 'params.f90'
  INTEGER,INTENT(IN) :: n
  real,dimension(n) :: x,y,vx,vy,ax,ay,x1,x2,y1,y2
  real :: Rv,dt1,dt2,dt3
  integer :: test
  integer,intent(out)::t1
 
  
  TEST=1
  if(((x(n)-xn)**2+(y(n)-yn)**2).gt.radiusend) then
     test=test*0  ! if we are more than one meter away from the last
     ! point 
  endif
  do i=1,n  
     an=accelerationn(x,y,vx,vy,ax,ay,i,n)
     if(abs(an).gt.g) then
        test=test*0
     endif
     at=accelerationt(x,y,vx,vy,ax,ay,i,n)
     if(abs(at).gt.g) then
        test=test*0
     endif
     

 
     v=sqrt(vx(i)*vx(i)+vy(i)*vy(i))
     Rv=sqrt(x(i)*x(i)+y(i)*y(i))

 
     if(v.GT.Vmax) then 
        test=test*0
     endif
     if(v.lt.Vmin) then
        test=test*0
     endif


     
     if((x(i).gt.0.0).and.(y(i).gt.0.0)) then ! to be improved, one is in the first quadrant
        if((Rv.gT.R2).or.(Rv.lT.R1)) then     ! one is outside the road
           test=test*0
        endif
     endif ! one is on the straight line
     if((x(i).lt.0.0).and.(y(i).gt.0.0)) then ! to be improved in general
        IF ((y(i).LT.R1).OR.(y(i).GT.R2)) THEN ! one is outside the road
           test=test*0
        ENDIF
     endif
     if((x(i).lt.0.0).and.(y(i).lt.0.0)) then ! to be improved in general         
        test=test*0
     endif
     if((x(i).gt.0.0).and.(y(i).lt.0.0)) then ! to be improved in general         
        IF ((x(i).LT.R1).OR.(x(i).GT.R2)) THEN ! one is outside the road
           test=test*0
        ENDIF
     endif
  enddo
  t1=test
  
  return
END subroutine  contraintes

!
!----------ran1-------------
!
! completely basic (and wrong) random number generator
! replace with your own !
!
      function ran1(idum)
      idum=123456789*idum+987654321
      idum=mod(idum,2**30)
      ran1=idum/(2**30*1.0)
      if(ran1.lt.0.0) ran1=ran1+1.0
      return
      end

























