/* Last edited on 2010-06-24 12:52:27 by stolfilocal */

#include <LS_system.h>
#include <assert.h>
#include <gauss_elim.h>
#include <rmxn.h>

pointlike_lighting_t* pld;
harmonic_lighting_t* hld;
radialbasis_lighting_t* rld;

void debug_ldata(void* l_data)
{
  pld = l_data;
  hld = l_data;
  rld = l_data;
}

void computeLeastSquaresMatrix
  ( double* A,
    phi_function* phi, 
    int basis_size, 
    r3_t* X,
    int n, 
    double w[], 
    double wpos[],
    void* l_data
  )
  {
    int r,s;
    int count = 0;
    int total = basis_size;
    for( r = 0; r < basis_size; r++)
      { for( s = 0; s < basis_size; s++)
          { int iA = (r*basis_size) + s;
            double valueA = 0;
            int i;
            for(i = 0; i < n; i++)
              { r3_t normal = X[i];
                double wi = w[i]*wpos[i];
                double phiR = phi(r,&normal,l_data);
                double phiS = phi(s,&normal,l_data);
                valueA+= (phiR*phiS)*wi;
              }
            A[iA] = valueA;
          }
        count++;
      }
  }

void computeLeastSquaresRHSVector
  ( double* b,
    phi_function* phi, 
    int basis_size,
    r3_t* X,
    double* F,
    int n, 
    double w[], 
    double wpos[],
    void* l_data
  )
  {
    int r;
    int count = 0;
    int total = basis_size;
    for( r = 0; r < basis_size; r++)
      { double valueB = 0;
        int i;
        for(i = 0; i < n; i++)
          { r3_t normal = X[i];
            double wi = w[i]*wpos[i];
            double Di = F[i];
            double phiR = phi(r,&normal,l_data);
            valueB+=(phiR*Di)*wi;
          }
        b[r] = valueB;
        count++;
      }
  }

void FitOneLight
  ( double* A,
    double* b ,
    double* c,
    phi_function* phi,
    int basis_size,
    validate_function* validateResults,
    r3_t* X,
    double* F,
    int n,
    double* w,
    double* wpos,
    void* l_data
  )
  { int valids = 0;
    int iterations = 0;
    double *Ared = (double *)malloc(basis_size*basis_size*sizeof(double)); // Matrix for reduced basis
    bool_t *valid_basis = (bool_t *)malloc(basis_size*sizeof(bool_t)); // Marks elements of reduced basis
    int *oldix = (int *)malloc(basis_size*sizeof(int)); // Maps reduced indices to full indices

    // The generic element of the reduced basis:
    auto double phiRed(int i, r3_t *normal, void* l_data);
    
    double phiRed(int i, r3_t *normal,void* l_data)
      { return phi(oldix[i], normal,l_data); }

    int i,j;
    // Initially all basis elements are valid:
    for(j = 0; j < basis_size; j++) { valid_basis[j] = TRUE; oldix[j] = j; }
    valids = basis_size;

    int last_valids;  // Number of valid basis element in previous iteration
    do
      { //(Re)compute the basis index table:
        int ixred = 0;
        for(j = 0; j < basis_size; j++)
          { if (valid_basis[j]) { oldix[ixred] = j; ixred++; } }
        last_valids = valids;
        //first we copy the relevant rows and columns from A
        ixred = 0;
        for(i = 0; i < basis_size; i++)
          { for(j = 0; j < basis_size; j++)
              { int index = (i*basis_size) + j;
                if( valid_basis[i] && valid_basis[j] )
                  { Ared[ixred] = A[index]; ixred++; }
              }
          }
        assert(ixred == valids*valids);
        // Now compute the RHS for the reduced basis:
        computeLeastSquaresRHSVector(b,&phiRed,valids,X,F,n,w,wpos,l_data);
        gsel_solve(valids,valids, Ared, 1, b, c);
        // Scatter the coefficients to their original number:
        ixred = valids;
        for (j = basis_size-1; j >= 0; j--)
          { if(valid_basis[j])
              { ixred--; c[j] = c[ixred]; }
            else
              { c[j] = 0.0; }
          }
        valids = validateResults(c,basis_size,valid_basis);
        iterations++;
      }
    while((last_valids != valids) && (valids != 0));

    for(i = 0; i < basis_size; i++) { if(!valid_basis[i]) { c[i] = 0.0; } }
    free(Ared);
    free(valid_basis);
    free(oldix);
  }


void FitGaugeToModel
  ( r3_t* X,
    double* F,
    int n,
    lighting_model_t* lm,
    r3_t view_dir,
    void* l_data
  )
  {
    //allocate the full linear system for the given vasis:
    int basis_size = lm->get_num_components(l_data);
    double* A = (double*)malloc(sizeof(double)*(basis_size*basis_size));
    double* b = (double*)malloc(sizeof(double)*basis_size);
    double* c = (double*)malloc(sizeof(double)*basis_size);
    
    //allocate the "a priori" position-dependent data point weights:
    double* wpos = (double*)malloc(sizeof(double)*n);
    //allocate the dynamically adjusted data point weights (inlier probabilities):
    double* w = (double*)malloc(sizeof(double)*n); 

    double zMin = 0.0;
    debug_ldata(l_data);

    int i;
    for(i = 0; i < n; i++)
      { //initialize all adjustabe weights to 1:
        w[i] = 1.0;
        r3_t normal = X[i];
        //compute a priori weights based on inclination with view direction:
        double dot = r3_dot(&view_dir,&normal);
        double wraw = fmax(0,( dot- zMin)/(1.0 - zMin));
        wpos[i] = pow(wraw,2.0);
      }	

    //iterations for data point weight adjustment:
    double sigma = 0.2;
    fprintf(stderr,"Basis size is %d\n",lm->get_num_components(l_data));
    fprintf(stderr,"Starting weight-adjusting iteration SIGMA = %9.6f\n",sigma);
    int pass = 0;
    int npasses = 3;

    double threshold = 0.01;
    if(lm->type == POINTLIKE_MODEL)
      {
        fprintf(stderr," Start : ");
        lm->write_param(stderr,l_data);
      }
    for(pass = 0; pass < npasses; pass++)
      { fprintf(stderr,"Weight adjustment step %d\n", pass);
        //generate the stiffness matrix for the full basis:
        computeLeastSquaresMatrix(A,lm->phi,basis_size,X,n,w,wpos,l_data);
        
        int num_iters = 0;
        int MAX_ITER = 2000;
        double epsilon = 10e-5;
        double diff = 1.0; 
        while( (num_iters < MAX_ITER) && (diff > epsilon))
          {
            void* l_data_old = lm->copy_lighting_data(l_data);
            FitOneLight
              ( A, b, c,
                lm->phi, basis_size,
                lm->validate_results,
                X,F,n,
                w,
                wpos,
                l_data
              );

            /*update light model*/
            lm->retrieve_components(c,basis_size,l_data);
            diff = lm->compare_lightings(l_data,l_data_old);
            if(lm->type == POINTLIKE_MODEL)
              {
                fprintf(stderr," Adjusted: ");
                lm->write_param(stderr,l_data);
              }
            fprintf(stderr,"Finished Iteration %d - (diff %lf vs %lf) \n",num_iters,diff,epsilon);
            lm->release_lighting_data(l_data_old);
            num_iters++;
          }

        fprintf(stderr,"Computing errors and adjusting weight...");
        lm->update_weights(l_data,X,F,w,n,&sigma);

        int bad_lines = 0;
        double sumw = 1.0e-200;
        for(i = 0; i<  n; i++)
          {
            sumw+=w[i];
          }
        for(i = 0; i<  n; i++)
          {
            w[i] = (w[i]/sumw)*n;
            if(w[i] < threshold) bad_lines++;
          }
        fprintf(stderr,"OK - %d  bad lines in table (%9.6f %%)\n",bad_lines,100.0*bad_lines/(double)n);

      }

    free(A);
    free(b);

    free(c);

  }