Skip to content
mpfit.pro 141 KiB
Newer Older
Antonino Francesco Lanza's avatar
Antonino Francesco Lanza committed
2001 2002 2003 2004 2005 2006 2007 2008 2009 2010 2011 2012 2013 2014 2015 2016 2017 2018 2019 2020 2021 2022 2023 2024 2025 2026 2027 2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038 2039 2040 2041 2042 2043 2044 2045 2046 2047 2048 2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062 2063 2064 2065 2066 2067 2068 2069 2070 2071 2072 2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084 2085 2086 2087 2088 2089 2090 2091 2092 2093 2094 2095 2096 2097 2098 2099 2100 2101 2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114 2115 2116 2117 2118 2119 2120 2121 2122 2123 2124 2125 2126 2127 2128 2129 2130 2131 2132 2133 2134 2135 2136 2137 2138 2139 2140 2141 2142 2143 2144 2145 2146 2147 2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160 2161 2162 2163 2164 2165 2166 2167 2168 2169 2170 2171 2172 2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183 2184 2185 2186 2187 2188 2189 2190 2191 2192 2193 2194 2195 2196 2197 2198 2199 2200 2201 2202 2203 2204 2205 2206 2207 2208 2209 2210 2211 2212 2213 2214 2215 2216 2217 2218 2219 2220 2221 2222 2223 2224 2225 2226 2227 2228 2229 2230 2231 2232 2233 2234 2235 2236 2237 2238 2239 2240 2241 2242 2243 2244 2245 2246 2247 2248 2249 2250 2251 2252 2253 2254 2255 2256 2257 2258 2259 2260 2261 2262 2263 2264 2265 2266 2267 2268 2269 2270 2271 2272 2273 2274 2275 2276 2277 2278 2279 2280 2281 2282 2283 2284 2285 2286 2287 2288 2289 2290 2291 2292 2293 2294 2295 2296 2297 2298 2299 2300 2301 2302 2303 2304 2305 2306 2307 2308 2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320 2321 2322 2323 2324 2325 2326 2327 2328 2329 2330 2331 2332 2333 2334 2335 2336 2337 2338 2339 2340 2341 2342 2343 2344 2345 2346 2347 2348 2349 2350 2351 2352 2353 2354 2355 2356 2357 2358 2359 2360 2361 2362 2363 2364 2365 2366 2367 2368 2369 2370 2371 2372 2373 2374 2375 2376 2377 2378 2379 2380 2381 2382 2383 2384 2385 2386 2387 2388 2389 2390 2391 2392 2393 2394 2395 2396 2397 2398 2399 2400 2401 2402 2403 2404 2405 2406 2407 2408 2409 2410 2411 2412 2413 2414 2415 2416 2417 2418 2419 2420 2421 2422 2423 2424 2425 2426 2427 2428 2429 2430 2431 2432 2433 2434 2435 2436 2437 2438 2439 2440 2441 2442 2443 2444 2445 2446 2447 2448 2449 2450 2451 2452 2453 2454 2455 2456 2457 2458 2459 2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470 2471 2472 2473 2474 2475 2476 2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488 2489 2490 2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2519 2520 2521 2522 2523 2524 2525 2526 2527 2528 2529 2530 2531 2532 2533 2534 2535 2536 2537 2538 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585 2586 2587 2588 2589 2590 2591 2592 2593 2594 2595 2596 2597 2598 2599 2600 2601 2602 2603 2604 2605 2606 2607 2608 2609 2610 2611 2612 2613 2614 2615 2616 2617 2618 2619 2620 2621 2622 2623 2624 2625 2626 2627 2628 2629 2630 2631 2632 2633 2634 2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671 2672 2673 2674 2675 2676 2677 2678 2679 2680 2681 2682 2683 2684 2685 2686 2687 2688 2689 2690 2691 2692 2693 2694 2695 2696 2697 2698 2699 2700 2701 2702 2703 2704 2705 2706 2707 2708 2709 2710 2711 2712 2713 2714 2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737 2738 2739 2740 2741 2742 2743 2744 2745 2746 2747 2748 2749 2750 2751 2752 2753 2754 2755 2756 2757 2758 2759 2760 2761 2762 2763 2764 2765 2766 2767 2768 2769 2770 2771 2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806 2807 2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819 2820 2821 2822 2823 2824 2825 2826 2827 2828 2829 2830 2831 2832 2833 2834 2835 2836 2837 2838 2839 2840 2841 2842 2843 2844 2845 2846 2847 2848 2849 2850 2851 2852 2853 2854 2855 2856 2857 2858 2859 2860 2861 2862 2863 2864 2865 2866 2867 2868 2869 2870 2871 2872 2873 2874 2875 2876 2877 2878 2879 2880 2881 2882 2883 2884 2885 2886 2887 2888 2889 2890 2891 2892 2893 2894 2895 2896 2897 2898 2899 2900 2901 2902 2903 2904 2905 2906 2907 2908 2909 2910 2911 2912 2913 2914 2915 2916 2917 2918 2919 2920 2921 2922 2923 2924 2925 2926 2927 2928 2929 2930 2931 2932 2933 2934 2935 2936 2937 2938 2939 2940 2941 2942 2943 2944 2945 2946 2947 2948 2949 2950 2951 2952 2953 2954 2955 2956 2957 2958 2959 2960 2961 2962 2963 2964 2965 2966 2967 2968 2969 2970 2971 2972 2973 2974 2975 2976 2977 2978 2979 2980 2981 2982 2983 2984 2985 2986 2987 2988 2989 2990 2991 2992 2993 2994 2995 2996 2997 2998 2999 3000
    r[j:n-1,j] = r[j,j:n-1]
  x = r[delm]
  wa = qtb
  ;; Below may look strange, but it's so we can keep the right precision
  zero = qtb[0]*0.
  half = zero + 0.5
  quart = zero + 0.25

  ;; Eliminate the diagonal matrix d using a givens rotation
  for j = 0L, n-1 do begin
      l = ipvt[j]
      if diag[l] EQ 0 then goto, STORE_RESTORE
      sdiag[j:*] = 0
      sdiag[j] = diag[l]

      ;; The transformations to eliminate the row of d modify only a
      ;; single element of (q transpose)*b beyond the first n, which
      ;; is initially zero.

      qtbpj = zero
      for k = j, n-1 do begin
          if sdiag[k] EQ 0 then goto, ELIM_NEXT_LOOP
          if abs(r[k,k]) LT abs(sdiag[k]) then begin
              cotan  = r[k,k]/sdiag[k]
              sine   = half/sqrt(quart + quart*cotan*cotan)
              cosine = sine*cotan
          endif else begin
              tang   = sdiag[k]/r[k,k]
              cosine = half/sqrt(quart + quart*tang*tang)
              sine   = cosine*tang
          endelse
          
          ;; Compute the modified diagonal element of r and the
          ;; modified element of ((q transpose)*b,0).
          r[k,k] = cosine*r[k,k] + sine*sdiag[k]
          temp = cosine*wa[k] + sine*qtbpj
          qtbpj = -sine*wa[k] + cosine*qtbpj
          wa[k] = temp

          ;; Accumulate the transformation in the row of s
          if n GT k+1 then begin
              temp = cosine*r[k+1:n-1,k] + sine*sdiag[k+1:n-1]
              sdiag[k+1:n-1] = -sine*r[k+1:n-1,k] + cosine*sdiag[k+1:n-1]
              r[k+1:n-1,k] = temp
          endif
ELIM_NEXT_LOOP:
      endfor

STORE_RESTORE:
      sdiag[j] = r[j,j]
      r[j,j] = x[j]
  endfor

  ;; Solve the triangular system for z.  If the system is singular
  ;; then obtain a least squares solution
  nsing = n
  wh = where(sdiag EQ 0, ct)
  if ct GT 0 then begin
      nsing = wh[0]
      wa[nsing:*] = 0
  endif

  if nsing GE 1 then begin
      wa[nsing-1] = wa[nsing-1]/sdiag[nsing-1] ;; Degenerate case
      ;; *** Reverse loop ***
      for j=nsing-2,0,-1 do begin  
          sum = total(r[j+1:nsing-1,j]*wa[j+1:nsing-1])
          wa[j] = (wa[j]-sum)/sdiag[j]
      endfor
  endif

  ;; Permute the components of z back to components of x
  x[ipvt] = wa

;  profvals.qrsolv = profvals.qrsolv + (systime(1) - prof_start)
  return
end
      
  
;
;     subroutine lmpar
;
;     given an m by n matrix a, an n by n nonsingular diagonal
;     matrix d, an m-vector b, and a positive number delta,
;     the problem is to determine a value for the parameter
;     par such that if x solves the system
;
;     a*x = b ,   sqrt(par)*d*x = 0 ,
;
;     in the least squares sense, and dxnorm is the euclidean
;     norm of d*x, then either par is zero and
;
;     (dxnorm-delta) .le. 0.1*delta ,
;
;     or par is positive and
;
;     abs(dxnorm-delta) .le. 0.1*delta .
;
;     this subroutine completes the solution of the problem
;     if it is provided with the necessary information from the
;     qr factorization, with column pivoting, of a. that is, if
;     a*p = q*r, where p is a permutation matrix, q has orthogonal
;     columns, and r is an upper triangular matrix with diagonal
;     elements of nonincreasing magnitude, then lmpar expects
;     the full upper triangle of r, the permutation matrix p,
;     and the first n components of (q transpose)*b. on output
;     lmpar also provides an upper triangular matrix s such that
;
;      t   t         t
;     p *(a *a + par*d*d)*p = s *s .
;
;     s is employed within lmpar and may be of separate interest.
;
;     only a few iterations are generally needed for convergence
;     of the algorithm. if, however, the limit of 10 iterations
;     is reached, then the output par will contain the best
;     value obtained so far.
;
;     the subroutine statement is
;
; subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
;      wa1,wa2)
;
;     where
;
; n is a positive integer input variable set to the order of r.
;
; r is an n by n array. on input the full upper triangle
;   must contain the full upper triangle of the matrix r.
;   on output the full upper triangle is unaltered, and the
;   strict lower triangle contains the strict upper triangle
;   (transposed) of the upper triangular matrix s.
;
; ldr is a positive integer input variable not less than n
;   which specifies the leading dimension of the array r.
;
; ipvt is an integer input array of length n which defines the
;   permutation matrix p such that a*p = q*r. column j of p
;   is column ipvt(j) of the identity matrix.
;
; diag is an input array of length n which must contain the
;   diagonal elements of the matrix d.
;
; qtb is an input array of length n which must contain the first
;   n elements of the vector (q transpose)*b.
;
; delta is a positive input variable which specifies an upper
;   bound on the euclidean norm of d*x.
;
; par is a nonnegative variable. on input par contains an
;   initial estimate of the levenberg-marquardt parameter.
;   on output par contains the final estimate.
;
; x is an output array of length n which contains the least
;   squares solution of the system a*x = b, sqrt(par)*d*x = 0,
;   for the output par.
;
; sdiag is an output array of length n which contains the
;   diagonal elements of the upper triangular matrix s.
;
; wa1 and wa2 are work arrays of length n.
;
;     subprograms called
;
; minpack-supplied ... dpmpar,enorm,qrsolv
;
; fortran-supplied ... dabs,dmax1,dmin1,dsqrt
;
;     argonne national laboratory. minpack project. march 1980.
;     burton s. garbow, kenneth e. hillstrom, jorge j. more
;
function mpfit_lmpar, r, ipvt, diag, qtb, delta, x, sdiag, par=par

  COMPILE_OPT strictarr
  common mpfit_machar, machvals
  common mpfit_profile, profvals
;  prof_start = systime(1)

  MACHEP0 = machvals.machep
  DWARF   = machvals.minnum

  sz = size(r)
  m = sz[1]
  n = sz[2]
  delm = lindgen(n) * (m+1) ;; Diagonal elements of r

  ;; Compute and store in x the gauss-newton direction.  If the
  ;; jacobian is rank-deficient, obtain a least-squares solution
  nsing = n
  wa1 = qtb
  rthresh = max(abs(r[delm]))*MACHEP0
  wh = where(abs(r[delm]) LT rthresh, ct)
  if ct GT 0 then begin
      nsing = wh[0]
      wa1[wh[0]:*] = 0
  endif

  if nsing GE 1 then begin
      ;; *** Reverse loop ***
      for j=nsing-1,0,-1 do begin  
          wa1[j] = wa1[j]/r[j,j]
          if (j-1 GE 0) then $
            wa1[0:(j-1)] = wa1[0:(j-1)] - r[0:(j-1),j]*wa1[j]
      endfor
  endif

  ;; Note: ipvt here is a permutation array
  x[ipvt] = wa1

  ;; Initialize the iteration counter.  Evaluate the function at the
  ;; origin, and test for acceptance of the gauss-newton direction
  iter = 0L
  wa2 = diag * x
  dxnorm = mpfit_enorm(wa2)
  fp = dxnorm - delta
  if fp LE 0.1*delta then goto, TERMINATE

  ;; If the jacobian is not rank deficient, the newton step provides a
  ;; lower bound, parl, for the zero of the function.  Otherwise set
  ;; this bound to zero.
  
  zero = wa2[0]*0.
  parl = zero
  if nsing GE n then begin
      wa1 = diag[ipvt]*wa2[ipvt]/dxnorm

      wa1[0] = wa1[0] / r[0,0] ;; Degenerate case 
      for j=1L, n-1 do begin   ;; Note "1" here, not zero
          sum = total(r[0:(j-1),j]*wa1[0:(j-1)])
          wa1[j] = (wa1[j] - sum)/r[j,j]
      endfor

      temp = mpfit_enorm(wa1)
      parl = ((fp/delta)/temp)/temp
  endif

  ;; Calculate an upper bound, paru, for the zero of the function
  for j=0L, n-1 do begin
      sum = total(r[0:j,j]*qtb[0:j])
      wa1[j] = sum/diag[ipvt[j]]
  endfor
  gnorm = mpfit_enorm(wa1)
  paru  = gnorm/delta
  if paru EQ 0 then paru = DWARF/min([delta,0.1])

  ;; If the input par lies outside of the interval (parl,paru), set
  ;; par to the closer endpoint

  par = max([par,parl])
  par = min([par,paru])
  if par EQ 0 then par = gnorm/dxnorm

  ;; Beginning of an interation
  ITERATION:
  iter = iter + 1
  
  ;; Evaluate the function at the current value of par
  if par EQ 0 then par = max([DWARF, paru*0.001])
  temp = sqrt(par)
  wa1 = temp * diag
  mpfit_qrsolv, r, ipvt, wa1, qtb, x, sdiag
  wa2 = diag*x
  dxnorm = mpfit_enorm(wa2)
  temp = fp
  fp = dxnorm - delta

  if (abs(fp) LE 0.1D*delta) $
    OR ((parl EQ 0) AND (fp LE temp) AND (temp LT 0)) $
    OR (iter EQ 10) then goto, TERMINATE

  ;; Compute the newton correction
  wa1 = diag[ipvt]*wa2[ipvt]/dxnorm

  for j=0L,n-2 do begin
      wa1[j] = wa1[j]/sdiag[j]
      wa1[j+1:n-1] = wa1[j+1:n-1] - r[j+1:n-1,j]*wa1[j]
  endfor
  wa1[n-1] = wa1[n-1]/sdiag[n-1] ;; Degenerate case

  temp = mpfit_enorm(wa1)
  parc = ((fp/delta)/temp)/temp

  ;; Depending on the sign of the function, update parl or paru
  if fp GT 0 then parl = max([parl,par])
  if fp LT 0 then paru = min([paru,par])

  ;; Compute an improved estimate for par
  par = max([parl, par+parc])

  ;; End of an iteration
  goto, ITERATION
  
TERMINATE:
  ;; Termination
;  profvals.lmpar = profvals.lmpar + (systime(1) - prof_start)
  if iter EQ 0 then return, par[0]*0.
  return, par
end

;; Procedure to tie one parameter to another.
pro mpfit_tie, p, _ptied
  COMPILE_OPT strictarr
  if n_elements(_ptied) EQ 0 then return
  if n_elements(_ptied) EQ 1 then if _ptied[0] EQ '' then return
  for _i = 0L, n_elements(_ptied)-1 do begin
      if _ptied[_i] EQ '' then goto, NEXT_TIE
      _cmd = 'p['+strtrim(_i,2)+'] = '+_ptied[_i]
      _err = execute(_cmd)
      if _err EQ 0 then begin
          message, 'ERROR: Tied expression "'+_cmd+'" failed.'
          return
      endif
      NEXT_TIE:
  endfor
end

;; Default print procedure
pro mpfit_defprint, p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, $
                    p11, p12, p13, p14, p15, p16, p17, p18, $
                    format=format, unit=unit0, _EXTRA=extra

  COMPILE_OPT strictarr
  if n_elements(unit0) EQ 0 then unit = -1 else unit = round(unit0[0])
  if n_params() EQ 0 then printf, unit, '' $
  else if n_params() EQ 1 then printf, unit, p1, format=format $
  else if n_params() EQ 2 then printf, unit, p1, p2, format=format $
  else if n_params() EQ 3 then printf, unit, p1, p2, p3, format=format $
  else if n_params() EQ 4 then printf, unit, p1, p2, p4, format=format 

  return
end


;; Default procedure to be called every iteration.  It simply prints
;; the parameter values.
pro mpfit_defiter, fcn, x, iter, fnorm, FUNCTARGS=fcnargs, $
                   quiet=quiet, iterstop=iterstop, iterkeybyte=iterkeybyte, $
                   parinfo=parinfo, iterprint=iterprint0, $
                   format=fmt, pformat=pformat, dof=dof0, _EXTRA=iterargs

  COMPILE_OPT strictarr
  common mpfit_error, mperr
  mperr = 0

  if keyword_set(quiet) then goto, DO_ITERSTOP
  if n_params() EQ 3 then begin
      fvec = mpfit_call(fcn, x, _EXTRA=fcnargs)
      fnorm = mpfit_enorm(fvec)^2
  endif

  ;; Determine which parameters to print
  nprint = n_elements(x)
  iprint = lindgen(nprint)

  if n_elements(iterprint0) EQ 0 then iterprint = 'MPFIT_DEFPRINT' $
  else iterprint = strtrim(iterprint0[0],2)

  if n_elements(dof0) EQ 0 then dof = 1L else dof = floor(dof0[0])
  call_procedure, iterprint, iter, fnorm, dof, $
    format='("Iter ",I6,"   CHI-SQUARE = ",G15.8,"          DOF = ",I0)', $
    _EXTRA=iterargs
  if n_elements(fmt) GT 0 then begin
      call_procedure, iterprint, x, format=fmt, _EXTRA=iterargs
  endif else begin
      if n_elements(pformat) EQ 0 then pformat = '(G20.6)'
      parname = 'P('+strtrim(iprint,2)+')'
      pformats = strarr(nprint) + pformat

      if n_elements(parinfo) GT 0 then begin
          parinfo_tags = tag_names(parinfo)
          wh = where(parinfo_tags EQ 'PARNAME', ct)
          if ct EQ 1 then begin
              wh = where(parinfo.parname NE '', ct)
              if ct GT 0 then $
                parname[wh] = strmid(parinfo[wh].parname,0,25)
          endif
          wh = where(parinfo_tags EQ 'MPPRINT', ct)
          if ct EQ 1 then begin
              iprint = where(parinfo.mpprint EQ 1, nprint)
              if nprint EQ 0 then goto, DO_ITERSTOP
          endif
          wh = where(parinfo_tags EQ 'MPFORMAT', ct)
          if ct EQ 1 then begin
              wh = where(parinfo.mpformat NE '', ct)
              if ct GT 0 then pformats[wh] = parinfo[wh].mpformat
          endif
      endif

      for i = 0L, nprint-1 do begin
          call_procedure, iterprint, parname[iprint[i]], x[iprint[i]], $
            format='("    ",A0," = ",'+pformats[iprint[i]]+')', $
            _EXTRA=iterargs
      endfor
  endelse

  DO_ITERSTOP:
  if n_elements(iterkeybyte) EQ 0 then iterkeybyte = 7b
  if keyword_set(iterstop) then begin
      k = get_kbrd(0)
      if k EQ string(iterkeybyte[0]) then begin
          message, 'WARNING: minimization not complete', /info
          print, 'Do you want to terminate this procedure? (y/n)', $
            format='(A,$)'
          k = ''
          read, k
          if strupcase(strmid(k,0,1)) EQ 'Y' then begin
              message, 'WARNING: Procedure is terminating.', /info
              mperr = -1
          endif
      endif
  endif

  return
end

;; Procedure to parse the parameter values in PARINFO
pro mpfit_parinfo, parinfo, tnames, tag, values, default=def, status=status, $
                   n_param=n

  COMPILE_OPT strictarr
  status = 0
  if n_elements(n) EQ 0 then n = n_elements(parinfo)

  if n EQ 0 then begin
      if n_elements(def) EQ 0 then return
      values = def
      status = 1
      return
  endif

  if n_elements(parinfo) EQ 0 then goto, DO_DEFAULT
  if n_elements(tnames) EQ 0 then tnames = tag_names(parinfo)
  wh = where(tnames EQ tag, ct)

  if ct EQ 0 then begin
      DO_DEFAULT:
      if n_elements(def) EQ 0 then return
      values = make_array(n, value=def[0])
      values[0] = def
  endif else begin
      values = parinfo.(wh[0])
      np = n_elements(parinfo)
      nv = n_elements(values)
      values = reform(values[*], nv/np, np)
  endelse

  status = 1
  return
end


;     **********
;
;     subroutine covar
;
;     given an m by n matrix a, the problem is to determine
;     the covariance matrix corresponding to a, defined as
;
;                    t
;           inverse(a *a) .
;
;     this subroutine completes the solution of the problem
;     if it is provided with the necessary information from the
;     qr factorization, with column pivoting, of a. that is, if
;     a*p = q*r, where p is a permutation matrix, q has orthogonal
;     columns, and r is an upper triangular matrix with diagonal
;     elements of nonincreasing magnitude, then covar expects
;     the full upper triangle of r and the permutation matrix p.
;     the covariance matrix is then computed as
;
;                      t     t
;           p*inverse(r *r)*p  .
;
;     if a is nearly rank deficient, it may be desirable to compute
;     the covariance matrix corresponding to the linearly independent
;     columns of a. to define the numerical rank of a, covar uses
;     the tolerance tol. if l is the largest integer such that
;
;           abs(r(l,l)) .gt. tol*abs(r(1,1)) ,
;
;     then covar computes the covariance matrix corresponding to
;     the first l columns of r. for k greater than l, column
;     and row ipvt(k) of the covariance matrix are set to zero.
;
;     the subroutine statement is
;
;       subroutine covar(n,r,ldr,ipvt,tol,wa)
;
;     where
;
;       n is a positive integer input variable set to the order of r.
;
;       r is an n by n array. on input the full upper triangle must
;         contain the full upper triangle of the matrix r. on output
;         r contains the square symmetric covariance matrix.
;
;       ldr is a positive integer input variable not less than n
;         which specifies the leading dimension of the array r.
;
;       ipvt is an integer input array of length n which defines the
;         permutation matrix p such that a*p = q*r. column j of p
;         is column ipvt(j) of the identity matrix.
;
;       tol is a nonnegative input variable used to define the
;         numerical rank of a in the manner described above.
;
;       wa is a work array of length n.
;
;     subprograms called
;
;       fortran-supplied ... dabs
;
;     argonne national laboratory. minpack project. august 1980.
;     burton s. garbow, kenneth e. hillstrom, jorge j. more
;
;     **********
function mpfit_covar, rr, ipvt, tol=tol

  COMPILE_OPT strictarr
  sz = size(rr)
  if sz[0] NE 2 then begin
      message, 'ERROR: r must be a two-dimensional matrix'
      return, -1L
  endif
  n = sz[1]
  if n NE sz[2] then begin
      message, 'ERROR: r must be a square matrix'
      return, -1L
  endif

  zero = rr[0] * 0.
  one  = zero  + 1.
  if n_elements(ipvt) EQ 0 then ipvt = lindgen(n)
  r = rr
  r = reform(rr, n, n, /overwrite)
  
  ;; Form the inverse of r in the full upper triangle of r
  l = -1L
  if n_elements(tol) EQ 0 then tol = one*1.E-14
  tolr = tol * abs(r[0,0])
  for k = 0L, n-1 do begin
      if abs(r[k,k]) LE tolr then goto, INV_END_LOOP
      r[k,k] = one/r[k,k]
      for j = 0L, k-1 do begin
          temp = r[k,k] * r[j,k]
          r[j,k] = zero
          r[0,k] = r[0:j,k] - temp*r[0:j,j]
      endfor
      l = k
  endfor
  INV_END_LOOP:

  ;; Form the full upper triangle of the inverse of (r transpose)*r
  ;; in the full upper triangle of r
  if l GE 0 then $
    for k = 0L, l do begin
      for j = 0L, k-1 do begin
          temp = r[j,k]
          r[0,j] = r[0:j,j] + temp*r[0:j,k]
      endfor
      temp = r[k,k]
      r[0,k] = temp * r[0:k,k]
  endfor

  ;; Form the full lower triangle of the covariance matrix
  ;; in the strict lower triangle of r and in wa
  wa = replicate(r[0,0], n)
  for j = 0L, n-1 do begin
      jj = ipvt[j]
      sing = j GT l
      for i = 0L, j do begin
          if sing then r[i,j] = zero
          ii = ipvt[i]
          if ii GT jj then r[ii,jj] = r[i,j]
          if ii LT jj then r[jj,ii] = r[i,j]
      endfor
      wa[jj] = r[j,j]
  endfor

  ;; Symmetrize the covariance matrix in r
  for j = 0L, n-1 do begin
      r[0:j,j] = r[j,0:j]
      r[j,j] = wa[j]
  endfor

  return, r
end

;; Parse the RCSID revision number
function mpfit_revision
  ;; NOTE: this string is changed every time an RCS check-in occurs
  revision = '$Revision: 1.82 $'

  ;; Parse just the version number portion
  revision = stregex(revision,'\$'+'Revision: *([0-9.]+) *'+'\$',/extract,/sub)
  revision = revision[1]
  return, revision
end

;; Parse version numbers of the form 'X.Y'
function mpfit_parse_version, version
  sz = size(version)
  if sz[sz[0]+1] NE 7 then return, 0

  s = stregex(version[0], '^([0-9]+)\.([0-9]+)$', /extract,/sub) 
  if s[0] NE version[0] then return, 0
  return, long(s[1:2])
end

;; Enforce a minimum version number
function mpfit_min_version, version, min_version
  mv = mpfit_parse_version(min_version)
  if mv[0] EQ 0 then return, 0
  v  = mpfit_parse_version(version)

  ;; Compare version components
  if v[0] LT mv[0] then return, 0
  if v[1] LT mv[1] then return, 0
  return, 1
end

; Manually reset recursion fencepost if the user gets in trouble
pro mpfit_reset_recursion
  common mpfit_fencepost, mpfit_fencepost_active
  mpfit_fencepost_active = 0
end

;     **********
;
;     subroutine lmdif
;
;     the purpose of lmdif is to minimize the sum of the squares of
;     m nonlinear functions in n variables by a modification of
;     the levenberg-marquardt algorithm. the user must provide a
;     subroutine which calculates the functions. the jacobian is
;     then calculated by a forward-difference approximation.
;
;     the subroutine statement is
;
; subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
;      diag,mode,factor,nprint,info,nfev,fjac,
;      ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
;
;     where
;
; fcn is the name of the user-supplied subroutine which
;   calculates the functions. fcn must be declared
;   in an external statement in the user calling
;   program, and should be written as follows.
;
;   subroutine fcn(m,n,x,fvec,iflag)
;   integer m,n,iflag
;   double precision x(n),fvec(m)
;   ----------
;   calculate the functions at x and
;   return this vector in fvec.
;   ----------
;   return
;   end
;
;   the value of iflag should not be changed by fcn unless
;   the user wants to terminate execution of lmdif.
;   in this case set iflag to a negative integer.
;
; m is a positive integer input variable set to the number
;   of functions.
;
; n is a positive integer input variable set to the number
;   of variables. n must not exceed m.
;
; x is an array of length n. on input x must contain
;   an initial estimate of the solution vector. on output x
;   contains the final estimate of the solution vector.
;
; fvec is an output array of length m which contains
;   the functions evaluated at the output x.
;
; ftol is a nonnegative input variable. termination
;   occurs when both the actual and predicted relative
;   reductions in the sum of squares are at most ftol.
;   therefore, ftol measures the relative error desired
;   in the sum of squares.
;
; xtol is a nonnegative input variable. termination
;   occurs when the relative error between two consecutive
;   iterates is at most xtol. therefore, xtol measures the
;   relative error desired in the approximate solution.
;
; gtol is a nonnegative input variable. termination
;   occurs when the cosine of the angle between fvec and
;   any column of the jacobian is at most gtol in absolute
;   value. therefore, gtol measures the orthogonality
;   desired between the function vector and the columns
;   of the jacobian.
;
; maxfev is a positive integer input variable. termination
;   occurs when the number of calls to fcn is at least
;   maxfev by the end of an iteration.
;
; epsfcn is an input variable used in determining a suitable
;   step length for the forward-difference approximation. this
;   approximation assumes that the relative errors in the
;   functions are of the order of epsfcn. if epsfcn is less
;   than the machine precision, it is assumed that the relative
;   errors in the functions are of the order of the machine
;   precision.
;
; diag is an array of length n. if mode = 1 (see
;   below), diag is internally set. if mode = 2, diag
;   must contain positive entries that serve as
;   multiplicative scale factors for the variables.
;
; mode is an integer input variable. if mode = 1, the
;   variables will be scaled internally. if mode = 2,
;   the scaling is specified by the input diag. other
;   values of mode are equivalent to mode = 1.
;
; factor is a positive input variable used in determining the
;   initial step bound. this bound is set to the product of
;   factor and the euclidean norm of diag*x if nonzero, or else
;   to factor itself. in most cases factor should lie in the
;   interval (.1,100.). 100. is a generally recommended value.
;
; nprint is an integer input variable that enables controlled
;   printing of iterates if it is positive. in this case,
;   fcn is called with iflag = 0 at the beginning of the first
;   iteration and every nprint iterations thereafter and
;   immediately prior to return, with x and fvec available
;   for printing. if nprint is not positive, no special calls
;   of fcn with iflag = 0 are made.
;
; info is an integer output variable. if the user has
;   terminated execution, info is set to the (negative)
;   value of iflag. see description of fcn. otherwise,
;   info is set as follows.
;
;   info = 0  improper input parameters.
;
;   info = 1  both actual and predicted relative reductions
;       in the sum of squares are at most ftol.
;
;   info = 2  relative error between two consecutive iterates
;       is at most xtol.
;
;   info = 3  conditions for info = 1 and info = 2 both hold.
;
;   info = 4  the cosine of the angle between fvec and any
;       column of the jacobian is at most gtol in
;       absolute value.
;
;   info = 5  number of calls to fcn has reached or
;       exceeded maxfev.
;
;   info = 6  ftol is too small. no further reduction in
;       the sum of squares is possible.
;
;   info = 7  xtol is too small. no further improvement in
;       the approximate solution x is possible.
;
;   info = 8  gtol is too small. fvec is orthogonal to the
;       columns of the jacobian to machine precision.
;
; nfev is an integer output variable set to the number of
;   calls to fcn.
;
; fjac is an output m by n array. the upper n by n submatrix
;   of fjac contains an upper triangular matrix r with
;   diagonal elements of nonincreasing magnitude such that
;
;    t     t     t
;   p *(jac *jac)*p = r *r,
;
;   where p is a permutation matrix and jac is the final
;   calculated jacobian. column j of p is column ipvt(j)
;   (see below) of the identity matrix. the lower trapezoidal
;   part of fjac contains information generated during
;   the computation of r.
;
; ldfjac is a positive integer input variable not less than m
;   which specifies the leading dimension of the array fjac.
;
; ipvt is an integer output array of length n. ipvt
;   defines a permutation matrix p such that jac*p = q*r,
;   where jac is the final calculated jacobian, q is
;   orthogonal (not stored), and r is upper triangular
;   with diagonal elements of nonincreasing magnitude.
;   column j of p is column ipvt(j) of the identity matrix.
;
; qtf is an output array of length n which contains
;   the first n elements of the vector (q transpose)*fvec.
;
; wa1, wa2, and wa3 are work arrays of length n.
;
; wa4 is a work array of length m.
;
;     subprograms called
;
; user-supplied ...... fcn
;
; minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
;
; fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
;
;     argonne national laboratory. minpack project. march 1980.
;     burton s. garbow, kenneth e. hillstrom, jorge j. more
;
;     **********
function mpfit, fcn, xall, FUNCTARGS=fcnargs, SCALE_FCN=scalfcn, $
                ftol=ftol0, xtol=xtol0, gtol=gtol0, epsfcn=epsfcn, $
                resdamp=damp0, $
                nfev=nfev, maxiter=maxiter, errmsg=errmsg, $
                factor=factor0, nprint=nprint0, STATUS=info, $
                iterproc=iterproc0, iterargs=iterargs, iterstop=ss,$
                iterkeystop=iterkeystop, $
                niter=iter, nfree=nfree, npegged=npegged, dof=dof, $
                diag=diag, rescale=rescale, autoderivative=autoderiv0, $
                pfree_index=ifree, $
                perror=perror, covar=covar, nocovar=nocovar, $
                bestnorm=fnorm, best_resid=fvec, $
                best_fjac=output_fjac, calc_fjac=calc_fjac, $
                parinfo=parinfo, quiet=quiet, nocatch=nocatch, $
                fastnorm=fastnorm0, proc=proc, query=query, $
                external_state=state, external_init=extinit, $
                external_fvec=efvec, external_fjac=efjac, $
                version=version, min_version=min_version0

  COMPILE_OPT strictarr
  info = 0L
  errmsg = ''

  ;; Compute the revision number, to be returned in the VERSION and
  ;; QUERY keywords.
  common mpfit_revision_common, mpfit_revision_str
  if n_elements(mpfit_revision_str) EQ 0 then $
     mpfit_revision_str = mpfit_revision()
  version = mpfit_revision_str

  if keyword_set(query) then begin
     if n_elements(min_version0) GT 0 then $
        if mpfit_min_version(version, min_version0[0]) EQ 0 then $
           return, 0
     return, 1
  endif

  if n_elements(min_version0) GT 0 then $
     if mpfit_min_version(version, min_version0[0]) EQ 0 then begin
     message, 'ERROR: minimum required version '+min_version0[0]+' not satisfied', /info
     return, !values.d_nan
  endif

  if n_params() EQ 0 then begin
      message, "USAGE: PARMS = MPFIT('MYFUNCT', START_PARAMS, ... )", /info
      return, !values.d_nan
  endif
  
  ;; Use of double here not a problem since f/x/gtol are all only used
  ;; in comparisons
  if n_elements(ftol0) EQ 0 then ftol = 1.D-10 else ftol = ftol0[0]
  if n_elements(xtol0) EQ 0 then xtol = 1.D-10 else xtol = xtol0[0]
  if n_elements(gtol0) EQ 0 then gtol = 1.D-10 else gtol = gtol0[0]
  if n_elements(factor0) EQ 0 then factor = 100. else factor = factor0[0]
  if n_elements(nprint0) EQ 0 then nprint = 1 else nprint = nprint0[0]
  if n_elements(iterproc0) EQ 0 then iterproc = 'MPFIT_DEFITER' else iterproc = iterproc0[0]
  if n_elements(autoderiv0) EQ 0 then autoderiv = 1 else autoderiv = autoderiv0[0]
  if n_elements(fastnorm0) EQ 0 then fastnorm = 0 else fastnorm = fastnorm0[0]
  if n_elements(damp0) EQ 0 then damp = 0 else damp = damp0[0]

  ;; These are special configuration parameters that can't be easily
  ;; passed by MPFIT directly.
  ;;  FASTNORM - decide on which sum-of-squares technique to use (1)
  ;;             is fast, (0) is slower
  ;;  PROC - user routine is a procedure (1) or function (0)
  ;;  QANYTIED - set to 1 if any parameters are TIED, zero if none
  ;;  PTIED - array of strings, one for each parameter
  common mpfit_config, mpconfig
  mpconfig = {fastnorm: keyword_set(fastnorm), proc: 0, nfev: 0L, damp: damp}
  common mpfit_machar, machvals

  iflag = 0L
  catch_msg = 'in MPFIT'
  nfree = 0L
  npegged = 0L
  dof = 0L
  output_fjac = 0L

  ;; Set up a persistent fencepost that prevents recursive calls
  common mpfit_fencepost, mpfit_fencepost_active
  if n_elements(mpfit_fencepost_active) EQ 0 then mpfit_fencepost_active = 0
  if mpfit_fencepost_active then begin
      errmsg = 'ERROR: recursion detected; you cannot run MPFIT recursively'
      goto, TERMINATE
  endif
  ;; Only activate the fencepost if we are not in debugging mode
  if NOT keyword_set(nocatch) then mpfit_fencepost_active = 1


  ;; Parameter damping doesn't work when user is providing their own
  ;; gradients.
  if damp NE 0 AND NOT keyword_set(autoderiv) then begin
      errmsg = 'ERROR: keywords DAMP and AUTODERIV are mutually exclusive'
      goto, TERMINATE
  endif      
  
  ;; Process the ITERSTOP and ITERKEYSTOP keywords, and turn this into
  ;; a set of keywords to pass to MPFIT_DEFITER.
  if strupcase(iterproc) EQ 'MPFIT_DEFITER' AND n_elements(iterargs) EQ 0 $
    AND keyword_set(ss) then begin
      if n_elements(iterkeystop) GT 0 then begin
          sz = size(iterkeystop)
          tp = sz[sz[0]+1]
          if tp EQ 7 then begin
              ;; String - convert first char to byte
              iterkeybyte = (byte(iterkeystop[0]))[0]
          endif
          if (tp GE 1 AND tp LE 3) OR (tp GE 12 AND tp LE 15) then begin
              ;; Integer - convert to byte
              iterkeybyte = byte(iterkeystop[0])
          endif
          if n_elements(iterkeybyte) EQ 0 then begin
              errmsg = 'ERROR: ITERKEYSTOP must be either a BYTE or STRING'
              goto, TERMINATE
          endif

          iterargs = {iterstop: 1, iterkeybyte: iterkeybyte}
      endif else begin
          iterargs = {iterstop: 1, iterkeybyte: 7b}
      endelse
  endif


  ;; Handle error conditions gracefully
  if NOT keyword_set(nocatch) then begin
      catch, catcherror
      if catcherror NE 0 then begin  ;; An error occurred!!!
          catch, /cancel
          mpfit_fencepost_active = 0
          err_string = ''+!error_state.msg
          message, /cont, 'Error detected while '+catch_msg+':'
          message, /cont,    err_string
          message, /cont, 'Error condition detected. Returning to MAIN level.'
          if errmsg EQ '' then $
            errmsg = 'Error detected while '+catch_msg+': '+err_string
          if info EQ 0 then info = -18
          return, !values.d_nan
      endif
  endif
  mpconfig = create_struct(mpconfig, 'NOCATCH', keyword_set(nocatch))

  ;; Parse FCN function name - be sure it is a scalar string
  sz = size(fcn)
  if sz[0] NE 0 then begin
      FCN_NAME:
      errmsg = 'ERROR: MYFUNCT must be a scalar string'
      goto, TERMINATE
  endif
  if sz[sz[0]+1] NE 7 then goto, FCN_NAME

  isext = 0
  if fcn EQ '(EXTERNAL)' then begin
      if n_elements(efvec) EQ 0 OR n_elements(efjac) EQ 0 then begin
          errmsg = 'ERROR: when using EXTERNAL function, EXTERNAL_FVEC '+$
            'and EXTERNAL_FJAC must be defined'
          goto, TERMINATE
      endif
      nv = n_elements(efvec)
      nj = n_elements(efjac)
      if (nj MOD nv) NE 0 then begin
          errmsg = 'ERROR: the number of values in EXTERNAL_FJAC must be '+ $
            'a multiple of the number of values in EXTERNAL_FVEC'
          goto, TERMINATE
      endif
      isext = 1
  endif

  ;; Parinfo:
  ;; --------------- STARTING/CONFIG INFO (passed in to routine, not changed)
  ;; .value   - starting value for parameter
  ;; .fixed   - parameter is fixed
  ;; .limited - a two-element array, if parameter is bounded on
  ;;            lower/upper side
  ;; .limits - a two-element array, lower/upper parameter bounds, if
  ;;           limited vale is set
  ;; .step   - step size in Jacobian calc, if greater than zero

  catch_msg = 'parsing input parameters'
  ;; Parameters can either be stored in parinfo, or x.  Parinfo takes
  ;; precedence if it exists.
  if n_elements(xall) EQ 0 AND n_elements(parinfo) EQ 0 then begin
      errmsg = 'ERROR: must pass parameters in P or PARINFO'
      goto, TERMINATE
  endif

  ;; Be sure that PARINFO is of the right type
  if n_elements(parinfo) GT 0 then begin
      ;; Make sure the array is 1-D
      parinfo = parinfo[*]
      parinfo_size = size(parinfo)
      if parinfo_size[parinfo_size[0]+1] NE 8 then begin
          errmsg = 'ERROR: PARINFO must be a structure.'
          goto, TERMINATE