/* See {pz_opt_matrix.h}. */
/* Last edited on 2008-02-08 18:14:06 by stolfi */

#include <Text.h>
#include <math.h>
#include <r4x4.h>
#include <r3.h>
#include <Minimizer.h>
#include <CoordMinimizer.h>
#include <BrentUniMin.h>
#include <pz_geo.h>

#include <Minimizer.h>
#include <pz_types.h>
#include <stdio.h>

#include <pz_opt_matrix.h>


void pz_opt_matrix_find
  ( pz_matrix_func_t func,  /* Function to optimize. */
    double scaleX,          /* Scale X length for translation. */
    double scaleY,          /* Scale Y length for translation. */
    unsigned maxEvals,      /* Maximum number of "func" calls. */ 
    unsigned *nEvals,       /* OUT: Total number of "func" calls performed. */
    r4x4_t *m,              /* IN: initial guess, OUT: optimized matrix. */
    double *f,              /* IN/OUT: "func(m)". */
    char *plotName,         /* Plot file for "func" (minus extension); or empty. */
  )
  {

    { /* with */ 
      REF Gradient *g [3] = (REF Gradient *)notnull(malloc(3*sizeof(REF Gradient)), "out of mem")^; /* Not used by CoordMinimizer */
      REF Point *x [3] = (REF Point *)notnull(malloc(3*sizeof(REF Point)), "out of mem")^;
      REF Point *xIni [3] = (REF Point *)notnull(malloc(3*sizeof(REF Point)), "out of mem")^;
      /* do */

      auto void ConvertXtoM(double x[], r4x4_t *m);
      auto void ConvertMtoX(r4x4_t *m, double x[])l
      auto double Evaluate(int m, double x[]);
      auto int Checkint m, double x[], double Fx);
      
    void ConvertXtoM(double x[], r4x4_t *m)
        { m = pz_geo.Translation(r3_t{scaleX*x[0], scaleY*x[1], 0.0e0});
          m.c[1][1] = cos(x[2]);
          m.c[1][2] = sin(x[2]);
          m.c[2][1] = -m.c[1][2];
          m.c[2][2] = m.c[1][1];
        }

    void ConvertMtoX(r4x4_t *m, double x[])
        { 
          affirm(scaleX != 0.0e0  );
          affirm(scaleY != 0.0e0  );
          x[0] = m.c[0][1]/scaleX;
          x[1] = m.c[0][2]/scaleY;
          x[2] = atan2(m.c[1][2], m.c[1][1]);;
        }

    int Check(int m, double x[], double Fx)
      {
        return (nEvals >= maxEvals);
      }

    double Evaluate(int m, double x[])
      { r4x4_t  m;
        nEvals++;
        fprintf(stderr, "  evals == %6d", nEvals);
        fprintf(stderr, "  trn == (%8.5f, %8.5f)", x[0],x[1]);
        fprintf(stderr, "  rot == %8.5f", x[2]);
        ConvertXtoM(x, &m);
        f = func(m);
        fprintf(stderr, "  val == %12.4f\n", f);
        return f;
      }
  

   void PlotEvalSection(double u[], double v[])
     {

        double f;
        int NSteps = 50;
        unsigned saveMaxEvals = maxEvals;
          { FILE *wrFile = open_write(plotName & ".plt", TRUE);
            REF Gradient *g [3] = (REF Gradient *)notnull(malloc(3*sizeof(REF Gradient)), "out of mem")^;
            double y[3];
            /* do */ 
            maxEvals = MAXINT;
            for (i = 0; i <= 2*NSteps ; i++)
              { double s = ((double)i)/((double)NSteps);
                double t = 1 - s;
                y[0] = t*u[0] + s*v[0];
                y[1] = t*u[1] + s*v[1];
                y[2] = t*u[2] + s*v[2];
                double f = Evaluate(3, y);
                fprintf(wrFile, Fmt.LongReal(t)); fprintf(wrFile, "  ");
                fprintf(wrFile, Fmt.LongReal(y[0])); fprintf(wrFile, "  ");
                fprintf(wrFile, Fmt.LongReal(y[1])); fprintf(wrFile, "  ");
                fprintf(wrFile, Fmt.LongReal(y[2])); fprintf(wrFile, "  ");
                fprintf(wrFile, Fmt.LongReal(f)); fprintf(wrFile, "  ");
                fprintf(wrFile, "\n");;
              };;
              };
            fclose(wrFile);
            maxEvals = saveMaxEvals;;
          };  
       } /* PlotEvalSection */
     
      { 
        ConvertMtoX(m,x);
        xIni = x;
        if (( maxEvals > 0 )){
          /* Prepare for minimization: */
          EVAL Evaluate(x, f, g);
          cMin.debug = FALSE; 

          /* Find "x" with minimum "f": */
          cMin.minimize(
            eval = Evaluate,      /* Evaluates the goal function. */
            x = x,                /* In: initial guess, out: minimum point. */ 
            fx = f,               /* In/out: function value at "x" */
            dfx = g,              /* In/out: gradient of "f" at "x" */
            dist = 1.0e-1,        /* Guessed distance from optimum */
            tol = 1.0e-1/scaleY,  /* Guessed radius of optimum region */
            check = NULL           /* Acceptance test */    
          );

          /* Return "x" to matrix form: */
          ConvertXtoM(x,m);;
        };
        
        f = func(m);
        nEvals++;

        /* Plot profile of goal function from initial guess to optimum: */
        if (( NOT Text.Empty(plotName) )){
          PlotEvalSection(xIni, x);
        };;
      };;
    };
  } /* pz_opt_matrix_find */


char *FLR(
    double x, 
    unsigned w,
    unsigned p 
  )
  {
    return Fmt.Pad(Fmt.LongReal(x, Fmt.Style.Fix, prec = p), w)
  } /* FLR */


char *FI(
    int x, 
    unsigned w 
  )
  {
    return Fmt.Pad(Fmt.Int(x), w)
  } /* FI */

{
/*
  Copyright © 2001 Universidade Estadual de Campinas (UNICAMP).
  Authors: Helena C. G. Leitão and Jorge Stolfi.
  
  This file can be freely distributed, used, and modified, provided
  that this copyright and authorship notice is preserved, and that any
  modified versions are clearly marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers chall be held
  responsible for any losses or damages that may result from its use.
*/