/////////////////////////////////////////////////////////////////////////////////
//// 
////  Verification routines for the jacobians employed in the expert & simple drivers
////  for sparse bundle adjustment based on the Levenberg - Marquardt minimization algorithm
////  Copyright (C) 2005  Manolis Lourakis (lourakis@ics.forth.gr)
////  Institute of Computer Science, Foundation for Research & Technology - Hellas
////  Heraklion, Crete, Greece.
////
////  This program is free software; you can redistribute it and/or modify
////  it under the terms of the GNU General Public License as published by
////  the Free Software Foundation; either version 2 of the License, or
////  (at your option) any later version.
////
////  This program is distributed in the hope that it will be useful,
////  but WITHOUT ANY WARRANTY; without even the implied warranty of
////  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
////  GNU General Public License for more details.
////
///////////////////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>

#include "sba.h"

#define emalloc(sz)       emalloc_(__FILE__, __LINE__, sz)

#define FABS(x)           (((x)>=0)? (x) : -(x))


/* inline */
#ifdef _MSC_VER
#define inline __inline //MSVC
#elif !defined(__GNUC__)
#define inline //other than MSVC, GCC: define empty
#endif

/* auxiliary memory allocation routine with error checking */
inline static void *emalloc_(char *file, int line, size_t sz)
{
void *ptr;

  ptr=(void *)malloc(sz);
  if(ptr==NULL){
    fprintf(stderr, "memory allocation request for %u bytes failed in file %s, line %d, exiting", sz, file, line);
    exit(1);
  }

  return ptr;
}

/* 
 * Check the jacobian of a projection function in nvars variables
 * evaluated at a point p, for consistency with the function itself.
 * Expert version
 *
 * Based on fortran77 subroutine CHKDER by
 * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
 * Argonne National Laboratory. MINPACK project. March 1980.
 *
 *
 * func points to a function from R^{nvars} --> R^{nobs}: Given a p in R^{nvars}
 *      it yields hx in R^{nobs}
 * jacf points to a function implementing the jacobian of func, whose consistency with
 *     func is to be tested. Given a p in R^{nvars}, jacf computes into the nvis*(Asz+Bsz)
 *     matrix jac the jacobian of func at p. Note the jacobian is sparse, consisting of
 *     all A_ij, B_ij and that row i of jac corresponds to the gradient of the i-th
 *     component of func, evaluated at p.
 * p is an input array of length nvars containing the point of evaluation.
 * idxij, rcidxs, rcsubs, mcon, cnp, pnp, mnp are as usual. Note that if cnp=0 or
 *     pnp=0 a jacobian corresponding resp. to motion or camera parameters
 *     only is assumed.
 * func_adata, jac_adata point to possible additional data and are passed
 *     uninterpreted to func, jacf respectively.
 * err is an array of length nobs. On output, err contains measures
 *     of correctness of the respective gradients. if there is
 *     no severe loss of significance, then if err[i] is 1.0 the
 *     i-th gradient is correct, while if err[i] is 0.0 the i-th
 *     gradient is incorrect. For values of err between 0.0 and 1.0,
 *     the categorization is less certain. In general, a value of
 *     err[i] greater than 0.5 indicates that the i-th gradient is
 *     probably correct, while a value of err[i] less than 0.5
 *     indicates that the i-th gradient is probably incorrect.
 *
 * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
 * following excerpt comes from CHKDER's documentation:
 *
 *     "The function does not perform reliably if cancellation or
 *     rounding errors cause a severe loss of significance in the
 *     evaluation of a function. therefore, none of the components
 *     of p should be unusually small (in particular, zero) or any
 *     other value which may cause loss of significance."
 */

void sba_motstr_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
const double factor=100.0, one=1.0, zero=0.0;
double *fvec, *fjac, *pp, *fvecp, *buf, *err;

int nvars, nobs, m, n, Asz, Bsz, nnz;
register int i, j, ii, jj;
double eps, epsf, temp, epsmch, epslog;
register double *ptr1, *ptr2, *pab;
double *pa, *pb;
int fvec_sz, pp_sz, fvecp_sz, numerr=0;

  nobs=idxij->nnz*mnp;
  n=idxij->nr; m=idxij->nc;
  nvars=m*cnp + n*pnp;
  epsmch=DBL_EPSILON;
  eps=sqrt(epsmch);

  Asz=mnp*cnp; Bsz=mnp*pnp;
  fjac=(double *)emalloc(idxij->nnz*(Asz+Bsz)*sizeof(double));

  fvec_sz=fvecp_sz=nobs;
  pp_sz=nvars;
  buf=(double *)emalloc((fvec_sz + pp_sz + fvecp_sz)*sizeof(double));
  fvec=buf;
  pp=fvec+fvec_sz;
  fvecp=pp+pp_sz;

  err=(double *)emalloc(nobs*sizeof(double));

  /* compute fvec=func(p) */
  (*func)(p, idxij, rcidxs, rcsubs, fvec, func_adata);

  /* compute the jacobian at p */
  (*jacf)(p, idxij, rcidxs, rcsubs, fjac, jac_adata);

  /* compute pp */
  for(j=0; j<nvars; ++j){
    temp=eps*FABS(p[j]);
    if(temp==zero) temp=eps;
    pp[j]=p[j]+temp;
  }

  /* compute fvecp=func(pp) */
  (*func)(pp, idxij, rcidxs, rcsubs, fvecp, func_adata);

  epsf=factor*epsmch;
  epslog=log10(eps);

  for(i=0; i<nobs; ++i)
    err[i]=zero;

  pa=p;
  pb=p + m*cnp;
  for(i=0; i<n; ++i){
    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero A_ij, B_ij, j=0...m-1, actual column numbers in rcsubs */
    for(j=0; j<nnz; ++j){
      if(rcsubs[j]<mcon) continue; // A_ij, B_ij are zero
 
      ptr2=err + idxij->val[rcidxs[j]]*mnp; // set ptr2 to point into err

      if(cnp){
        ptr1=fjac + idxij->val[rcidxs[j]]*Asz; // set ptr1 to point to A_ij
        pab=pa + rcsubs[j]*cnp;
        for(jj=0; jj<cnp; ++jj){
          temp=FABS(pab[jj]);
          if(temp==zero) temp=one;

          for(ii=0; ii<mnp; ++ii)
            ptr2[ii]+=temp*ptr1[ii*cnp+jj];
        }
      }

      if(pnp){
        ptr1=fjac + idxij->nnz*Asz + idxij->val[rcidxs[j]]*Bsz; // set ptr1 to point to B_ij
        pab=pb + i*pnp;
        for(jj=0; jj<pnp; ++jj){
          temp=FABS(pab[jj]);
          if(temp==zero) temp=one;

          for(ii=0; ii<mnp; ++ii)
            ptr2[ii]+=temp*ptr1[ii*pnp+jj];
        }
      }
    }
  }

  for(i=0; i<nobs; ++i){
    temp=one;
    if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
        temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
    err[i]=one;
    if(temp>epsmch && temp<eps)
        err[i]=(log10(temp) - epslog)/epslog;
    if(temp>=eps) err[i]=zero;
  }

  free(fjac);
  free(buf);

  for(i=0; i<n; ++i){
    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero err_ij, j=0...m-1 */
    for(j=0; j<nnz; ++j){
      if(rcsubs[j]<mcon) continue; // corresponding gradients are taken to be zero

      ptr1=err + idxij->val[rcidxs[j]]*mnp; // set ptr1 to point into err
      for(ii=0; ii<mnp; ++ii)
        if(ptr1[ii]<=0.5){
          fprintf(stderr, "Gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
                  idxij->val[rcidxs[j]]*mnp+ii, ii, i, rcsubs[j], (ptr1[ii]==0.0)? "wrong" : "probably wrong", ptr1[ii]);
          ++numerr;
        }
    }
  }
  if(numerr) fprintf(stderr, "Found %d suspicious gradients out of %d\n\n", numerr, nobs);

  free(err);

  return;
}

void sba_mot_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata)
{
  sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, mcon, cnp, 0, mnp, func_adata, jac_adata);
}

void sba_str_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int pnp, int mnp, void *func_adata, void *jac_adata)
{
  sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, 0, 0, pnp, mnp, func_adata, jac_adata);
}

#if 0
/* Routines for directly checking the jacobians supplied to the simple drivers.
 * They shouldn't be necessary since these jacobians can be verified indirectly
 * through the expert sba_XXX_chkjac_x() routines.
 */

/*****************************************************************************************/
// Sample code for using sba_motstr_chkjac():

  for(i=0; i<n; ++i)
    for(j=mcon; j<m; ++j){
      if(!vmask[i*m+j]) continue; // point i does not appear in image j

    sba_motstr_chkjac(proj, projac, p+j*cnp, p+m*cnp+i*pnp, j, i, cnp, pnp, mnp, adata, adata);
  }


/*****************************************************************************************/


/* union used for passing pointers to the user-supplied functions for the motstr/mot/str simple drivers */
union proj_projac{
  struct{
    void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata);
    void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata);
  } motstr;

  struct{
    void (*proj)(int j, int i, double *aj, double *xij, void *adata);
    void (*projac)(int j, int i, double *aj, double *Aij, void *adata);
  } mot;

  struct{
    void (*proj)(int j, int i, double *bi, double *xij, void *adata);
    void (*projac)(int j, int i, double *bi, double *Bij, void *adata);
  } str;
};


/* 
 * Check the jacobian of a projection function in cnp+pnp variables
 * evaluated at a point p, for consistency with the function itself.
 * Simple version of the above, NOT to be called directly
 *
 * Based on fortran77 subroutine CHKDER by
 * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
 * Argonne National Laboratory. MINPACK project. March 1980.
 *
 *
 * proj points to a function from R^{cnp+pnp} --> R^{mnp}: Given a p=(aj, bi) in R^{cnp+pnp}
 *      it yields hx in R^{mnp}
 * projac points to a function implementing the jacobian of func, whose consistency with proj
 *     is to be tested. Given a p in R^{cnp+pnp}, jacf computes into the matrix jac=[Aij | Bij]
 *     jacobian of proj at p. Note that row i of jac corresponds to the gradient of the i-th
 *     component of proj, evaluated at p.
 * aj, bi are input arrays of lengths cnp, pnp containing the parameters for the point of
 *     evaluation, i.e. j-th camera and i-th point
 * jj, ii specify the point (ii) whose projection jacobian in image (jj) is being checked
 * cnp, pnp, mnp are as usual. Note that if cnp=0 or
 *     pnp=0 a jacobian corresponding resp. to motion or camera parameters
 *     only is assumed.
 * func_adata, jac_adata point to possible additional data and are passed
 *     uninterpreted to func, jacf respectively.
 * err is an array of length mnp. On output, err contains measures
 *     of correctness of the respective gradients. if there is
 *     no severe loss of significance, then if err[i] is 1.0 the
 *     i-th gradient is correct, while if err[i] is 0.0 the i-th
 *     gradient is incorrect. For values of err between 0.0 and 1.0,
 *     the categorization is less certain. In general, a value of
 *     err[i] greater than 0.5 indicates that the i-th gradient is
 *     probably correct, while a value of err[i] less than 0.5
 *     indicates that the i-th gradient is probably incorrect.
 *
 * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
 * following excerpt comes from CHKDER's documentation:
 *
 *     "The function does not perform reliably if cancellation or
 *     rounding errors cause a severe loss of significance in the
 *     evaluation of a function. therefore, none of the components
 *     of p should be unusually small (in particular, zero) or any
 *     other value which may cause loss of significance."
 */

static void sba_chkjac(
    union proj_projac *funcs, double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
const double factor=100.0, one=1.0, zero=0.0;
double *fvec, *fjac, *Aij, *Bij, *ajp, *bip, *fvecp, *buf, *err;

int Asz, Bsz;
register int i, j;
double eps, epsf, temp, epsmch, epslog;
int fvec_sz, ajp_sz, bip_sz, fvecp_sz, err_sz, numerr=0;

  epsmch=DBL_EPSILON;
  eps=sqrt(epsmch);

  Asz=mnp*cnp; Bsz=mnp*pnp;
  fjac=(double *)emalloc((Asz+Bsz)*sizeof(double));
  Aij=fjac;
  Bij=Aij+Asz;

  fvec_sz=fvecp_sz=mnp;
  ajp_sz=cnp; bip_sz=pnp;
  err_sz=mnp;
  buf=(double *)emalloc((fvec_sz + ajp_sz + bip_sz + fvecp_sz + err_sz)*sizeof(double));
  fvec=buf;
  ajp=fvec+fvec_sz;
  bip=ajp+ajp_sz;
  fvecp=bip+bip_sz;
  err=fvecp+fvecp_sz;

  /* compute fvec=proj(p), p=(aj, bi) & the jacobian at p */
  if(cnp && pnp){
    (*(funcs->motstr.proj))(jj, ii, aj, bi, fvec, func_adata);
    (*(funcs->motstr.projac))(jj, ii, aj, bi, Aij, Bij, jac_adata);
  }
  else if(cnp){
    (*(funcs->mot.proj))(jj, ii, aj, fvec, func_adata);
    (*(funcs->mot.projac))(jj, ii, aj, Aij, jac_adata);
  }
  else{
    (*(funcs->str.proj))(jj, ii, bi, fvec, func_adata);
    (*(funcs->str.projac))(jj, ii, bi, Bij, jac_adata);
  }

  /* compute pp, pp=(ajp, bip) */
  for(j=0; j<cnp; ++j){
    temp=eps*FABS(aj[j]);
    if(temp==zero) temp=eps;
    ajp[j]=aj[j]+temp;
  }
  for(j=0; j<pnp; ++j){
    temp=eps*FABS(bi[j]);
    if(temp==zero) temp=eps;
    bip[j]=bi[j]+temp;
  }

  /* compute fvecp=proj(pp) */
  if(cnp && pnp)
    (*(funcs->motstr.proj))(jj, ii, ajp, bip, fvecp, func_adata);
  else if(cnp)
    (*(funcs->mot.proj))(jj, ii, ajp, fvecp, func_adata);
  else
    (*(funcs->str.proj))(jj, ii, bip, fvecp, func_adata);

  epsf=factor*epsmch;
  epslog=log10(eps);

  for(i=0; i<mnp; ++i)
    err[i]=zero;

  for(j=0; j<cnp; ++j){
    temp=FABS(aj[j]);
    if(temp==zero) temp=one;

    for(i=0; i<mnp; ++i)
      err[i]+=temp*Aij[i*cnp+j];
  }
  for(j=0; j<pnp; ++j){
    temp=FABS(bi[j]);
    if(temp==zero) temp=one;

    for(i=0; i<mnp; ++i)
      err[i]+=temp*Bij[i*pnp+j];
  }

  for(i=0; i<mnp; ++i){
    temp=one;
    if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
        temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
    err[i]=one;
    if(temp>epsmch && temp<eps)
        err[i]=(log10(temp) - epslog)/epslog;
    if(temp>=eps) err[i]=zero;
  }

  for(i=0; i<mnp; ++i)
    if(err[i]<=0.5){
      fprintf(stderr, "Gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
                i, i, ii, jj, (err[i]==0.0)? "wrong" : "probably wrong", err[i]);
      ++numerr;
  }
  if(numerr) fprintf(stderr, "Found %d suspicious gradients out of %d\n\n", numerr, mnp);

  free(fjac);
  free(buf);

  return;
}

void sba_motstr_chkjac(
    void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.motstr.proj=proj;
  funcs.motstr.projac=projac;

  sba_chkjac(&funcs, aj, bi, jj, ii, cnp, pnp, mnp, func_adata, jac_adata);
}

void sba_mot_chkjac(
    void (*proj)(int jj, int ii, double *aj, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.mot.proj=proj;
  funcs.mot.projac=projac;

  sba_chkjac(&funcs, aj, NULL, jj, ii, cnp, 0, mnp, func_adata, jac_adata);
}

void sba_str_chkjac(
    void (*proj)(int jj, int ii, double *bi, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.str.proj=proj;
  funcs.str.projac=projac;

  sba_chkjac(&funcs, NULL, bi, jj, ii, 0, pnp, mnp, func_adata, jac_adata);
}
#endif /* 0 */