source: PSPA/madxPSPA/src/matchjc.f90 @ 430

Last change on this file since 430 was 430, checked in by touze, 11 years ago

import madx-5.01.00
File size: 21.6 KB
Line 
1      subroutine mtjac(ncon,nvar,strategy,cool,balance,random,          &
2     &nrep,bisec,cond,match_mode,                                       &
3     &tol,calls,call_lim,                                               &
4     &vect,dvect,fun_vec,                                               &
5     &w_ifjac,w_iwa4,fval,xstart,xold)
6
7      use matchfi
8      implicit none
9!----------------------------------------------------------------------*
10! Purpose:                                                             *
11!   JACOBIAN command.                                                  *
12! Attributes:                                                          *
13!   ncon      (int)     # constraints                                  *
14!   nvar      (int)     # variables                                    *
15!   strategy  (int)     # strategy   1 normal                          *
16!                                    2 print jacobian                  *
17!                                    3 cancel variables                *
18!   balance   (real)    # balance cooling factor, <0 use opt values    *
19!   cool      (real)    # cooling factor                               *
20!   random    (real)    # random  factor                               *
21!   bisec     (int)     # bisec iteration number                       *
22!   cond      (real)    # condition number for rank                    *
23!   match_mode(int)     # mode use_macro=2                             *
24!   nrep      (int)     # number of repetition                         *
25!   tol       (real)    Final tolerance for match.                     *
26!   calls     (int)     current call count                             *
27!   call_lim  (int)     current call limit                             *
28!   vect      (real)    variable values                                *
29!   dvect     (real)    variable steps                                 *
30!   fun_vect  (real)    function values                                *
31!   all other working spaces for jacobian                              *
32!----------------------------------------------------------------------*
33      integer calls,call_lim,ncon,nvar
34      integer strategy,nrep,i
35! icovar: functionality still unclear  HG 28.2.02
36! ilevel: print level
37      double precision tol,vect(*),dvect(*),fun_vec(*)
38      double precision w_ifjac(*),w_iwa4(*)
39      double precision fval(*),xold(*),xstart(*)
40      double precision random,cool,balance,cond
41      integer bisec,match_mode
42      external mtfcn
43
44      fval(1)=0
45
46! mtfcn store the parameter using mtputi(x) and compute the function
47! using mtcond
48
49      icovar = 0
50      ilevel = 0
51! check if the variables are in the contraint and reset them if necessary
52      call mtgeti(vect,dvect)
53! call the main routine
54      write(*,*) "JACOBIAN Strategy =", strategy
55
56!  Repeat the program ntimes
57!      write(*,*) nrep
58      do i=1,nrep
59        if (strategy .ge. 1) then
60!          calls=0
61          call jacob(mtfcn,ncon,nvar,strategy,calls,call_lim,           &
62     &vect,fun_vec,tol,                                                 &
63     &w_ifjac,w_iwa4,                                                   &
64     &xstart,xold,cool,balance,random,bisec,cond,match_mode)
65        endif
66      enddo
67      end
68
69
70      subroutine jacob(fcn,m,n,strategy,calls,call_lim,                 &
71     &x,fvec,epsfcn,                                                    &
72     &fjac,wa4,                                                         &
73     &xstart,xold,cool,balance,random,bisec,cond,match_mode)
74
75      use matchfi
76      implicit none
77!----------------------------------------------------------------------*
78! Purpose:                                                             *
79!   main JACOBIAN routine.                                             *
80! Attributes:                                                          *
81!   fcn       (real)    # function                                     *
82!   m         (int)     # constraints                                  *
83!   n         (int)     # variables                                    *
84!   strategy  (int)     # strategy   1 normal,2 print jacobian,3 smart *
85!   tol       (real)    Final tolerance for match.                     *
86!   calls     (int)     current call count                             *
87!   call_lim  (int)     current call limit                             *
88!   x         (real)    variable values                                *
89!   dvect     (real)    variable steps                                 *
90!   fvec     (real)    function values                                 *
91!                      mtfjac2 fjac,wa4                                *
92!   cool      (real)    # cooling factor                               *
93!   balance   (real)    # balance cooling factor                       *
94!   random    (real)    # random  factor                               *
95!   bisec     (int)     # bisec iteration number                       *
96!   cond      (real)    # condition number for rank                    *
97!   match_mode(int)     # mode use_macro=2                             *
98!----------------------------------------------------------------------*
99
100      integer izero
101      integer i,iflag,info,j,level,m,n,calls,call_lim
102      integer ireset,strategy,bisec,match_mode
103      double precision fmin_old,epsfcn
104      double precision ftol,gtol
105      double precision dxnorm,xnorm,dx(n),fmin_start,fmin_old2
106      double precision vdot
107      double precision xtol,x(n),xstart(n),xold(n),fvec(m)
108      double precision xopt(n),xbest(n),fminbest,condnum
109      double precision fjac(m,n),wa4(m),zero,one,two
110      double precision epsil,epsmch,cool,balance,random
111      parameter(zero=0d0,one=1d0,two=2d0, epsil=1d-9,epsmch=1d-16)
112      external fcn, mtcond
113
114!      double precision xdiff(n)
115      double precision effjac(M,N),effsol(M+N),effrhs(M+N)
116      double precision DNRM2
117      double precision WORK(1000*(N+M)),SV(N+M),COND
118      integer IWORK(30*(N+M)),RANK
119      integer effcon, effvar,coninfo(M),varinfo(N)
120
121      ireset = 0
122      izero = 0
123      info = 0
124      level = 0
125      ftol = epsfcn
126      gtol = epsil
127      xtol = epsil
128
129!---- Store the starting values in xstart for mtslope
130      do j = 1, n
131        xstart(j) = x(j)
132      enddo
133
134!---- Apply a cooling factor: reduce distance from solution to a point
135!---- defined by balance and the limit or opt values
136      call mtcool(x,cool,balance,xopt)
137
138!---- Apply a random factor:
139      call mtrandom(x,random)
140
141!---- Check if the limit is within the constraint and
142!---- reset the illegal values
143      call mtlimit(x,ireset)
144
145!---- Compute matching functions in fvec (penalty values)
146      call FCN(M,N,X,fvec,IFLAG)
147      calls=calls+1
148      if (iflag .ne. 0) then
149        call aawarn('JACOBIAN', ' stopped, possibly unstable')
150        info = - 1
151        go to 300
152      endif
153
154!---- Compute the norm of the function values (penalty values)
155      fmin = vdot(m,fvec,fvec)
156      write(*,'("Initial Penalty Function = ",e24.16,//)') fmin
157      fmin_start=fmin
158
159!---- store the first best set
160      fminbest=fmin
161      do j = 1, n
162        xbest(j) = x(j)
163      enddo
164
165!---- Check the input parameters for errors.
166      if (n .lt. 0                                                      &
167     &.or. ftol .lt. zero .or. xtol .lt. zero .or. gtol .lt. zero       &
168     &.or. call_lim .le. 0                      ) then
169        call aawarn('JACOBIAN', ' error in the input parameters')
170        go to 300
171      endif
172
173!---- Quit, when initial value is already OK
174!---- Do not apply for calculating jacobian
175      if (strategy.ne.2) then
176        if (fmin .le. ftol) then
177          call aawarn('JACOBIAN', ' penalty function already ok')
178          go to 300
179        endif
180      endif
181      if (ilevel .ge. 1) call mtprnt('old', n, x)
182
183
184!---- Start main loop
185  20  continue
186
187!---- Reset ireset
188      ireset=0
189
190!---- Calculate the jacobian
191      call fdjac2(fcn,m,n,x,fvec,fjac,m,iflag,xtol,wa4)
192
193      if (strategy.eq.2) then
194!        call DGESDD('A',M,N,fjac,M,SV,U,M,VT,N,                         &
195!     & WORK, 1000*(N+M), IWORK, INFO )
196!---- Print the jacobian on the match2 variables and exit
197        call jacob_print(n,match_mode)
198        goto 300
199      endif
200
201!---- Reset solution vector
202      do i=1,N+M
203        effsol(i)=0
204      enddo
205!---- Reset varinfo
206      do i=1,N
207        varinfo(i)=0
208      enddo
209
210!---- All the variables are affective
211      effvar=N
212
213!---- Cancel the zero lines in the jacobian corresponding to
214!---- an inequality that is not effective or variable not effective
215!---- Reset constraint counter
216      effcon=0
217      do i=1,M
218!---- Assume bad constraint: coninfo=1
219        coninfo(i)=1
220!---- Compute the norm of a row
221!---- 2-dim array are stored by colums
222        if (DNRM2(N,fjac(i,1),M).ge.1D-16) then
223!---- Good constraint: coninfo=0
224          coninfo(i)=0
225!---- Increase constraint counter
226          effcon=effcon+1
227!---- Copy RHS in the solution vector
228          effsol(effcon)=fvec(i)
229          do j=1,N
230!---- Update the effective jacobian
231            effjac(effcon,j)=fjac(i,j)
232          enddo
233!---- Save a copy of RHS
234          effrhs(effcon)=effsol(effcon)
235        endif
236      end do
237
238 33   continue
239
240
241!---- Solve the least square problem and put solution in effsol vector
242!---- Debug
243!      write(*,*) "!!! pre solve routine"
244!      write(*,*) "!!!effcon,effvar"
245!      write(*,*) "!!!",effcon,effvar
246!      write(*,*) "!!!i,j,effjac(i,j),effsol(j)"
247!      do i=1,effcon
248!      do j=1,effvar
249!      write(*,*) "!!!",i,j,effjac(i,j)
250!      enddo
251!      enddo
252!---- Debug
253!---- CALL DGELS(TRANSA, M, N, NRHS, DA, LDA, DB, LDB, DWORK,
254!---- LDWORK,INFO)
255      write(*,*) "Solve system with ",effcon,"con,",effvar,"var"
256!      call DGELS ('N',effcon,effvar,1,effjac,M,effsol,N+M,              &
257!     &WORK,2*(N+M),INFO)
258!      DGELSD( M, N, NRHS, A, LDA, B, LDB, S,  RCOND,
259!      RANK,  WORK, LWORK, IWORK, INFO )
260      call DGELSD (effcon,effvar,1,effjac,M,effsol,N+M,SV,COND,         &
261     &RANK,WORK,1000*(N+M),IWORK,INFO)
262      condnum=SV(1)/SV(min(effcon,effvar))
263!      call DGELSS (effcon,effvar,1,effjac,M,effsol,N+M,SV,RCOND,        &
264!     &RANK,WORK,1000*(N+M),INFO)
265      write(*,*) "Rank  ",RANK,                                         &
266     &"  Condition number ",condnum
267
268!---- Debug
269!      write(*,*) "!!! solve routine"
270!      write(*,*) "!!!",info,effsol(1)
271!      do j=1,n
272!      write(*,*) "!!!",j,effsol(j)
273!      enddo
274!---- Debug
275      if (info.lt.0) then
276        call aawarn('JACOBIAN', ' system solving routine failure')
277        print *, '++++++++++ JACOBIAN ended: DGELSD failure'
278        print *, '++++++++++ JACOBIAN ended: info = ', info
279        goto 300
280      endif
281      if (effsol(1).ne.effsol(1)) then
282        print *, '++++++++++ JACOBIAN ended: NaN in system solving'
283        goto 300
284      endif
285
286
287!---- Update the starting point
288      effvar=0
289      do i=1,N
290!----   Save in xold
291        xold(i)=x(i)
292        if (varinfo(i).eq.0) then
293          effvar=effvar+1
294          x(i)=x(i)-effsol(effvar)
295        endif
296      enddo
297
298
299!     Check for slope and limits and set the results in varinfo and
300!     the number of effective variables in effvar
301      if(strategy.eq.3) then
302        call mtvarinfo(x,xstart,varinfo,effvar)
303      endif
304
305!     If needed recalculate the solution excluding some variables
306      if ((effvar.lt.N).and.(effvar.ge.effcon)) then
307        write(*,*) "Reset system to ",effcon,"con,",effvar,"var"
308
309!----   Recalculate  effvar
310        effvar=0
311        do j=1,N
312!        write(*,*) j,varinfo(j)
313          if (varinfo(j).eq.0) then
314            effvar=effvar+1
315          endif
316        enddo
317
318!----   Restore the starting point
319        do i=1,N
320          x(i)=xold(i)
321        enddo
322
323!----   Reset RHS and jacobian
324        do i=1,M
325          do j=1,N
326            effjac(i,j)=0
327          enddo
328        enddo
329        do i=1,N+M
330          effsol(i)=0
331        enddo
332
333!----   Rewrite the effective jacobian
334        effcon=0
335        do i=1,M
336          if (coninfo(i).eq.0) then
337            effcon=effcon+1
338            effvar=0
339            do j=1,N
340              if (varinfo(j).eq.0) then
341                effvar=effvar+1
342                effsol(effcon)=effrhs(effcon)
343                effjac(effcon,effvar)=fjac(i,j)
344              endif
345            enddo
346          endif
347        enddo
348
349        if (ireset.eq.20) then
350          write(*,*) "Too loops in system resizing, set strategy=1"
351          strategy=1
352!----     Exit var cancel loop
353          goto 34
354        else
355          ireset=ireset+1
356        endif
357
358        if ((effvar.lt.effcon+1).or.(condnum.gt.1E10)) then
359!----     Impossible to get  better
360          write(*,*) "Too var to exclude, set strategy=1"
361          strategy=1
362!----     Exit var cancel loop
363          goto 34
364        endif
365
366!----   Solve the system again
367        goto 33
368      endif
369
370  34  continue
371
372!---- Check if the solution respect the slope and
373!---- reset the illegal values
374      call mtslope(x,xstart)
375
376!---- Check if the limit is within the constraint and
377!---- reset the illegal values
378      call mtlimit(x,ireset)
379
380!----- Calculate the penalty function
381      call FCN(M,N,x,fvec,IFLAG)
382
383      fmin_old=fmin
384      fmin = vdot(m, fvec,fvec)
385
386      xnorm=DNRM2(N, x, 1)
387      dxnorm=DNRM2(effvar, effsol, 1)/xnorm
388      write(*,*) 'Step length ', dxnorm
389
390      ! Bisection search
391      fmin_old2=1E20
392      j=0
39336    continue
394      if(fmin.ge.fmin_old .and. j.lt. bisec ) then
395        ! go back to average solution
396        do i=1,N
397          x(i)=(x(i)+xold(i))*.5
398          dx(i)=(x(i)-xold(i))*.5
399        enddo
400        j=j+1
401        call FCN(M,N,x,fvec,IFLAG)
402        fmin = vdot(m,fvec,fvec)
403        dxnorm=sqrt(vdot(N, dx, dx)/vdot(N, x, x))
404        if (fmin.gt.fmin_old2) then
405          goto 37
406        endif
407        fmin_old2=fmin
408        goto 36
409      endif
41037    continue
411
412      if (j.gt.0) then
413        write(*,*) 'Bisec iteration ', j
414      endif
415
416
417!---- calculate the target function and set new values
418      call FCN(M,N,x,fvec,IFLAG)
419      calls=calls+1
420
421      fmin = vdot(m,fvec,fvec)
422
423      xnorm=DNRM2(N, x, 1)
424      dxnorm=DNRM2(effvar, effsol, 1)/xnorm
425      write(*,"('call: ',I5,' Dx = ', e16.8,                            &
426     &'  Penalty function =',e24.16)")  calls,dxnorm,fmin
427
428!      Store the best point:
429      if ((fmin .lt. fminbest)) then
430        do i=1,N
431          xbest(i)=x(i)
432        enddo
433        fminbest=fmin
434      endif
435
436      ! check if the target explode
437      if((fmin .gt. 1D20)) then
438        print *, '++++++++++ JACOBIAN ended: infinity penalty function'
439        goto 300
440      endif
441
442      ! check if the function converge
443      if(fmin .le. ftol) then
444        print *, '++++++++++ JACOBIAN ended: converged successfully'
445        goto 300
446      endif
447      ! Iteration do not converge
448      ! For the first calls it can happen due to not fisical penalty
449      if((calls.ge.3).and.(abs(1-fmin/fmin_old).lt.1D-15) ) then
450!        print *, 'dbg',fmin,fmin_old,abs(1-fmin/fmin_old)
451        print *, '++++++++++ JACOBIAN ended: minimum found'
452        goto 300
453      endif
454      if(calls.ge.call_lim) then
455        print *, '++++++++++ JACOBIAN ended: call limit'
456        goto 300
457      endif
458
459!     restart main loop
460      goto 20
461
462!      open(25,file="jacobian.dat")
463!      write(25,*) "# ", m,n
464!      do i=1,m
465!      do j=1,n
466!      write(25,999) i,j,fjac(i,j)
467!      enddo
468!      enddo
469!      close(25)
470
471!      call jacob_print(n,match_mode)
472!  999 format(i3,i3,e16.8)
473  300 continue
474
475!     go back to the best solution
476      do i=1,N
477        x(i)=xbest(i)
478      enddo
479      ! set values to the previous solution
480      call FCN(M,N,X,fvec,IFLAG)
481
482!---- Store the final distance
483      do i = 1, n
484        dx(i)=(x(i)-xstart(i))
485      enddo
486
487!      xnorm=DNRM2(N, x, 1)
488      dxnorm=sqrt(DNRM2(N, dx, 1))
489      write(*,*) 'Final difference norm:',dxnorm
490
491      end
492
493      subroutine jacob_print(n,match_mode)
494
495      use name_lenfi
496      implicit none
497
498      integer n,ivar,nvar,match_mode
499      logical local
500      integer ncon,next_constraint,next_global,i,j,pos,type,range(2),   &
501     &flag,get_option,restart_sequ,advance_to_pos,string_from_table_row
502!      integer double_from_table_row
503      double precision value,c_min,c_max,weight
504      character*(name_len) namevar,name,node_name
505      integer next_vary,slope
506      double precision step,opt
507      integer oldpos,nnode,mtputconsname,void
508
509
510      if(match_mode.eq.1) then
511
512        ncon=1
513        nnode=0
514        local=get_option('match_local ') .ne. 0
515        call table_range('twiss ','#s/#e ',range)
516        if(local) then
517          j=restart_sequ()
518          oldpos=range(1)
519          do pos=range(1),range(2)
520            j=advance_to_pos('twiss ',pos)
521 20         continue
522            i=next_constraint(name,name_len,type,value,c_min,c_max,weight)
523            if(i.ne.0)  then
524              if (pos.ne.oldpos) then
525                nnode=nnode+1
526                ncon=1
527                oldpos=pos
528              endif
529              flag=string_from_table_row('twiss ','name ',pos,node_name)
530              do nvar=1,n
531 22             ivar=next_vary(namevar,name_len,c_min,c_max,step,slope,opt)
532                if (ivar.eq.0) then
533                  goto 22
534                endif
535                void=mtputconsname(node_name,nnode,name,ncon)
536              enddo
537              ncon=ncon+1
538              goto 20
539            endif
540          enddo
541        endif
542        nnode=nnode+1
543 30     continue
544        i=next_global(name,name_len,type,value,c_min,c_max,weight)
545        if(i.ne.0)  then
546          pos=1
547          do nvar=1,n
548 32         ivar=next_vary(namevar,name_len,c_min,c_max,step,slope,opt)
549            if (ivar.eq.0) then
550              goto 32
551            endif
552            void=mtputconsname('Global              ',nnode,name,ncon)
553          enddo
554          ncon=ncon+1
555          goto 30
556        endif
557
558      endif
559
560  996 format(a)
561  997 format(3(a16,1x),a16)
562  998 format(3(a16,1x),e16.8)
563  999 format(i3,i3,e16.8)
564      end
565
566      subroutine mtmove(vect,varinfo,dir,balance)
567      use name_lenfi
568      implicit none
569      integer j,next_vary,slope
570      integer varinfo,effvar
571      double precision vect(*),c_min,c_max,step,opt
572      double precision val,dir,balance
573      character*(name_len) name
574
575      effvar=0
576 1    continue
577      j = next_vary(name,name_len,c_min,c_max,step,slope,opt)
578      if (j .ne. 0)  then
579        if (varinfo(j).eq.0) then
580          effvar=effvar+1
581          if (opt.ge.0) then
582            val =vect(effvar)+dir*opt
583          else
584            val =vect(effvar)+dir*((1-balance)*c_max+balance*c_min)
585          endif
586          vect(effvar) = val
587          goto 1
588        endif
589      endif
590      end
591
592
593      subroutine mtcool(vect,cool,balance,xopt)
594      use name_lenfi
595      implicit none
596      integer j,next_vary,slope
597      double precision vect(*),c_min,c_max,step,opt
598      double precision val,xopt(*),cool,balance
599      character*(name_len) name
600
601      if (cool.gt.0) then
602        write(*,'(4a16)') "name","oldvalue","opt value","new value"
603      endif
604 1    continue
605      j = next_vary(name,name_len,c_min,c_max,step,slope,opt)
606      if (j .ne. 0)  then
607        if (opt.gt.0) then
608          xopt(j)=opt
609        else
610          xopt(j)=(1-balance)*c_max+balance*c_min
611        endif
612        val=(1-cool)*vect(j)+cool*xopt(j)
613        if (cool.gt.0) then
614          write(*,'(a15,3e16.5)') name,vect(j),xopt(j),val
615        endif
616        vect(j) = val
617        goto 1
618      endif
619      end
620
621      subroutine mtrandom(vect,random)
622      use name_lenfi
623      implicit none
624      integer j,next_vary,slope
625      double precision vect(*),c_min,c_max,step,opt
626      double precision val,random,frndm
627      character*(name_len) name
628
629 1    continue
630      j = next_vary(name,name_len,c_min,c_max,step,slope,opt)
631      if (j .ne. 0)  then
632        val = (1+ random *( frndm() - 0.5) ) * vect(j)
633        vect(j) = val
634        goto 1
635      endif
636      end
637
638      subroutine mtslope(x,xstart)
639
640      use name_lenfi
641      implicit none
642
643
644      integer j,next_vary,slope
645      double precision x(*),xstart(*),c_min,c_max,step,opt
646      double precision diff
647      character*(name_len) name
648
649 1    continue
650      j = next_vary(name,name_len,c_min,c_max,step,slope,opt)
651      if (j .ne. 0)  then
652        if (slope.ne.0) then
653          diff=x(j)-xstart(j)
654          if(slope*diff.lt.0) then
655            write(*,831) "reset parameter:",name,                       &
656     &"from",x(j),"to",xstart(j)
657            x(j)=xstart(j)
658          endif
659        endif
660        goto 1
661      endif
662  831 format(a16,1x,a24,a4,e16.8,a4,e16.8)
663      end
664
665
666      subroutine mtvarinfo(x,xstart,varinfo,effvar)
667
668      use name_lenfi
669      implicit none
670
671
672      integer j,next_vary,slope,varinfo(*),effvar
673      double precision x(*),xstart(*),c_min,c_max,step,opt
674      double precision diff,val,oldval
675      character*(name_len) name
676
677      effvar=0
678 1    continue
679      j = next_vary(name,name_len,c_min,c_max,step,slope,opt)
680      if (j .ne. 0)  then
681!        varinfo(j)=0
682        effvar=effvar+1
683        val=x(j)
684        oldval=xstart(j)
685        if (slope.ne.0) then
686          diff=val-oldval
687          if(slope*diff.lt.0) then
688            write(*,*) "exclude parameter:",name,"bad slope"
689            varinfo(j)=1
690            effvar=effvar-1
691          endif
692        endif
693        if (val.lt.c_min) then
694          write(*,*) "exclude parameter:",name,"hit minimum"
695          varinfo(j)=1
696          effvar=effvar-1
697        endif
698        if (val.gt.c_max) then
699          write(*,*) "exclude parameter:",name,"hit maximum"
700          varinfo(j)=1
701          effvar=effvar-1
702        endif
703        goto 1
704      endif
705  831 format(a16,1x,a24,a4,e16.8,a4,e16.8)
706      end
707
708!  TODO
709!  Find target function in collect
710!  Find variable constraint in vary
711
712!  Loop for divide the funtion in steps
713
714
715
716      subroutine mtsvd(M,N,fjac,SV,U,VT)
717      implicit none
718      integer M,N,IWORK(30*(N+M)),INFO
719      double precision fjac(m,n),SV(N+M),U(M,M),VT(N,N),WORK(1000*(N+M))
720      call DGESDD('A',M,N,fjac,M,SV,U,M,VT,N,WORK,1000*(N+M),IWORK,INFO)
721      end
Note: See TracBrowser for help on using the repository browser.