/*
 * Copyright (C) 2021, Thibaud Briand, ENS Cachan <thibaud.briand@ens-cachan.fr>
 * Copyright (C) 2021, Jonathan Vacher, ENS Cachan <jvacher@ens-cachan.fr>
 *
 * 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 3 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#include "ps_lib.h"
#include "toolbox.h"
#include "Eigen/Dense"
#include <unsupported/Eigen/Polynomials>
#include <Eigen/Eigenvalues>

using namespace Eigen;
using namespace std;

// Allocate memory for the sample statistics
void allocate_stats(statsStruct *stats, const paramsStruct params, int nz)
{
  int i;
  int N_pyr = params.N_pyr;
  int N_steer = params.N_steer;
  int Na = params.Na;

  stats->pixelStats = (float *) malloc(N_PIXELSTATS*nz*sizeof(float));
  stats->skewLow = (float *) malloc((1+N_pyr)*nz*sizeof(float));
  stats->kurtLow = (float *) malloc((1+N_pyr)*nz*sizeof(float));
  stats->varHigh = (float *) malloc(nz*sizeof(float));
  stats->magMeans = (float *) malloc(N_pyr*N_steer*nz*sizeof(float));
  stats->autoCorLow = (float **) malloc((1+N_pyr)*sizeof(float *));
  for(i = 0; i < 1+N_pyr; i++)
    stats->autoCorLow[i] = (float *) malloc(Na*Na*nz*sizeof(float));
  stats->autoCorMag = (float **) malloc(N_pyr*N_steer*sizeof(float *));
  for(i = 0; i < N_pyr*N_steer; i++)
    stats->autoCorMag[i] = (float *) malloc(Na*Na*nz*sizeof(float));
  stats->cousinMagCor = (float **) malloc(N_pyr*sizeof(float *));
  for(i = 0; i < N_pyr; i++)
    stats->cousinMagCor[i] = (float *)
      malloc(N_steer*N_steer*nz*nz*sizeof(float));
  stats->parentMagCor = (float **) malloc((N_pyr-1)*sizeof(float *));
  for(i = 0; i < N_pyr-1; i++)
    stats->parentMagCor[i] = (float *)
      malloc(N_steer*N_steer*nz*nz*sizeof(float));
  stats->parentRealCor = (float **) malloc(N_pyr*sizeof(float *));
  for(i = 0; i < N_pyr-1; i++)
    stats->parentRealCor[i] = (float *)
      malloc(2*N_steer*N_steer*nz*nz*sizeof(float));
  if (nz == 3) {
    stats->parentRealCor[N_pyr-1] = (float *)
      malloc(N_SMALLEST*N_steer*nz*nz*sizeof(float));
    stats->pixelStatsPCA = (float *) malloc(N_PIXELSTATSPCA*nz*sizeof(float));
    stats->autoCorPCA = (float *) malloc(Na*Na*nz*sizeof(float));
    stats->cousinRealCor = (float **) malloc((1+N_pyr)*sizeof(float *));
    for(i = 0; i < N_pyr; i++)
      stats->cousinRealCor[i] = (float *)
        malloc(N_steer*N_steer*nz*nz*sizeof(float));
    stats->cousinRealCor[N_pyr] = (float *)
      malloc(N_SMALLEST*N_SMALLEST*nz*nz*sizeof(float));
  }
}

// Free memory for the sample statistics
void free_stats(statsStruct stats, const paramsStruct params, int nz)
{
  int i;
  int N_pyr = params.N_pyr;
  int N_steer = params.N_steer;

  free(stats.pixelStats);
  free(stats.skewLow);
  free(stats.kurtLow);
  free(stats.varHigh);
  free(stats.magMeans);
  for(i = 0; i < 1+N_pyr; i++) {
    free(stats.autoCorLow[i]);
    if (nz == 3)
      free(stats.cousinRealCor[i]);
  }
  free(stats.autoCorLow);
  for(i = 0; i < N_pyr*N_steer; i++)
    free(stats.autoCorMag[i]);
  free(stats.autoCorMag);
  for(i = 0; i < N_pyr - 1; i++) {
    free(stats.parentMagCor[i]);
    free(stats.parentRealCor[i]);
  }
  if ( nz == 3)
    free(stats.parentRealCor[N_pyr-1]);
  for(i = 0; i < N_pyr; i++) {
    free(stats.cousinMagCor[i]);
  }
  free(stats.cousinMagCor);
  free(stats.parentMagCor);
  free(stats.parentRealCor);
  if( nz == 3 ) {
    free(stats.cousinRealCor);
    free(stats.pixelStatsPCA);
    free(stats.autoCorPCA);
  }
}

// Function for writing the statistics in a file
void write_statistics(const statsStruct stats, const paramsStruct params, int nz)
{
  int i, j, l, ind;
  int N_pyr = params.N_pyr;
  int N_steer = params.N_steer;
  int Na = params.Na;

  // open file
  FILE *fp = fopen("statistics_evolution.txt", "a");

  // summary statistics (i)(a)
  for (i = 0; i < 1+N_pyr; i++) {
    for (l = 0; l < nz; l++)
      fprintf(fp, " %f %f", stats.skewLow[i + (1+N_pyr)*l],
              stats.kurtLow[i + (1+N_pyr)*l]);
    fprintf(fp, ",");
  }

  // summary statistics (i)(b)
  for(l = 0; l < nz; l++)
    fprintf(fp, " %f", stats.varHigh[l]);
  fprintf(fp, ",");

  // summary statistics (i)(c)
  for (l = 0; l < nz; l++)
    for (i = 0; i < 6; i++)
      fprintf(fp, " %f", stats.pixelStats[i + N_PIXELSTATS*l]);
  fprintf(fp, ",");

  // summary statistics (ii)
  for (i = 0; i < 1+N_pyr; i++) {
    for(l = 0; l < Na*Na*nz; l++)
      fprintf(fp, " %f", stats.autoCorLow[i][l]);
    fprintf(fp, ",");
  }

  // summary statistics (iii)
  for(i = 0; i < N_pyr; i++) {
    for(j = 0; j < N_steer; j++) {
      ind = j + i*N_steer;
      for(l = 0; l < Na*Na*nz; l++)
        fprintf(fp, " %f", stats.autoCorMag[ind][l]);
      fprintf(fp, ",");
    }
  }

  // summary statistics (iv)
  for(i = 0; i < N_pyr; i++) {
    for(l = 0; l < N_steer*N_steer*nz*nz; l++)
      fprintf(fp, " %f", stats.cousinMagCor[i][l]);
    fprintf(fp, ",");
  }

  // summary statistics (v)
  for(i = 0; i < N_pyr - 1; i++) {
    for(l = 0; l < N_steer*N_steer*nz*nz; l++)
      fprintf(fp, " %f", stats.parentMagCor[i][l]);
    fprintf(fp, ",");
  }

  // summary statistics (vi)
  for(i = 0; i < N_pyr - 1; i++) {
    for(l = 0; l < 2*N_steer*N_steer*nz*nz; l++)
      fprintf(fp, " %f", stats.parentRealCor[i][l]);
    fprintf(fp, ",");
  }

  if( nz == 3 ) {
    // summary statistics (vii)
    for(l = 0; l < 3; l++)
      for(i = 0; i < 3; i++)
        fprintf(fp, " %f", stats.covariancePCA[l][i]);
    fprintf(fp, ",");

    // summary statistics (viii)
    for(i = 0; i < Na*Na*nz; i++)
      fprintf(fp, " %f", stats.autoCorPCA[i]);
    fprintf(fp, ",");

    // summary statistics (ix)
    for (l = 0; l < 3; l++)
      for (i = 0; i < 2; i++)
        fprintf(fp, " %f", stats.pixelStatsPCA[i + N_PIXELSTATSPCA*l]);
    fprintf(fp, ",");

    // summary statistics (x)
    for(i = 0; i < N_pyr; i++) {
      for(l = 0; l < N_steer*N_steer*nz*nz; l++)
        fprintf(fp, " %f", stats.cousinRealCor[i][l]);
      fprintf(fp, ",");
    }

    // summary statistics (xi)
    for(l = 0; l < N_SMALLEST*N_SMALLEST*nz*nz; l++)
      fprintf(fp, " %f", stats.cousinRealCor[N_pyr][l]);
    fprintf(fp, ",");

    // summary statistics (xii)
    for(l = 0; l < N_steer*N_SMALLEST*nz*nz; l++)
      fprintf(fp, " %f", stats.parentRealCor[N_pyr-1][l]);
    fprintf(fp, ",");
  }

  // close file
  fprintf(fp, "\n");
  fclose(fp);
}

// Apply an integer shift of (ofx, ofy) to an image
// out(i,j) = in(i - ofx, j - ofy)
// It is done using the nearest neighbor interpolation
// with periodic boundary condition
void shift(float *out, const float *in, int ofx, int ofy,
           int nx, int ny, int nz)
{
  int i, j, l, i2, j2;
  for(j = 0; j < ny; j++) {
    j2 = (j - ofy) % ny;
    j2 = (j2 < 0) ? j2 + ny : j2;
    for(i = 0; i < nx; i++) {
      i2 = (i - ofx) % nx;
      i2 = (i2 < 0) ? i2 + nx : i2;
      for(l = 0; l < nz; l++)
          out[i+j*nx+l*nx*ny] = in[i2 + j2*nx + l*nx*ny];
    }
  }
}

/* moment part */

// Compute the mean of an array
float mean(const float *data, int N)
{
  double m = 0.0;

  // sum loop
  for(int i = 0; i < N; i++)
    m += data[i];

  // normalization
  m /= N;

  return m;
}

// Compute the moment of a given order of an array (Equation 39)
// To save computations the mean is computed outside this function
float compute_moment(const float *data, float m, int order, int N)
{
  double moment = 0.0;
  double tmp, tmp2;

  // sum loop
  for(int i = 0; i < N; i++) {
    tmp = 1.0;
    tmp2 = data[i] - m;
    for(int j = 0; j < order; j++)
      tmp *= tmp2;
    moment += tmp;
  }

  // normalization
  moment /= N;

  return moment;
}

// Compute the skewness of an array (Equation 40)
// Set the value to 0 for a constant image
float compute_skewness(const float *data_in, float m, float var, int N)
{
  float order_3 = compute_moment(data_in, m, 3, N);
  float skewness = (var > 0) ? order_3/sqrt(var*var*var) : 0;

  return skewness;
}

// Compute the kurtosis of an array (Equation 40)
// Set the value to 3 for a constant image
float compute_kurtosis(const float *data_in, float m, float var, int N)
{
  float order_4 = compute_moment(data_in, m, 4, N);
  float kurtosis = (var > 0) ? order_4/(var*var) : 3;

  return kurtosis;
}

/* range part */

// Compute the min and max values of an array
void min_and_max(float *m, float *M, const float *tab, int N)
{
  *m = *M = tab[0];

  for(int i = 1; i < N; i++) {
    if (tab[i] < *m) *m = tab[i];
    if (tab[i] > *M) *M = tab[i];
  }
}

// Adjust the range of an array so that it fits in [m;M] (Appendix A.1.1)
// This corresponds to Equation 49
void adjust_range(float *data, float m, float M, int N)
{
  // loop on the array
  for(int i = 0; i < N; i++) {
    // projection in [m,M]
    if (data[i] < m)
      data[i] = m;
    else if (data[i] > M)
      data[i] = M;
  }
}

/* Modification part of marginal statistics */

// Adjust the mean and the variance of an array (Appendix A.1.2)
// This corresponds to Equation 50
void adjust_mean_variance(float *data, float mean_out, float var_out, int N)
{
  // compute the input mean and variance
  float m = mean(data, N);
  float var_in = compute_moment(data, m, 2, N);

  float factor = (var_in > 0) ? sqrt(var_out/var_in) : 1;
  // loop on the array
  for(int i = 0; i < N; i++)
    data[i] = factor*(data[i]-m) + mean_out;
}

// Adjust the skewness of an array (Appendix A.1.3)
// This corresponds to Algorithm 6
// The input data is assumed to have a zero mean
void adjust_skewness(float *data, float sk_out, int N)
{
  int i;

  // computation of the moments (Line 1)
  // this is done outside of the function compute_moment to save computations
  double m2 = 0.0;
  double m3 = 0.0;
  double m4 = 0.0;
  double m5 = 0.0;
  double m6 = 0.0;
  double tmp, tmp2;
  for (i = 0; i < N; i++) {
    tmp2 = tmp = data[i];

    tmp2 *= tmp;
    m2 += tmp2;

    tmp2 *= tmp;
    m3 += tmp2;

    tmp2 *= tmp;
    m4 += tmp2;

    tmp2 *= tmp;
    m5 += tmp2;

    tmp2 *= tmp;
    m6 += tmp2;
  }
  tmp = 1.0/N;
  m2 *= tmp;
  m3 *= tmp;
  m4 *= tmp;
  m5 *= tmp;
  m6 *= tmp;

  // compute skewness of the input (Line 2)
  double std = sqrt(m2);
  double sk_in = m3/(std*std*std);

  // security check
  double snr = 20*log10(fabs(sk_out/(sk_out-sk_in)));

  // do nothing if the skewness are too close
  if ( snr <= 60 ) {
    // polynomial coefficients computation (Line 3)
    // coefficients of the numerator P (Equation 56)
    double p0 = sk_in*m2*std;
    double p1 = 3 * ( m4 - m2 * m2 * ( 1 + sk_in*sk_in ) );
    double p2 = 3 * ( m5 - 2*std*sk_in*m4 + m2*m2*std*sk_in*sk_in*sk_in );
    double p3 = m6 - 3*std*sk_in*m5 + 3*m2*(sk_in*sk_in - 1)*m4
                + m2*m2*m2*(2 + 3*sk_in*sk_in - sk_in*sk_in*sk_in*sk_in);
    Vector4d poly_num;
    poly_num << p0, p1, p2, p3;

    // coefficients of the denominator Q (Equation 56)
    double q0 = m2;
    double q1 = 0;
    double q2 = m4 - (1 + sk_in*sk_in)*m2*m2;
    Vector3d poly_denom;
    poly_denom << q0, q1, q2;

    // coefficients of Q^3(X^2) (Equation 60)
    double b0 = q0*q0*q0;
    double b2 = 3*q0*q0*q2;
    double b4 = 3*q0*q2*q2;
    double b6 = q2*q2*q2;
    VectorXd b_poly(4);
    b_poly << b0, b2, b4, b6;

    // derivative with respect to lambda (Equation 62)
    double d0 = p1*b0;
    double d1 = -p0*b2 + 2*p2*b0;
    double d2 = 3*p3*b0;
    double d3 = -2*p0*b4 + p2*b2;
    double d4 = -p1*b4 + 2*p3*b2;
    double d5 = -3*p0*b6;
    double d6 = -2*p1*b6 + p3*b4;
    double d7 = -p2*b6;
    VectorXd derivative(8);
    derivative << d0, d1, d2, d3, d4, d5, d6, d7;

    // compute the roots of d (Line 4)
    PolynomialSolver< double, 7 > dsolve( derivative );

    // find the minimal and maximal skewness reachable (Line 5 and Line 6)
    double lneg = - 1e6;
    double lpos = 1e6;
    double real_part;
    for (i = 0; i < 7; i++) {
      real_part = dsolve.roots()[i].real();
      if ( fabs(dsolve.roots()[i].imag()/real_part)<1e-6 ) {
        if ( real_part < 0 && real_part > lneg )
          lneg = real_part;
        else if ( real_part > 0 && real_part < lpos )
          lpos = real_part;
      }
    }
    double skmin = poly_eval(poly_num, lneg)/sqrt(poly_eval(b_poly, lneg*lneg));
    double skmax = poly_eval(poly_num, lpos)/sqrt(poly_eval(b_poly, lpos*lpos));

    // solve for lambda (Line 7 to 19)
    double lambda = 0.0;
    if ( sk_out <= skmin ) // saturating down the skewness (Line 8 and Line 9)
      lambda = lneg;
    else if ( sk_out >= skmax ) // saturating up the skewness (Line 10 and Line 11)
      lambda = lpos;
    else {
      // define the polynomial A = P^2 - sk_out^2*Q^2 (Equation 58)
      // This is done in Line 3
      double sk_out2 = sk_out * sk_out;
      double a0 = p0*p0 - sk_out2*b0;
      double a1 = 2*p1*p0;
      double a2 = p1*p1 + 2*p2*p0 - sk_out2*b2;
      double a3 = 2*(p3*p0 + p1*p2);
      double a4 = p2*p2 + 2*p3*p1 - sk_out2*b4;
      double a5 = 2*p3*p2;
      double a6 = p3*p3 - sk_out2*b6;

      // declare the polynomials
      VectorXd poly(7);
      poly << a0, a1, a2, a3, a4, a5, a6;

      // solve the polynomial (Line 13)
      PolynomialSolver< double, 6 > psolve( poly );

      // keep the real roots (Line 14)
      double real_roots[6] = {0};
      int p = 0; // number of real roots
      for(i = 0; i < 6; i++) {
        real_part = psolve.roots()[i].real();
        if ( fabs(psolve.roots()[i].imag()/real_part) < 1e-6 )
          real_roots[p++]= real_part;
      }

      if ( p == 1 ) // if only one solution left (Line 15 and Line 16)
        lambda = real_roots[0];
      else if ( p > 1 ) { // if several solutions left (Line 17)
        // keep the roots giving the numerator of good sign (Line 18)
        // if the sign is 0 it is accepted
        int q = 0; // number of acceptable solution
        double final_roots[6]={0};
        int sign0 = ( fabs(sk_out)<1e-6 ) ? 0 : (sk_out>0)-(sk_out<0);
        int sign;
        double numerator;
        for(i = 0; i < p; i++) {
          numerator = poly_eval(poly_num, real_roots[i]);
          sign = ( fabs(numerator)<1e-6 ) ? 0 : (numerator > 0)-(numerator < 0);
          if( sign == sign0 || sign*sign0 == 0 )
            final_roots[q++]= real_roots[i];
        }

        // get the solution with minimal modulus (Line 19)
        if(q > 0) {
          lambda = final_roots[0];
          double modulus = fabs(lambda);
          for (i = 1; i < q; i++) {
            double modulus2 = fabs(final_roots[i]);
            if ( modulus2 < modulus) {
              lambda = final_roots[i];
              modulus = modulus2;
            }
          }
        }
      }
    }

    // gradient descent (Line 20)
    double g;
    double stdsk = std*sk_in;
    for(i = 0; i < N ; i++) {
      g = data[i]*(data[i]-stdsk)-m2;
      data[i] += lambda*g;
    }

    // adjust the mean and the variance (to insure the stability of the code)
    adjust_mean_variance(data, 0.0, m2, N);
  }
}

// Adjust the kurtosis of an array (Appendix A.1.4)
// This corresponds to Algorithm 7
// The input data is assumed to have a zero mean
void adjust_kurtosis(float *data, float ku_out, int N)
{
  int i;

  // computation of the moments (Line 1)
  // this is done outside of compute_moment to save computations
  double m2 = 0.0;
  double m3 = 0.0;
  double m4 = 0.0;
  double m5 = 0.0;
  double m6 = 0.0;
  double m7 = 0.0;
  double m8 = 0.0;
  double m9 = 0.0;
  double m10 = 0.0;
  double m12 = 0.0;
  double tmp, tmp2;
  for (i = 0; i < N; i++) {
    tmp2 = tmp = data[i];

    tmp2 *= tmp;
    m2 += tmp2;

    tmp2 *= tmp;
    m3 += tmp2;

    tmp2 *= tmp;
    m4 += tmp2;

    tmp2 *= tmp;
    m5 += tmp2;

    tmp2 *= tmp;
    m6 += tmp2;

    tmp2 *= tmp;
    m7 += tmp2;

    tmp2 *= tmp;
    m8 += tmp2;

    tmp2 *= tmp;
    m9 += tmp2;

    tmp2 *= tmp;
    m10 += tmp2;

    tmp2 *= tmp*tmp;
    m12 += tmp2;
  }
  tmp = 1.0/N;
  m2 *= tmp;
  m3 *= tmp;
  m4 *= tmp;
  m5 *= tmp;
  m6 *= tmp;
  m7 *= tmp;
  m8 *= tmp;
  m9 *= tmp;
  m10 *= tmp;
  m12 *= tmp;

  // compute the kurtosis of the input (Line 2)
  double ku_in = m4/(m2*m2);

  // security check
  double snr = 20*log10(fabs(ku_out/(ku_out-ku_in)));
  if ( snr <= 60 ) { // do nothing if the kurtoses are too close
    // polynomial coefficients computation (Line 3)
    // auxilary useful variable
    double alpha = m4/m2;

    // define the coefficients of the numerator (Line 67)
    double p0 = m4;
    double p1 = 4 * ( m6 - alpha * alpha * m2 - m3 * m3 );
    double p2 = 6 * ( m8 - 2 * alpha * m6 - 2 * m3 * m5 + alpha * alpha * m4
      + (m2 + 2*alpha) * m3 * m3 );
    double p3 = 4 * ( m10 - 3 * alpha * m8 - 3 * m3 * m7
      + 3 * alpha * alpha * m6 + 6 * alpha * m3 * m5
      + 3 * m3 * m3 * m4 - alpha * alpha * alpha * m4
      - 3 * alpha * alpha * m3 * m3 - 3 * m4 * m3 * m3 );
    double p4 =  m12 - 4 * alpha * m10 - 4 * m3 * m9
      + 6 * alpha * alpha * m8 + 12 * alpha * m3 * m7 +  6 * m3 * m3 * m6
      - 4 * alpha * alpha * alpha * m6 - 12 * alpha * alpha * m3 * m5
      + alpha * alpha * alpha * alpha * m4 - 12 * alpha * m3 * m3 * m4
      + 4 * alpha * alpha * alpha * m3 * m3 + 6 * alpha * alpha * m3 * m3 * m2
      - 3 * m3 * m3 * m3 * m3;
    VectorXd poly_num(5);
    poly_num << p0, p1, p2, p3, p4;

    // define the coefficients of the denominator (Line 67)
    double q0 = m2;
    double q1 = 0;
    double q2 = p1*0.25;
    Vector3d poly_denom;
    poly_denom << q0, q1, q2;

    // derivative with respect to lambda (Line 71)
    double d0 = p1*q0;
    double d1 = -4*q2*p0 + 2*p2*q0;
    double d2 = -3*q2*p1 + 3*p3*q0;
    double d3 = -2*p2*q2 + 4*p4*q0;
    double d4 = -p3*q2;
    VectorXd derivative(5);
    derivative << d0, d1, d2, d3, d4;

    // compute the roots of d (Line 4)
    PolynomialSolver< double, 4 > dsolve( derivative );

    // find the minimal and maximal kurtosis reachable (Line 5 and Line 6)
    double lneg = - 1e6;
    double lpos = 1e6;
    double real_part;
    for (i = 0; i<4; i++) {
      real_part = dsolve.roots()[i].real();
      if ( fabs(dsolve.roots()[i].imag()/real_part) < 1e-6 ) {
        if ( real_part < 0 && real_part > lneg )
          lneg = real_part;
        else if ( real_part > 0 && real_part < lpos )
          lpos = real_part;
      }
    }
    tmp = poly_eval(poly_denom, lneg);
    double kumin = poly_eval(poly_num, lneg)/(tmp*tmp);
    tmp = poly_eval(poly_denom, lpos);
    double kumax = poly_eval(poly_num, lpos)/(tmp*tmp);

    // solves for lambda (Line 7 to 15)
    double lambda = 0.0;
    if ( ku_out <= kumin ) // saturating down the skewness (Line 8 and Line 9)
      lambda = lneg;
    else if ( ku_out >= kumax ) // saturating up the skewness (Line 10 and Line 11)
      lambda = lpos;
    else {
      // define the polynomial (Equation 69)
      // this is done in Line 3
      double a4 = p4 - ku_out*q2*q2;
      double a3 = p3;
      double a2 = p2 - 2*ku_out*q0*q2;
      double a1 = p1;
      double a0 = p0 - ku_out*q0*q0;
      VectorXd poly(5);
      poly << a0, a1, a2, a3, a4;

      // compute the roots of a (Line 13)
      PolynomialSolver< double, 4 > psolve( poly );

      // keep the real roots (Line 14)
      double real_roots[4] = {0};
      int p = 0; // number of real roots
      for(i = 0; i < 4; i++) {
        real_part = psolve.roots()[i].real();
        if ( fabs(psolve.roots()[i].imag()/real_part) < 1e-6 )
            real_roots[p++] = real_part;
      }

      // if acceptable solutions get the one with smallest modulus (Line 15)
      if ( p > 0 ) {
        lambda = real_roots[0];
        double modulus = fabs(lambda);
        for (i = 1; i < p; i++) {
          double modulus2 = fabs(real_roots[i]);
          if ( modulus2 < modulus) {
            lambda = real_roots[i];
            modulus = modulus2;
          }
        }
      }
    }

    // gradient descent (Line 16)
    double g;
    for(i = 0; i < N ; i++) {
      g = data[i] * (data[i] * data[i] - alpha ) - m3;
      data[i] += lambda * g;
    }

    // adjust the mean and the variance
    adjust_mean_variance(data, 0.0, m2, N);
  }
}

/* Correlation part */

// Compute the central part of the auto-correlation of an image (Equation 42)
// Na is assumed to be an odd number smaller than nx and ny
// The input data is assumed to have a zero mean
void compute_auto_cor(float *Ac, const float *in, fftwf_complex *in_plan,
                      fftwf_complex *out_plan, fftwf_plan plan,
                      fftwf_plan iplan, int nx, int ny, int nz, int Na)
{
  int i, j, l;

  // memory allocation
  fftwf_complex *fft = (fftwf_complex *)
    fftwf_malloc( nx*ny*nz * sizeof(fftwf_complex));
  float *full_auto_cor = (float *) malloc(nx*ny*nz*sizeof(float));

  // compute the fft
  do_fft_plan_real(plan, out_plan, in_plan, fft, in, nx*ny, nz);

  // squared modulus
  for(i = 0; i < nx*ny*nz; i++) {
    fft[i][0] = fft[i][0]*fft[i][0] + fft[i][1]*fft[i][1];
    fft[i][1] = 0.0;
  }

  // compute the inverse fft
  do_ifft_plan_real(iplan, out_plan, in_plan, full_auto_cor, fft, nx*ny, nz);

  // keep the central part of size Na*Na
  int hNa = (Na-1)/2;
  float ifactor = 1.0/(nx*ny);
  int ind;
  for(i = 0; i < Na ;i++) {
    for(j = 0; j < Na; j++) {
      if((i < hNa) && (j < hNa))
        ind = nx - hNa + i + (ny-hNa+j)*nx;
      else if((i < hNa) && (j> hNa-1))
        ind = nx - hNa + i + (j-hNa)*nx;
      else if((i > hNa-1) && (j < hNa))
        ind = i - hNa + (ny-hNa+j)*nx;
      else
        ind = i - hNa + (j-hNa)*nx;

      for(l = 0; l < nz; l++)
        Ac[i + j*Na + l*Na*Na] = full_auto_cor[ind + l*nx*ny]*ifactor;
    }
  }

  // free memory
  fftwf_free(fft);
  free(full_auto_cor);
}

// Compute the pairwise cross-correlation matrix (Equation 43)
// The input data is assumed to have a zero mean
void compute_cross_cor(float *cross_cor, float **data, int N_data,
                       int N, int nz)
{
  int i, j, l;
  MatrixXf M(N_data*nz, N);

  // put data in a matrix M
  for(i = 0; i < N_data; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l < nz; l++)
        M(i + l*N_data,j) = data[i][j + l*N];

  // compute the cross-correlation
  MatrixXf tmp = M*(M.transpose())/N;

  // put the cross-correlation in the ouput array
  for(i = 0; i < N_data*nz; i++)
    for(j = 0; j < N_data*nz; j++)
      cross_cor[j + i*N_data*nz] = tmp(i,j);
}

// Compute the cross-correlation matrix (Equation 44)
// The input data is assumed to have a zero mean
void compute_cross_scale_cor(float *cross_scale_cor, float **data1,
                             float **data2, int N_data1, int N_data2,
                             int N, int nz)
{
  int i, j, l;
  MatrixXf X(N_data1*nz, N);
  MatrixXf Y(N_data2*nz, N);

  // put data in a matrix X
  for(i = 0; i < N_data1; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l<nz; l++)
        X(i + l*N_data1,j)= data1[i][j + l*N];

  // put data in a matrix Y
  for(i = 0; i < N_data2; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l<nz; l++)
        Y(i + l*N_data2,j) = data2[i][j + l*N];

  MatrixXf tmp = X*(Y.transpose())/N;

  // put the cross-correlation in the ouput array
  for(i = 0; i < N_data1*nz; i++)
    for(j = 0; j < N_data2*nz; j++)
      cross_scale_cor[j + i*N_data2*nz] = tmp(i,j);
}

// Adjust the central part of the auto-correlation (Appendix A.2)
// This corresponds to Algorithm 8
// The input data is assumed to have a zero mean
void adjust_auto_cor(float *data, const float *Ac, const float *var0,
                  const float *vari, fftwf_complex *in_plan,
                  fftwf_complex *out_plan, fftwf_plan plan, fftwf_plan iplan,
                  int nx, int ny, int nz, int Na, int scale)
{
  int i, j, k, l, p;

  // central location
  int hNa = ((Na-1)/2);

  // memory allocation
  fftwf_complex *fft_data = (fftwf_complex *)
    fftwf_malloc( nx*ny*nz * sizeof(fftwf_complex));
  fftwf_complex *fft_tmp = (fftwf_complex *)
    fftwf_malloc( nx*ny*nz * sizeof(fftwf_complex));
  float *tmp = (float *) malloc(nx*ny*nz*sizeof(float));
  int Na2 = 2*Na - 1;
  float *Ac_in = (float *) malloc(Na2*Na2*nz*sizeof(float));

  // compute the fft of the input (Line 1)
  do_fft_plan_real(plan, out_plan, in_plan, fft_data, data, nx*ny, nz);

  // computation of the auto-correlation image (Line 2)
  // square the modulus of the DFT
  for(i = 0; i<nx*ny*nz; i++) {
    fft_tmp[i][0] = fft_data[i][0]*fft_data[i][0]
      + fft_data[i][1]*fft_data[i][1];
    fft_tmp[i][1] = 0.0;
  }

  // compute inverse fft
  do_ifft_plan_real(iplan, out_plan, in_plan, tmp, fft_tmp, nx*ny, nz);

  // keep the central part of size (2*Na-1)*(2*Na-1) and renormalize
  int hNa2 = (Na2-1)/2;
  float ifactor = 1.0/(nx*ny);
  int ind;
  for(i = 0; i < Na2 ; i++) {
    for(j = 0; j < Na2; j++) {
      if((i < hNa2) && (j < hNa2))
        ind = nx - hNa2 + i + (ny-hNa2+j)*nx;
      else if((i < hNa2) && (j > hNa2-1))
        ind = nx - hNa2 + i + (j-hNa2)*nx;
      else if((i > hNa2-1) && (j < hNa2))
        ind = i - hNa2 + (ny-hNa2+j)*nx;
      else
        ind = i - hNa2 + (j-hNa2)*nx;

      for(l = 0; l < nz; l++)
        Ac_in[i + j*Na2 + l*Na2*Na2] = tmp[ind + l*nx*ny]*ifactor;
    }
  }

  // declaration for the following computations
  int t = ((Na*Na+1)/2);
  MatrixXf A(t,t);
  VectorXf B(t);
  MatrixXf M(Na,Na);
  MatrixXf rM(Na,Na);
  MatrixXf auto_cor_in(Na2,Na2);
  MatrixXf auto_cor_out(Na,Na);
  int end_loop;
  float factor;
  float tol = (nz == 3) ? 1e-3 : 1e-4;

  // loop over the channels
  for(l = 0; l < nz; l++) {
    if ( vari[l]/var0[l] > tol ) { // perform adjustment
      // put auto-correlation arrays in matrices
      for(i = 0; i < Na2; i++)
        for(j = 0; j < Na2; j++)
          auto_cor_in(j,i) = Ac_in[i + j*Na2 + l*Na2*Na2];
      for(i = 0; i < Na; i++)
        for(j = 0; j < Na; j++)
          auto_cor_out(j,i) = Ac[i + j*Na + l*Na*Na];

      // build the matrices involved in Equation 76
      // A <--> R(v)
      // B <--> R(u)
      for(k = hNa; k < Na; k++) {
        end_loop = (k < Na-1) ? hNa + Na : Na;
        for(p = hNa; p<end_loop; p++) {
          M = auto_cor_in.block(k - hNa, p - hNa, Na, Na);

          for(i = 0; i < Na; i++)
            for(j = 0; j < Na; j++)
              rM(i,j) = M(i,j) + M(Na-1-i,Na-1-j);
          rM(hNa, hNa) /= 2;
          rM.resize(Na*Na,1);
          for(i = 0; i < t; i++)
            A((k - hNa)*Na + p - hNa,i)=rM(i,0);
          rM.resize(Na,Na);
          B((k - hNa)*Na + p - hNa) = auto_cor_out(k - hNa,p - hNa);
        }
      }

      // Solve Equation 76 (Line 3)
      // A * sol = B
      VectorXf sol = A.colPivHouseholderQr().solve(B);

      // Rearrange indices to build the center of R(h_lambda)
      MatrixXf fullsol(2*t-1,1);
      for(i = 0; i < t; i++) {
        fullsol(i,0)=sol(i);
        if(i<t-1)
          fullsol(t + i,0) = sol(t - i -2);
      }
      fullsol.resize(Na,Na);

      // Pad the center with zeros
      MatrixXf hsquared0 = MatrixXf::Zero(ny,nx);
      hsquared0.block(ny/2-hNa,nx/2-hNa, Na, Na) = fullsol;
      MatrixXf shiftedhsquared0(ny,nx);
      shiftedhsquared0 << hsquared0.block(ny/2,nx/2,ny/2,nx/2),
        hsquared0.block(ny/2,0,ny/2,nx/2), hsquared0.block(0,nx/2,ny/2,nx/2),
        hsquared0.block(0,0,ny/2,nx/2);

      // store the matrix in an array before computing its DFT
      for(j = 0; j < ny; j++)
        for(i = 0; i < nx; i++)
          tmp[i + j*nx + l*nx*ny] = shiftedhsquared0(j,i);

      // compute the dft of the computed array (Line 4)
      do_fft_plan_real(plan, out_plan, in_plan, fft_tmp + l*nx*ny,
                       tmp + l*nx*ny, nx*ny, 1);

      // compute the convolution in Fourier domain (Line 5 and Equation 75)
      for(i = 0; i < nx*ny; i++) {
        // compute the square root of the absolute value of the real part
        factor = sqrt(fabs(fft_tmp[i + l*nx*ny][0]));

        // multiplication in Fourier domain
        fft_data[i + l*nx*ny][0] *= factor;
        fft_data[i + l*nx*ny][1] *= factor;
      }

      // compute the inverse fft (Line 6)
      do_ifft_plan_real(iplan, out_plan, in_plan, data + l*nx*ny,
                        fft_data + l*nx*ny, nx*ny, 1);
    }
    else { // variance adjustment
      adjust_mean_variance(data + l*nx*ny, 0.0, vari[l]/pow(16, scale), nx*ny);
    }
  }

  // free memory
  fftwf_free(fft_data);
  fftwf_free(fft_tmp);
  free(tmp);
  free(Ac_in);
}

// Adjust the pairwise cross-correlation of a list of sub-bands (Appendix A.3)
// This corresponds to Algorithm 9
// Linearly adjust variables in data to have the pairwise cross-correlation cross_cor
// The input data is assumed to have a zero mean
void adjust_cross_cor(float **data, const float *cross_cor,
                   int N_data, int N, int nz)
{
  int i, j, l;
  MatrixXf V(N_data*nz, N);

  // put data in a matrix V
  for(i = 0; i < N_data; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l < nz; l++)
        V(i + l*N_data,j) = data[i][j + l*N];

  // store the target cross-correlation in a matrix
  MatrixXf tildeC(N_data*nz, N_data*nz);
  for(i = 0; i < N_data*nz; i++)
    for(j = 0; j < N_data*nz; j++)
      tildeC(i,j) = cross_cor[j + i*N_data*nz];

  // compute the input pairwise cross-correlation (Line 1)
  MatrixXf C = V*(V.transpose())/N;

  // eigen decomposition to compute the square root of matrices
  SelfAdjointEigenSolver<MatrixXf> eigensolver_in(C);
  SelfAdjointEigenSolver<MatrixXf> eigensolver_out(tildeC);
  MatrixXf D_in = eigensolver_in.eigenvalues().asDiagonal();
  MatrixXf P_in = eigensolver_in.eigenvectors();
  MatrixXf D_out = eigensolver_out.eigenvalues().asDiagonal();
  MatrixXf P_out = eigensolver_out.eigenvectors();

  // square root and inverse square root
  // isD_in = D_in^{-1/2}
  // sD_out = D_out^{1/2}
  MatrixXf isD_in = MatrixXf::Zero(N_data*nz,N_data*nz);
  MatrixXf sD_out = MatrixXf::Zero(N_data*nz,N_data*nz);
  int test1 = 0, test2 = 0; // to test if the matrices stay at 0 or not
  for(i = 0; i < N_data*nz; i++) {
    if( D_in(i,i) > 1e-12 ) {
      isD_in(i,i) = 1.0/sqrt(D_in(i,i));
      test1 = 1;
    }
    if( D_out(i,i) > 0) {
      sD_out(i,i) = sqrt(D_out(i,i));
      test2 = 1;
    }
  }

  if (test1 && test2) { // if both matrices have non-zero values
    MatrixXf Lambda = P_out*sD_out*(P_out.transpose())*P_in*isD_in*(P_in.transpose());

    // update the matrix V (Line 3)
    V = Lambda*V;

    // update data
    for(i = 0; i < N_data; i++) {
      for(j = 0; j < N; j++)
        for(l = 0; l < nz; l++)
          data[i][j + l*N] = V(i + l*N_data,j);
    }
  }
}

// Adjust the color covariance matrix of PCA bands (Appendix A.3)
// This corresponds to Algorithm 9
// Linearly adjust variables in data to have the covariance matrix cov
// The computations are similar as in adjust_cross_cor but the structure
// of data is different
void adjust_covariance_color(float *data, const float cov[3][3], int N)
{
  int i, j;
  MatrixXf U(3, N);

  // put data in a matrix U
  for(i = 0; i < 3; i++)
    for(j = 0; j < N; j++)
      U(i,j) = data[j + i*N];

  // store the target cross-correlation in a matrix
  Matrix3f tildeC;
  for(i = 0; i < 3; i++)
    for(j = 0; j < 3; j++)
      tildeC(i,j) = cov[i][j];

  // compute the covariance matrix (Line 1)
  Matrix3f C = U*(U.transpose())/N;

  // eigen decomposition to compute the square root of matrices
  SelfAdjointEigenSolver<Matrix3f> eigensolver_in(C);
  SelfAdjointEigenSolver<Matrix3f> eigensolver_out(tildeC);
  Matrix3f D_in = eigensolver_in.eigenvalues().asDiagonal();
  Matrix3f P_in = eigensolver_in.eigenvectors();
  Matrix3f D_out = eigensolver_out.eigenvalues().asDiagonal();
  Matrix3f P_out = eigensolver_out.eigenvectors();

  // square root and inverse square root
  // isD_in = D_in^{-1/2}
  // sD_out = D_out^{1/2}
  Matrix3f isD_in = MatrixXf::Zero(3,3);
  Matrix3f sD_out = MatrixXf::Zero(3,3);
  int test1 = 0, test2 = 0; // to test if the matrices stay at 0 or not
  for(i = 0; i < 3; i++) {
    if( D_in(i,i) > 1e-12 ) {
      isD_in(i,i) = 1.0/sqrt(D_in(i,i));
      test1 = 1;
    }
    if( D_out(i,i) > 0 ) {
      sD_out(i,i) = sqrt(D_out(i,i));
      test2 = 1;
    }
  }

  if (test1 && test2) { // if both matrices have non-zero values
    // compute the solution matrix (Line 2)
    MatrixXf Lambda = P_out*sD_out*(P_out.transpose())*P_in*isD_in*(P_in.transpose());

    // update the matrix U (Line 3)
    U = Lambda*U;

    // update data
    for(i = 0; i < 3; i++) {
      for(j = 0; j < N; j++)
        data[j + i*N] = U(i,j);
    }
  }
}

// Adjust the cross-correlations of a list of sub-bands given a list
// of fixed sub-bands (Appendix A.3)
// This corresponds to Algorithm 10
// Linearly adjust variables in data1 to have the pairwise cross-correlation
// cross_cor and the cross-correlation cross_scale_cor
// The input data is assumed to have a zero mean
// Note that data2 is not modified
void adjust_cross_scale_cor(float **data1, float **data2, const float *cross_cor,
                         const float *cross_scale_cor, int N_data1, int N_data2,
                         int N, int nz)
{
  int i, j, l;
  MatrixXf V(N_data1*nz, N);
  MatrixXf W(N_data2*nz, N);

  float tol = (nz == 3) ? 1e-3 : 1e-6;

  // put data in a matrix V
  for(i = 0; i < N_data1; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l < nz; l++)
        V(i + l*N_data1,j) = data1[i][j + l*N];

  // put data in a matrix W
  for(i = 0; i < N_data2; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l < nz; l++)
        W(i + l*N_data2,j) = data2[i][j + l*N];

  // store the target pairwise cross-correlation in a matrix
  MatrixXf tildeC(N_data1*nz, N_data1*nz);
  for(i = 0; i < N_data1*nz; i++)
    for(j = 0; j < N_data1*nz; j++)
      tildeC(i,j) = cross_cor[j + i*N_data1*nz];

  // store the target cross-correlation in a matrix
  MatrixXf tildeD(N_data1*nz, N_data2*nz);
  for(i = 0; i < N_data1*nz; i++)
    for(j = 0; j < N_data2*nz; j++)
      tildeD(i,j) = cross_scale_cor[j + i*N_data2*nz];

  // compute the cross-correlations (Line 1)
  MatrixXf C = V*(V.transpose())/N;
  MatrixXf D = V*(W.transpose())/N;

  // matrix computation (Line 2 and Line 3)
  MatrixXf invE = (W*(W.transpose())/N).inverse();
  MatrixXf F = C - D * invE *(D.transpose());
  MatrixXf tildeF = tildeC - tildeD * invE * (tildeD.transpose());

  // eigen decomposition to compute the square root of matrices
  // F = P_in * D_in * P_in^T
  // tildeF = P_out * D_out* P_out^T
  // the following matrices are actually real-valued
  SelfAdjointEigenSolver<MatrixXf> eigensolver_in(F);
  SelfAdjointEigenSolver<MatrixXf> eigensolver_out(tildeF);
  MatrixXcf D_in = eigensolver_in.eigenvalues().asDiagonal();
  MatrixXcf P_in = eigensolver_in.eigenvectors();
  MatrixXcf D_out = eigensolver_out.eigenvalues().asDiagonal();
  MatrixXcf P_out = eigensolver_out.eigenvectors();

  // square root and inverse square root
  // isD_in = D_in^{-1/2}
  // sD_out = D_out^{1/2}
  MatrixXcf isD_in = MatrixXcf::Zero(N_data1*nz,N_data1*nz);
  MatrixXcf sD_out = MatrixXcf::Zero(N_data1*nz,N_data1*nz);
  int test = 0; // to test if the matrix stays at 0 or not
  for(i = 0; i < N_data1*nz; i++) {
    if( fabs(D_in.real()(i,i)) > 1e-12 ) {
      isD_in(i,i) = ((complex<float>) 1)/sqrt((complex<float>) D_in(i,i));
      test = 1;
    }
    sD_out(i,i) = sqrt((complex<float>) D_out(i,i));
  }

  if ( test ) { // if the matrix has non-zero values
    // define new matrix Vnew = Lambda*V + Sigma*Y (Line 4 to Line 6)
    MatrixXcf Lambda = P_out*sD_out*(P_out.transpose())*P_in*isD_in*(P_in.transpose());
    MatrixXcf Sigma = (tildeD.cast<complex<float> >() - Lambda*D.cast<complex<float> >())
                  * invE.cast<complex<float> >();
    MatrixXcf Vnew = Lambda*V.cast<complex<float> >() + Sigma*W.cast<complex<float> >();

    // compute variance of the real and imaginary part
    // done outside compute_moment because of the data structure
    for(l = 0; l < nz; l++) {
      double mr = 0.0;
      double mi = 0.0;
      for(i = 0; i < N_data1; i++)
        for(j = 0; j < N; j++) {
          mr += Vnew.real()(i + l*N_data1,j);
          mi += Vnew.imag()(i + l*N_data1,j);
        }
      mr /= (N_data1*N);
      mi /= (N_data1*N);
      double vr = 0.0;
      double vi = 0.0;
      for(i = 0; i < N_data1; i++)
        for(j = 0; j < N; j++) {
          vr += (Vnew.real()(i + l*N_data1,j) - mr)
            * (Vnew.real()(i + l*N_data1,j) - mr);
          vi += (Vnew.imag()(i + l*N_data1,j) - mi)
            * (Vnew.imag()(i + l*N_data1,j) - mi);
        }

      // do not modify data1 if the variance of the imaginary part is too high
      if(vi/vr < tol) {
        for(i = 0; i < N_data1; i++)
          for(j = 0; j < N; j++)
            data1[i][j + l*N]= Vnew.real()(i + l*N_data1,j);
      }
    }
  }
}

// Adjust the cross-correlations of an image given a list of
// fixed sub-bands (Appendix A.3)
// This corresponds to Algorithm 10
// Linearly adjust variables in data1 to have the variance var
// and the cross-correlation cross_scale_cor
// The input data is assumed to have a zero mean
// Note that data2 is not modified
// The computations are similar to adjust_cross_scale_cor but the data
// has a different structure
void adjust_cross_scale_cor2(float *data1, float **data2,
                          float var, const float *cross_scale_cor,
                          int N_data, int N)
{
  int i, j, l;
  MatrixXf V(1, N);
  MatrixXf W(3*N_data, N);

  // put data in a matrix X
  for(j = 0; j < N; j++)
    V(0,j) = data1[j];

  // put data in a matrix Y
  for(i = 0; i < N_data; i++)
    for(j = 0; j < N; j++)
      for(l = 0; l < 3; l++)
        W(i + l*N_data,j) = data2[i][j + l*N];

  // store the target pairwise cross-correlation in a matrix
  MatrixXf tildeC(1, 1);
  tildeC(0,0) = var;

  // store the target cross-correlation in a matrix
  MatrixXf tildeD(1, 3*N_data);
  for(j = 0; j < 3*N_data; j++)
    tildeD(0,j) = cross_scale_cor[j];

  // compute the cross-correlations (Line 1)
  MatrixXf C = V*(V.transpose())/N;
  MatrixXf D = V*(W.transpose())/N;

  // matrix computation (Line 2 and Line 3)
  MatrixXf invE = (W*(W.transpose())/N).inverse();
  MatrixXf F = C - D * invE * (D.transpose());
  MatrixXf tildeF = tildeC - tildeD * invE * (tildeD.transpose());

  // square root and inverse square root
  // isD_in = F^{-1/2}
  // sD_out = tildeF^{1/2}
  MatrixXcf isD_in = MatrixXcf::Zero(1,1);
  MatrixXcf sD_out = MatrixXcf::Zero(1,1);
  if( fabs(F(0,0) > 1e-12) ) { // only modify if non-zero value
    isD_in(0,0)=((complex<float>) 1)/sqrt((complex<float>) F(0,0));
    sD_out(0,0)=sqrt((complex<float>) tildeF(0,0));

    // define new matrix Vnew = Lambda*V + Sigma*Y (Line 4 to Line 6)
    MatrixXcf Lambda = sD_out*isD_in;
    MatrixXcf Sigma = (tildeD.cast<complex<float> >() - Lambda*D.cast<complex<float> >())
                  * invE.cast<complex<float> >();
    MatrixXcf Vnew = Lambda*V.cast<complex<float> >() + Sigma*W.cast<complex<float> >();

    // compute variance of the real and imaginary part
    // done outside a function because of the data structure
    double mr = 0.0;
    double mi = 0.0;
    for(j=0; j<N; j++) {
      mr += Vnew.real()(0,j);
      mi += Vnew.imag()(0,j);
    }
    mr /= N;
    mi /= N;
    double vr = 0.0;
    double vi = 0.0;
    for(j=0; j<N; j++) {
      vr += (Vnew.real()(0,j)-mr)*(Vnew.real()(0,j)-mr);
      vi += (Vnew.imag()(0,j)-mi)*(Vnew.imag()(0,j)-mi);
    }

    // do not modify data1 if the variance of the imaginary part is too high
    if(vi/vr < 1e-3) {
      for(j=0; j<N; j++)
        data1[j]= Vnew.real()(0,j);
    }
  }
}


#include <float.h>
#include <math.h>
#include <sstream>
#include <boost/any.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include "misc/blas.h"
#include "mspass/seismic/keywords.h"
#include "mspass/seismic/CoreSeismogram.h"
#include "mspass/utility/MsPASSError.h"
#include "mspass/utility/SphericalCoordinate.h"

namespace mspass::seismic
{
using namespace std;
using namespace mspass::utility;
namespace py=pybind11;
/*
 *  Start with all the constructors.
 *
*/
//
// Default constructor for CoreSeismogram could be
// done inline in seispp.h, but it is complication enough I put
// it here
//
CoreSeismogram::CoreSeismogram() : BasicTimeSeries(),Metadata()
{
  /* mlive and tref are set in BasicTimeSeries so we don't use putters for
  them here.   These three initialize Metadata properly for these attributes*/
    this->set_dt(1.0);
    this->set_t0(0.0);
    this->set_npts(0);
    components_are_orthogonal=true;
    components_are_cardinal=true;
    for(int i=0; i<3; ++i)
        for(int j=0; j<3; ++j)
            if(i==j)
                tmatrix[i][i]=1.0;
            else
                tmatrix[i][j]=0.0;
}
CoreSeismogram::CoreSeismogram(const size_t nsamples)
    : BasicTimeSeries(),Metadata()
{
  /* IMPORTANT:  this constructor assumes BasicTimeSeries initializes the
  equivalent of:
  set_dt(1.0)
  set_t0(0.0)
  set_tref(TimeReferenceType::Relative)
  this->kill() - i.e. marked dead
  */
  this->set_npts(nsamples);  // Assume this is an allocator of the 3xnsamples matrix
  components_are_orthogonal=true;
  components_are_cardinal=true;
  for(int i=0; i<3; ++i)
    for(int j=0; j<3; ++j)
      if(i==j)
        tmatrix[i][i]=1.0;
      else
        tmatrix[i][j]=0.0;
}

CoreSeismogram::CoreSeismogram(const CoreSeismogram& t3c) :
    BasicTimeSeries(dynamic_cast<const BasicTimeSeries&>(t3c)),
    Metadata(dynamic_cast<const Metadata&>(t3c)),
    u(t3c.u)
{
    int i,j;
    components_are_orthogonal=t3c.components_are_orthogonal;
    components_are_cardinal=t3c.components_are_cardinal;
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j) tmatrix[i][j]=t3c.tmatrix[i][j];
}
bool CoreSeismogram::tmatrix_is_cardinal()
{
    /* Test for 0 or 1 to 5 figures - safe but conservative for
       float input although tmatrix is double */
    double scale(10000.0);
    int itest(10000);
    int i,j;
    for(i=0;i<3;++i)
    {
        for(j=0;j<3;++j)
        {
            int ival=static_cast<int>(tmatrix[i][j]*scale);
            if(i==j)
            {
                if(ival!=itest) return false;
            }
            else
            {
                if(ival!=0) return false;
            }
        }
    }
    return true;
}
CoreSeismogram::CoreSeismogram(const Metadata& md,
                const bool load_data) : Metadata(md)
{
    string dfile, dir;
    long foff;
    FILE *fp;
    double *inbuffer;

    components_are_orthogonal=true;
    mlive=false;
    try {
        /* Names used are from mspass defintions as of Jan 2020.
        We don't need to call the set methods for these attributes as they
        would add the overhead of setting delta, startime, and npts to the
        same value passed. */
        this->mdt = this->get_double(SEISMICMD_dt);
        this->mt0 = this->get_double(SEISMICMD_t0);
        this->nsamp = this->get_long(SEISMICMD_npts);
        /* Assume the data t0 is UTC. */
        this->set_tref(TimeReferenceType::UTC);
        /* This section is done specially to handle interaction with MongoDB.
        We store tmatrix there as a python object so we use a get_any to fetch
        it.   Oct 22, 2021 added a bug fix to handle tmatrix not defined.
        We will take a null (undefined) tmatrix stored in the database to
        imply the data are cardinal and orthogonal (i.e. stardard geographic
        coordinates in e,n,z order.)*/
        if(this->is_defined("tmatrix"))
        {
          this->set_transformation_matrix
                    (boost::any_cast<py::object>(this->get_any("tmatrix")));
          components_are_cardinal=this->tmatrix_is_cardinal();
          if(components_are_cardinal)
            components_are_orthogonal=true;
          else
            components_are_orthogonal=false;  //May be wrong but cost is tiny
        }
        else
        {
          /* this might not be needed but best be explicit*/
          for(auto i=0;i<3;++i)
            for(auto j=0;j<3;++j) this->tmatrix[i][j] = 0.0;
          for(auto i=0;i<3;++i) this->tmatrix[i][i] = 1.0;
          components_are_orthogonal = true;
          components_are_cardinal = true;
        }

        u=dmatrix(3,nsamp);
        if(load_data)
        {
            dir = this->get_string(SEISMICMD_dir);
            dfile = this->get_string(SEISMICMD_dfile);
            foff = this->get_long(SEISMICMD_foff);
            string fname=dir+"/"+dfile;
            if((fp=fopen(fname.c_str(),"r")) == NULL)
                throw(MsPASSError(string("Open failure for file ")+fname,
					             ErrorSeverity::Invalid));
            if (foff>0)fseek(fp,foff,SEEK_SET);
            /* The older seispp code allowed byte swapping here.   For
            efficiency we don't support that here and assume can do a
            raw fread from the file and get valid data.  If support for
            other types is needed this will need to be extended.  Here
            we just point fread at the internal u array. */
            inbuffer = this->u.get_address(0,0);
            unsigned int nt=3*this->nsamp;
            if(fread((void *)(inbuffer),sizeof(double),nt,fp)
                    != nt )
            {
                throw(MsPASSError(string("CoreSeismogram constructor:  fread error on file ")+fname,
                      ErrorSeverity::Invalid));
            }
            fclose(fp);
	          mlive=true;
	    }
        else
        {
          /* Initialize the matrix in this case but leave the object marked
          as dead */
          this->u.zero();
        }
    } catch (MsPASSError& mpe) {
        throw(mpe);
    } catch (boost::bad_any_cast& be) {
        throw(MsPASSError(string("CoreSeismogram constructor:  tmatrix type is not recognized"),
                ErrorSeverity::Invalid));
    } catch (...) {
      throw;
    };
}

CoreSeismogram::CoreSeismogram(const vector<CoreTimeSeries>& ts,
                       const unsigned int component_to_clone)
    : BasicTimeSeries(dynamic_cast<const BasicTimeSeries&>(ts[component_to_clone])),
     Metadata(dynamic_cast<const Metadata&>(ts[component_to_clone])),
      u()
{
    const string base_error("CoreSeismogram constructor from 3 Time Series:  ");
    int i,j;
    /* This is needed in case nsamp does not match s.size(0) */
    int nstest = ts[component_to_clone].s.size();
    if(nsamp!=nstest) this->nsamp=nstest;
    /* this method allocates u and sets the proper metadata for npts*/
    this->CoreSeismogram::set_npts(this->nsamp);
    /* beware irregular sample rates, but don' be too machevelian.
           Abort only if the mismatch is large defined as accumulated time
           over data range of this constructor is less than half a sample */
    if( (ts[0].dt()!=ts[1].dt()) || (ts[1].dt()!=ts[2].dt()) )
    {
        double ddtmag1=fabs(ts[0].dt()-ts[1].dt());
        double ddtmag2=fabs(ts[1].dt()-ts[2].dt());
        double ddtmag;
        if(ddtmag1>ddtmag1)
            ddtmag=ddtmag1;
        else
            ddtmag=ddtmag2;
        ddtmag1=fabs(ts[0].dt()-ts[2].dt());
        if(ddtmag1>ddtmag)  ddtmag=ddtmag1;
        double ddtcum=ddtmag*((double)ts[0].s.size());
        if(ddtcum>(ts[0].dt())/2.0)
        {
            stringstream ss;
            ss << base_error
               << "Sample intervals of components are not consistent"<<endl;
            for(int ie=0; ie<3; ++ie) ss << "Component "<<ie<<" dt="<<ts[ie].dt()<<" ";
            ss<<endl;
            throw MsPASSError(ss.str(),ErrorSeverity::Invalid);
        }
    }
    // temporaries to hold component values
    double t0_component[3];
    double hang[3];
    double vang[3];
    // Load up these temporary arrays inside this try block and arrange to
    // throw an exception if required metadata are missing
    try {
        /* WARNING hang and vang attributes in Metadata
        are always assumed to have been read from a database where they
        were stored in degrees.  We convert these to radians below to
        compute the transformation matrix.

        Feb 2021:  converted to use keywords.h definitions assuming these
        came from the channel collection in MongoDB - Can be changed in
        kewords.h for application outside mspass */
        hang[0]=ts[0].get_double(SEISMICMD_hang);
        hang[1]=ts[1].get_double(SEISMICMD_hang);
        hang[2]=ts[2].get_double(SEISMICMD_hang);
        vang[0]=ts[0].get_double(SEISMICMD_vang);
        vang[1]=ts[1].get_double(SEISMICMD_vang);
        vang[2]=ts[2].get_double(SEISMICMD_vang);
    } catch (MetadataGetError& mde)
    {
        stringstream ss;
        ss << base_error
           << "missing hang or vang variable in component TimeSeries objects received"<<endl;
        ss << "Message posted by Metadata::get_double:  "<<mde.what()<<endl;
	throw MsPASSError(ss.str(),ErrorSeverity::Invalid);
    }
    /* We couldn't get here if hang and vang were not set on comp 0 so
       we don't test for that condition.  We do need to clear hang and vang
       from result here, however, as both attributes are meaningless
       for a 3C seismogram */
    this->erase(SEISMICMD_hang);
    this->erase(SEISMICMD_vang);
    // These are loaded just for convenience
    t0_component[0]=ts[0].t0();
    t0_component[1]=ts[1].t0();
    t0_component[2]=ts[2].t0();

    // Treat the normal case specially and avoid a bunch of work unless
    // it is required
    if( (ts[0].s.size()==ts[1].s.size()) && (ts[1].s.size()==ts[2].s.size())
            && (fabs( (t0_component[0]-t0_component[1])/dt() )<1.0)
            && (fabs( (t0_component[1]-t0_component[2])/dt() )<1.0))
    {
        /* Older code had this.   No longer needed with logic above that
        calls set_npts.  that method creates and initialized the u dmatrix*/
        //this->u=dmatrix(3,nsamp);
        // Load data by a simple copy operation
        /* This is a simple loop version
        for(j=0;j<nsamp;++nsamp)
        {
        	this->u(0,j)=ts[0].s[j];
        	this->u(1,j)=ts[1].s[j];
        	this->u(2,j)=ts[2].s[j];
        }
        */
        // This is a vector version that I'll use because it will
        // be faster albeit infinitely more obscure and
        // intrinsically more dangerous
        dcopy(nsamp,&(ts[0].s[0]),1,u.get_address(0,0),3);
        dcopy(nsamp,&(ts[1].s[0]),1,u.get_address(1,0),3);
        dcopy(nsamp,&(ts[2].s[0]),1,u.get_address(2,0),3);
    }
    else
    {
        /*Land here if the start time or number of samples
        is irregular.  We cut the output to latest start time to earliest end time*/
        /* WARNING - debugging may be needed for this block. SEISPP versio of this
        used gaps.  Here we cut the output to match an irregularities. */
        double tsmax,temin;
        tsmax=max(t0_component[0],t0_component[1]);
        tsmax=max(tsmax,t0_component[2]);
        temin=min(ts[0].endtime(),ts[1].endtime());
        temin=min(temin,ts[2].endtime());
        nstest=(int)round((temin-tsmax)/mdt);
        if(nstest<=0)
          throw MsPASSError(base_error
                +"Irregular time windows of components have no overlap",
                        ErrorSeverity::Invalid);
        else
          this->CoreSeismogram::set_npts(nstest);
        // Now load the data.  Use the time and sample number methods
        // to simplify process
        double t;
        t=tsmax;
        this->set_t0(t);
        double delta=this->dt();
        for(int ic=0; ic<3; ++ic)
        {
            t=this->t0();
            for(j=0; j<ts[ic].s.size(); ++j)
            {
                i=ts[ic].sample_number(t);
                // silently do nothing if outside bounds.  This
                // perhaps should be an error as it shouldn't really
                // happen with the above algorithm, but safety is good
                if( (i>=0) && (i<nsamp) ) this->u(ic,j)=ts[ic].s[i];
                t += delta;
            }
        }
    }
    /* Finally we need to set the transformation matrix.
     This is a direct application of conversion of routines
    in spherical coordinate procedures.  They are procedural
    routines, not objects so the code is procedural.
    */
    SphericalCoordinate scor;
    double *nu;
    // convert all the hang values to spherical coordinate phi
    // (angle from postive east) from input assumed in degrees
    // azimuth from north.  At the same time convert vang to radians.
    for(i=0; i<3; ++i)
    {
        hang[i]=mspass::utility::rad(90.0-hang[i]);
        vang[i]=mspass::utility::rad(vang[i]);
    }
    for(i=0; i<3; ++i)
    {
        scor.phi=hang[i];
        scor.theta=vang[i];
        nu=SphericalToUnitVector(scor);
        for(j=0; j<3; ++j)tmatrix[i][j]=nu[j];
        delete [] nu;
    }
    components_are_cardinal = this->tmatrix_is_cardinal();
    if(components_are_cardinal)
        components_are_orthogonal=true;
    else
        components_are_orthogonal=false;
}
// Note on usage in this group of functions.  The rotation algorithms used here
// all key on the BLAS for speed.  That is, a transformation matrix could be done
// by using the * operator between matrix objects.

void CoreSeismogram::rotate_to_standard()
{
    if( (u.size()[1]<=0) || this->dead()) return; // do nothing in these situations
    double *work[3];
    int i,j;
    if(components_are_cardinal) return;
    /* We assume nsamp is the number of samples = number of columns in u - we don't
    check here for efficiency */
    for(j=0; j<3; ++j) work[j]=new double[nsamp];
    if(components_are_orthogonal)
    {
        //
        //Use a daxpy algorithm.  tmatrix stores the
        //forward transformation used to get current
        //Use the transpose to get back
        //
        for(i=0; i<3; ++i)
        {
            // x has a stride of 3 because we store in fortran order in x
            dcopy(nsamp,u.get_address(0,0),3,work[i],1);
            dscal(nsamp,tmatrix[0][i],work[i],1);
            daxpy(nsamp,tmatrix[1][i],u.get_address(1,0),3,work[i],1);
            daxpy(nsamp,tmatrix[2][i],u.get_address(2,0),3,work[i],1);
        }
        for(i=0; i<3; ++i) dcopy(nsamp,work[i],1,u.get_address(i,0),3);
    }
    else
    {
        //
        //Enter here only when the transformation matrix is
        //not orthogonal.  We have to construct a fortran
        //order matrix a to use LINPACK routine in sunperf/perf
        //This could be done with the matrix template library
        //but the overhead ain't worth it
        //
        double a[9];
        int ipivot[3];
        int info;
        a[0] = tmatrix[0][0];
        a[1] = tmatrix[1][0];
        a[2] = tmatrix[2][0];
        a[3] = tmatrix[0][1];
        a[4] = tmatrix[1][1];
        a[5] = tmatrix[2][1];
        a[6] = tmatrix[0][2];
        a[7] = tmatrix[1][2];
        a[8] = tmatrix[2][2];
        //LAPACK routine with FORTRAN interface using pass by reference and pointers
        int three(3);
        dgetrf(three,three,a,three,ipivot,info);
        if(info!=0)
        {
            for(i=0; i<3; ++i) delete [] work[i];
            throw(MsPASSError(
                      string("rotate_to_standard:  LU factorization of transformation matrix failed")),
                  ErrorSeverity::Invalid);
        }
        // inversion routine after factorization from lapack FORT$RAN interface
        double awork[10];  //Larger than required but safety value small cost
        int ldwork(10);
        dgetri(three,a,three,ipivot,awork,ldwork,info);
        // This is the openblas version
        //info=LAPACKE_dgetri(LAPACK_COL_MAJOR,3,a,3,ipivot);
        if(info!=0)
        {
            for(i=0; i<3; ++i) delete [] work[i];
            throw(MsPASSError(
                      string("rotate_to_standard:  LU factorization inversion of transformation matrix failed")),
                  ErrorSeverity::Invalid);
        }

        tmatrix[0][0] = a[0];
        tmatrix[1][0] = a[1];
        tmatrix[2][0] = a[2];
        tmatrix[0][1] = a[3];
        tmatrix[1][1] = a[4];
        tmatrix[2][1] = a[5];
        tmatrix[0][2] = a[6];
        tmatrix[1][2] = a[7];
        tmatrix[2][2] = a[8];
        /* The inverse is now in tmatrix so we reverse the
           rows and columms from above loop */

        for(i=0; i<3; ++i)
        {
            dcopy(nsamp,u.get_address(0,0),3,work[i],1);
            dscal(nsamp,tmatrix[i][0],work[i],1);
            daxpy(nsamp,tmatrix[i][1],u.get_address(1,0),3,work[i],1);
            daxpy(nsamp,tmatrix[i][2],u.get_address(2,0),3,work[i],1);
        }
        for(i=0; i<3; ++i) dcopy(nsamp,work[i],1,u.get_address(i,0),3);
        components_are_orthogonal = true;
    }
    //
    //Have to set the transformation matrix to an identity now
    //
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j)
            if(i==j)
                tmatrix[i][i]=1.0;
            else
                tmatrix[i][j]=0.0;

    components_are_cardinal=true;
    for(i=0; i<3; ++i) delete [] work[i];
}

/* This routine takes a spherical coordinate vector that defines
a given direction in space and returns a transformation matrix that
should be viewed as a transformation to ray coordinates under an
assumption that this vector points in the direction of P wave
particle motion.  If the theta angle is greater than PI/2 it
switches the azimuth by 180 degrees so that the direction of the
transformed x1 axis will be pointing upward in space.  This removes
ambiguities in the transformation that make it easier to sort out
handedness of the transformation.

The transformation produced for a P wave will be true ray coordinates
with X1 = transverse, X2 = radial, and X3 = longitudinal.
The best way to understand the transformation is as a pair of
rotations:  (1) rotate North to radial about z, (2) rotate z to
transverse around X1 (transverse).  Note this leaves X1 (transverse)
always as a purely horizontal direction.  It should also work for a
principal component direction determined for an S phase, but the
appropriate the only component that will make any sense after the
transformation, in that case, is the X3 direction = direction of
inferred peak particle motion.

One special case has to be dealt with.  If the direction passed into
the program is purely vertical (up or down), the function can only
return an identity matrix because there is no way to determine a
horizontal rotation direction.

Arguments:
	xsc - spherical coordinate structure defining unit vector used
		to define the transform (radius is ignored).  Angles
		are assumed in radians.

Author:  Gary L. Pavlis
Written:  Sept. 1999
Modified:  Feb 2003
Original was plain C.  Adapted to C++ for seismic processing
*/
void CoreSeismogram::rotate(SphericalCoordinate& xsc)
{
    if( (u.size()[1]<=0) || dead()) return; // do nothing in these situations

    //Earlier version had a reset of the nsamp variable here - we need to trust
    //that is correct here for efficiency.  We the new API it would be hard
    //to have that happen. without a serious blunder
    int i;
    double theta, phi;  /* corrected angles after dealing with signs */
    double a,b,c,d;

    //
    //Undo any previous transformations
    //
    this->rotate_to_standard();
    if(xsc.theta == M_PI)
    {
        //This will be left handed
        tmatrix[2][2] = -1.0;
        dscal(nsamp,-1.0,u.get_address(2,0),3);
        return;
    }

    if(xsc.theta < 0.0)
    {
        theta = -(xsc.theta);
        phi = xsc.phi + M_PI;
        if(phi > M_PI) phi -= (2.0*M_PI);
    }
    else if(xsc.theta > M_PI)
    {
        theta = xsc.theta - M_PI;
        phi = xsc.phi + M_PI;
        if(phi > M_PI) phi -= (2.0*M_PI);
    }
    else
    {
        theta = xsc.theta;
        phi = xsc.phi;
    }
    /* Am using a formula here for azimuth with is pi/2 - phi*/
    double azimuth=M_PI_2-phi;
    a = cos(azimuth);
    b = sin(azimuth);
    c = cos(theta);
    d = sin(theta);

    tmatrix[0][0] = a;
    tmatrix[1][0] = b*c;
    tmatrix[2][0] = b*d;
    tmatrix[0][1] = -b;
    tmatrix[1][1] = a*c;
    tmatrix[2][1] = a*d;
    tmatrix[0][2] = 0.0;
    tmatrix[1][2] = -d;
    tmatrix[2][2] = c;

    /* Now multiply the data by this transformation matrix.  */
    double *work[3];
    for(i=0; i<3; ++i)work[i] = new double[nsamp];
    for(i=0; i<3; ++i)
    {
        dcopy(nsamp,u.get_address(0,0),3,work[i],1);
        dscal(nsamp,tmatrix[i][0],work[i],1);
        daxpy(nsamp,tmatrix[i][1],u.get_address(1,0),3,work[i],1);
        daxpy(nsamp,tmatrix[i][2],u.get_address(2,0),3,work[i],1);
    }
    for(i=0; i<3; ++i) dcopy(nsamp,work[i],1,u.get_address(i,0),3);
    components_are_cardinal=false;
    for(i=0; i<3; ++i) delete [] work[i];
}
void CoreSeismogram::rotate(const double nu[3])
{
    if( (u.size()[1]<=0) || this->dead()) return; // do nothing in these situations
    SphericalCoordinate xsc=UnitVectorToSpherical(nu);
    this->rotate(xsc);
}
/* simplified procedure to rotate only zonal angle by phi radians.
 Similar to above but using only azimuth angle AND doing a simple
 rotation in the horizontal plane.  Efficient algorithm only
 alters 0 and 1 components. */
void CoreSeismogram::rotate(double phi)
{
    if( (u.size()[1]<=0) || dead()) return; // do nothing in these situations
    int i,j,k;
    double a,b;
    a=cos(phi);
    b=sin(phi);
    double tmnew[3][3];
    tmnew[0][0] = a;
    tmnew[1][0] = b;
    tmnew[2][0] = 0.0;
    tmnew[0][1] = -b;
    tmnew[1][1] = a;
    tmnew[2][1] = 0.0;
    tmnew[0][2] = 0.0;
    tmnew[1][2] = 0.0;
    tmnew[2][2] = 1.0;

    /* Now multiply the data by this transformation matrix.
     Not trick in this i only goes to 2 because 3 component
     is an identity.*/
    double *work[2];
    for(i=0; i<2; ++i)work[i] = new double[nsamp];
    for(i=0; i<2; ++i)
    {
        dcopy(nsamp,u.get_address(0,0),3,work[i],1);
        dscal(nsamp,tmnew[i][0],work[i],1);
        daxpy(nsamp,tmnew[i][1],u.get_address(1,0),3,work[i],1);
    }
    for(i=0; i<2; ++i) dcopy(nsamp,work[i],1,u.get_address(i,0),3);
    double tm_tmp[3][3];
    double prod;
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j)
        {
            for(prod=0.0,k=0; k<3; ++k)
                prod+=tmnew[i][k]*tmatrix[k][j];
            tm_tmp[i][j]=prod;
        }
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j)tmatrix[i][j]=tm_tmp[i][j];
    components_are_cardinal=false;
    for(i=0; i<2; ++i) delete [] work[i];
}
void CoreSeismogram::transform(const double a[3][3])
{
    if( (u.size()[1]<=0) || dead()) return; // do nothing in these situations
    /* Older version had this - we need to trust ns is already u.columns().  */
    //size_t ns = u.size()[1];
    size_t i,j,k;
    double *work[3];
    for(i=0; i<3; ++i) work[i] = new double[nsamp];
    for(i=0; i<3; ++i)
    {
        dcopy(nsamp,u.get_address(0,0),3,work[i],1);
        dscal(nsamp,a[i][0],work[i],1);
        daxpy(nsamp,a[i][1],u.get_address(1,0),3,work[i],1);
        daxpy(nsamp,a[i][2],u.get_address(2,0),3,work[i],1);
    }
    for(i=0; i<3; ++i) dcopy(nsamp,work[i],1,u.get_address(i,0),3);
    for(i=0; i<3; ++i) delete [] work[i];
    /* Hand code this rather than use dmatrix or other library.
       Probably dumb, but this is just a 3x3 system.  This
       is simply a multiply of a*tmatrix with result replacing
       the internal tmatrix */
    double tmnew[3][3];
    double prod;
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j)
        {
            for(prod=0.0,k=0; k<3; ++k)
                prod+=a[i][k]*tmatrix[k][j];
            tmnew[i][j]=prod;
        }
    for(i=0; i<3; ++i)
        for(j=0; j<3; ++j)tmatrix[i][j]=tmnew[i][j];
    components_are_cardinal = this->tmatrix_is_cardinal();
    /* Assume this method does not yield cartesian coordinate directions.*/
    if(!components_are_cardinal) components_are_orthogonal=false;
}
/* This function computes and applies the free surface tranformaton
matrix described by Kennett 1991.  The result is a ray coordinate
transformation with x1=transverse, x2=radial, and x3=longitudinal.
Note this transformation is into a nonorthogonal system.

Algorithm first applies a rotation of horizontal coordinates to
horizonal radial and transverse, then applies free surface
transformation to the radial-vertical plane.

The free surface transformation code segment is a direct
translation of m file from Michael Bostock.

Author:  Gary Pavlis
*/
void CoreSeismogram::free_surface_transformation(SlownessVector uvec,
        double a0, double b0)
{
    if( (u.size()[1]<=0) || dead()) return; // do nothing in these situations
    double a02,b02,pslow,p2;
    double qa,qb,vpz,vpr,vsr,vsz;
    pslow=uvec.mag();
    // silently do nothing if magnitude of the slowness vector is 0
    // (vertical incidence)
    if(pslow<DBL_EPSILON) return;
    // Can't handle evanescent waves with this operator
    double vapparent=1.0/pslow;
    if(vapparent<a0 || vapparent<b0)
    {
        stringstream ss;
        ss<<"CoreSeismogram::free_surface_transformation method:  illegal input"<<endl
          << "Apparent velocity defined by input slowness vector="<<vapparent<<endl
          << "Smaller than specified surface P velocity="<<a0<<" or S velocity="<<b0<<endl
          << "That implies evanescent waves that violate the assumption of this operator"<<endl;
        throw MsPASSError(ss.str(),ErrorSeverity::Invalid);
    }

    // First the horizonal rotation
    SphericalCoordinate scor;
    //rotation angle is - azimuth to put x2 (north in standard coord)
    //in radial direction
    scor.phi=atan2(uvec.uy,uvec.ux);
    scor.theta=0.0;
    scor.radius=1.0;
    // after this transformation x1=transverse horizontal
    // x2=radial horizonal, and x3 is still vertical
    this->rotate(scor);

    a02=a0*a0;
    b02=b0*b0;
    p2=pslow*pslow;
    qa=sqrt((1.0/a02)-p2);
    qb=sqrt((1.0/b02)-p2);
    vpz=-(1.0-2.0*b02*p2)/(2.0*a0*qa);
    vpr=pslow*b02/a0;
    vsr=(1.0-2.0*b02*p2)/(2.0*b0*qb);
    vsz=pslow*b0;
    /* Now construct the transformation matrix
     This is different from Bostock's original code
     in sign and order.  Also note this transformation
         is not scaled to have a unit matrix norm so amplitudes
         after the transformation are distorted.  rotate_to_standard,
         however, should still restore original data within roundoff
         error if called on the result. */
    double fstran[3][3];
    fstran[0][0]=0.5;
    fstran[0][1]=0.0;
    fstran[0][2]=0.0;
    fstran[1][0]=0.0;
    fstran[1][1]=vsr;
    fstran[1][2]=vpr;
    fstran[2][0]=0.0;
    fstran[2][1]=-vsz;
    fstran[2][2]=-vpz;
    this->transform(fstran);

    components_are_cardinal=false;
    components_are_orthogonal=false;
}
bool CoreSeismogram::set_transformation_matrix(const dmatrix& A)
{
    for(int i=0;i<3;++i)
        for(int j=0;j<3;++j) tmatrix[i][j]=A(i,j);
    py::list tmatrix_l;
    for(int i=0;i<3;++i)
        for(int j=0;j<3;++j) tmatrix_l.append(A(i, j));
    this->put_object(SEISMICMD_tmatrix, tmatrix_l);
    bool cardinal;
    cardinal=this->tmatrix_is_cardinal();
    if(cardinal)
    {
        components_are_cardinal=true;
        components_are_orthogonal=true;
    }
    else
    {
        components_are_cardinal=false;
        /* Not necessarily true, but small overhead cost*/
        components_are_orthogonal=false;
    }
    return components_are_cardinal;
}
bool CoreSeismogram::set_transformation_matrix(const double a[3][3])
{
    for(int i=0;i<3;++i)
        for(int j=0;j<3;++j) tmatrix[i][j]=a[i][j];
    py::list tmatrix_l;
    for(int i=0;i<3;++i)
        for(int j=0;j<3;++j) tmatrix_l.append(a[i][j]);
    this->put_object(SEISMICMD_tmatrix, tmatrix_l);
    bool cardinal;
    cardinal=this->tmatrix_is_cardinal();
    if(cardinal)
    {
        components_are_cardinal=true;
        components_are_orthogonal=true;
    }
    else
    {
        components_are_cardinal=false;
        /* Not necessarily true, but small overhead cost*/
        components_are_orthogonal=false;
    }
    return components_are_cardinal;
}
bool CoreSeismogram::set_transformation_matrix(py::object tmatrix_py)
{
    if(py::isinstance<py::array>(tmatrix_py)) {
        auto tmatrix_ary = tmatrix_py.cast<py::array_t<double, py::array::c_style | py::array::forcecast>>();
        py::buffer_info info = tmatrix_ary.request();
        if ((info.ndim == 2 && info.shape[0]*info.shape[1] == 9) ||
            (info.ndim == 1 && info.shape[0] == 9)
           )
            return this->set_transformation_matrix(static_cast<double(*)[3]>(info.ptr));
        else
            throw(MsPASSError(string("set_transformation_matrix: tmatrix should be a 3x3 matrix"),
                    ErrorSeverity::Invalid));
    } else if (py::isinstance<dmatrix>(tmatrix_py)) {
        auto tmatrix_ary = tmatrix_py.cast<dmatrix>();
        if (tmatrix_ary.rows() != 3 || tmatrix_ary.columns() != 3)
            throw(MsPASSError(string("set_transformation_matrix: tmatrix should be a 3x3 matrix"),
                    ErrorSeverity::Invalid));
        return this->set_transformation_matrix(tmatrix_ary);
    } else if (py::isinstance<py::list>(tmatrix_py)) {
        dmatrix tmatrix_ary(3,3);
        double* ptr = tmatrix_ary.get_address(0,0);
        if(py::len(tmatrix_py) == 9) {
            int i = 0;
            for (auto item : tmatrix_py) {
                try{
                    *(ptr+i) = item.cast<double>();
                    i++;
                } catch (...) {
                    throw(MsPASSError(string("set_transformation_matrix: the elements of tmatrix should be float"),
                            ErrorSeverity::Invalid));
                }
            }
        } else if(py::len(tmatrix_py) == 3) {
            int i = 0;
            for (auto items : tmatrix_py) {
                if(!py::isinstance<py::list>(items))
                    throw(MsPASSError(string("set_transformation_matrix: tmatrix should be a 3x3 list of list"),
                            ErrorSeverity::Invalid));
                else if (py::len(items) != 3)
                    throw(MsPASSError(string("set_transformation_matrix: tmatrix should be a 3x3 list of list"),
                            ErrorSeverity::Invalid));
                else {
                    for (auto item : items) {
                        try{
                            *(ptr+i) = item.cast<double>();
                            i++;
                        } catch (...) {
                            throw(MsPASSError(string("set_transformation_matrix: the elements of tmatrix should be float"),
                                ErrorSeverity::Invalid));
                        }
                    }
                }
            }
        } else {
            throw(MsPASSError(string("set_transformation_matrix: tmatrix should be a list of 9 floats or a 3x3 list of list"),
                ErrorSeverity::Invalid));
        }
        return this->set_transformation_matrix(tr(tmatrix_ary));
    } else {
        throw(MsPASSError(string("set_transformation_matrix: tmatrix's type is not recognized"),
                ErrorSeverity::Invalid));
    }
}
CoreSeismogram& CoreSeismogram::operator=(const CoreSeismogram& seisin)
{
    if(this!=&seisin)
    {
        this->BasicTimeSeries::operator=(seisin);
        this->Metadata::operator=(seisin);
        components_are_orthogonal=seisin.components_are_orthogonal;
        components_are_cardinal=seisin.components_are_cardinal;
        for(int i=0; i<3; ++i)
        {
            for(int j=0; j<3; ++j)
            {
                tmatrix[i][j]=seisin.tmatrix[i][j];
            }
        }
        u=seisin.u;
    }
    return(*this);
}
CoreSeismogram& CoreSeismogram::operator*=(const double scale)
{
  /* do nothing to empty data or data marked dead*/
  if((this->npts()==0) || (this->dead())) return(*this);
  /* We can use this dscal blas function because dmatrix puts all the data
  in a continguous block. Beware if there is am implementation change for
  the matrix data*/
  double *ptr=this->u.get_address(0,0);
  dscal(3*this->npts(),scale,ptr,1);
  return(*this);
}
CoreSeismogram& CoreSeismogram::operator+=(const CoreSeismogram& data)
{
    int i,iend,jend;
    size_t j,i0,j0;
    // Sun's compiler complains about const objects without this.
    CoreSeismogram& d=const_cast<CoreSeismogram&>(data);
    // Silently do nothing if d is marked dead
    if(!d.mlive) return(*this);
    // Silently do nothing if d does not overlap with data to contain sum
    if( (d.endtime()<mt0)
            || (d.mt0>(this->endtime())) ) return(*this);
    if(d.tref!=(this->tref))
        throw MsPASSError("CoreSeismogram += operator cannot handle data with inconsistent time base\n",
                          ErrorSeverity::Invalid);
    /* this defines the range of left and right hand sides to be summed */
    i=d.sample_number(this->mt0);
    if(i<0)
    {
      j0=this->sample_number(d.t0());
      i0=0;
    }
    else
    {
      j0=0;
      i0=i;
    }
    iend=d.sample_number(this->endtime());
    jend=this->sample_number(d.endtime());
    if(iend>=(d.npts()))
    {
        iend=d.npts()-1;
    }
    if(jend>=this->npts())
    {
      jend=this->npts()-1;
    }
    for(i=i0,j=j0; i<=iend && j<=jend; ++i,++j)
    {
        this->u(0,j)+=d.u(0,i);
        this->u(1,j)+=d.u(1,i);
        this->u(2,j)+=d.u(2,i);
    }
    return(*this);
}
/* IMPORTANT:  this code is absolutely identical to that for operator+=
except the += in the last loop becomes -=.  Any changes in operator+=
must have exactly the same change here (other than a message with a
tag to the function)*/
CoreSeismogram& CoreSeismogram::operator-=(const CoreSeismogram& data)
{
    int i,iend,jend;
    size_t j,i0,j0;
    // Sun's compiler complains about const objects without this.
    CoreSeismogram& d=const_cast<CoreSeismogram&>(data);
    // Silently do nothing if d is marked dead
    if(!d.mlive) return(*this);
    // Silently do nothing if d does not overlap with data to contain sum
    if( (d.endtime()<mt0)
            || (d.mt0>(this->endtime())) ) return(*this);
    if(d.tref!=(this->tref))
        throw MsPASSError("CoreSeismogram += operator cannot handle data with inconsistent time base\n",
                          ErrorSeverity::Invalid);
    /* this defines the range of left and right hand sides to be summed */
    i=d.sample_number(this->mt0);
    if(i<0)
    {
      j0=this->sample_number(d.t0());
      i0=0;
    }
    else
    {
      j0=0;
      i0=i;
    }
    iend=d.sample_number(this->endtime());
    jend=this->sample_number(d.endtime());
    if(iend>=(d.npts()))
    {
        iend=d.npts()-1;
    }
    if(jend>=this->npts())
    {
      jend=this->npts()-1;
    }
    for(i=i0,j=j0; i<=iend && j<=jend; ++i,++j)
    {
        this->u(0,j)-=d.u(0,i);
        this->u(1,j)-=d.u(1,i);
        this->u(2,j)-=d.u(2,i);
    }
    return(*this);
}
const CoreSeismogram CoreSeismogram::operator+(const CoreSeismogram& other) const
{
  CoreSeismogram result(*this);
  result += other;
  return result;
}
const CoreSeismogram CoreSeismogram::operator-(const CoreSeismogram& other) const
{
  CoreSeismogram result(*this);
  result -= other;
  return result;
}
void CoreSeismogram::set_dt(const double sample_interval)
{
  this->BasicTimeSeries::set_dt(sample_interval);
  /* This is the unique name defined in the mspass schema - we always set it. */
  this->put(SEISMICMD_dt,sample_interval);
  /* these are hard coded aliases for sample_interval */
  std::set<string> aliases;
  std::set<string>::iterator aptr;
  /* Note these aren't set in keywords - aliases are flexible and this
  can allow another way to make these attribute names more flexible. */
  aliases.insert("dt");
  for(aptr=aliases.begin();aptr!=aliases.end();++aptr)
  {
    if(this->is_defined(*aptr))
    {
      this->put(*aptr,sample_interval);
    }
  }
}
void CoreSeismogram::set_t0(const double t0in)
{
  this->BasicTimeSeries::set_t0(t0in);
  /* This is the unique name - we always set it.  Pulled from keywords.h
  which should match the schema.  aliases are hard coded not defined as
  keywords */
  this->put(SEISMICMD_t0,t0in);
  /* these are hard coded aliases for sample_interval */
  std::set<string> aliases;
  std::set<string>::iterator aptr;
  aliases.insert("t0");
  aliases.insert("time");
  for(aptr=aliases.begin();aptr!=aliases.end();++aptr)
  {
    if(this->is_defined(*aptr))
    {
      this->put(*aptr,t0in);
    }
  }
}
void CoreSeismogram::set_npts(const size_t npts)
{
  this->BasicTimeSeries::set_npts(npts);
  /* This is the unique name - we always set it.  The weird
  cast is necessary to avoid type mismatch with unsigned.
  We use the name defined in keywords.h which we can always assume
  matches the schema for the unique name*/
  this->put(SEISMICMD_npts,(long int)npts);
  /* these are hard coded aliases for sample_interval */
  std::set<string> aliases;
  std::set<string>::iterator aptr;
  aliases.insert("nsamp");
  aliases.insert("wfdisc.nsamp");
  for(aptr=aliases.begin();aptr!=aliases.end();++aptr)
  {
    if(this->is_defined(*aptr))
    {
      this->put(*aptr,(long int)npts);
    }
  }
  /* this method has the further complication that npts sets the size of the
  data matrix.   Here we resize the matrix and initialize it to 0s.*/
  this->u=dmatrix(3,npts);
  this->u.zero();
}
void CoreSeismogram::sync_npts()
{
  if(nsamp != this->u.columns()) {
    this->BasicTimeSeries::set_npts(this->u.columns());
    /* This is the unique name - we always set it.  The weird
    cast is necessary to avoid type mismatch with unsigned.
    As above converted to keywords.h const string to make
    this easier to maintain*/
    this->put(SEISMICMD_npts,(long int)nsamp);
    /* these are hard coded aliases for sample_interval */
    std::set<string> aliases;
    std::set<string>::iterator aptr;
    aliases.insert("nsamp");
    aliases.insert("wfdisc.nsamp");
    for(aptr=aliases.begin();aptr!=aliases.end();++aptr)
    {
        if(this->is_defined(*aptr))
        {
          this->put(*aptr,(long int)nsamp);
        }
    }
  }
}

vector<double> CoreSeismogram::operator[] (const int i)const
{
    try {
        vector<double> result;
        result.reserve(3);
        for(int k=0;k<3;++k)
          result.push_back(this->u(k,i));
        return result;
    }catch(...){throw;};
}
vector<double> CoreSeismogram::operator[] (const double t) const
{
    try {
        vector<double> result;
        int i=this->sample_number(t);
        /* could test for a negative i and i too large but we assume
        the dmatrix container will throw an exception if it resolves that way*/
        for(int k=0;k<3;++k)
          result.push_back(this->u(k,i));
        return result;
    }catch(...){throw;};
}
} // end namespace SEISPP


// Copyright 2018 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

/// \copyright Copyright 2018 the Autoware Foundation
/// \file
/// \brief Header for hungarian algorithm for optimal linear assignment
#ifndef HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_
#define HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_

/// \brief Ensure Eigen does not allocate memory dynamically
#define EIGEN_NO_MALLOC
/// \brief Prevent Eigen from yelling at you for larged fix-sized matrices
#define EIGEN_STACK_ALLOCATION_LIMIT 0

#include <Eigen/Core>
#include <hungarian_assigner/visibility_control.hpp>
#include <utility>
#include <limits>
#include <array>
#include "common/types.hpp"

using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;

namespace autoware
{
namespace fusion
{
/// \brief this namespace is for all functions, structs, classes and constants in the
///        hungarian_assigner package
namespace hungarian_assigner
{

/// \brief indexing matches what matrices use
using index_t = Eigen::Index;

/// \brief implementation of the hungarian/kuhn-munkres/jacobi algorithm for
/// minimum weight assignment problem in O(N^3 ) time
/// \tparam Capacity maximum number of things that can be matched, sets matrix size
template<uint16_t Capacity>
class HUNGARIAN_ASSIGNER_PUBLIC hungarian_assigner_c
{
  static_assert(Capacity > 0, "Capacity must be positive");
  /// \brief markers for each matrix slot, not strongly typed so it can decay to numeric type
  enum mark_e : int8_t
  {
    NO_LINK = 0,   ///< no cost has been assigned, so assignment is assumed to be impossible
    UNMARKED = 1,  ///< this location is just a normal weight
    ZERO = 2,      ///< zero, but unmarked
    PRIMED = 3,    ///< primed zero
    STARRED = 4    ///< starred zero, corresponds to an assignment
  };

  /// \brief this definition is for internal book-keeping
  using index2_t = std::pair<index_t, index_t>;

  /// \brief This exception is for a bad edge case when the assigner tries to add a new zero
  ///        but the only uncovered entries in the matrix are not valid links/weights
  class HUNGARIAN_ASSIGNER_LOCAL no_uncovered_values_c : public std::runtime_error
  {
public:
    no_uncovered_values_c();
  };  // class no_uncovered_values_c

public:
  /// \brief This index denotes a worker for which no job assignment was possible
  static constexpr index_t UNASSIGNED = std::numeric_limits<index_t>::max();

  /// \brief constructor
  hungarian_assigner_c();

  /// \brief constructor, equivalent of construct(); set_size(num_rows, num_cols)
  /// \param[in] num_rows number of rows/jobs
  /// \param[in] num_cols number of columns/workers
  hungarian_assigner_c(const index_t num_rows, const index_t num_cols);

  /// \brief set the size of the matrix. Must be less than capacity. This should be done before
  ///        set_weight() calls
  /// \param[in] num_rows number of rows/jobs
  /// \param[in] num_cols number of columns/workers
  /// \throw std::length_error if num_rows or num_cols is bigger than capacity
  /// \throw std::domain_error if matrix shape is skinny
  void set_size(const index_t num_rows, const index_t num_cols);

  /// \brief set weight, update book-keeping for min in row
  ///        This function is meant to be called asynchronously over jdx, so
  ///        a thread should have fixed ownership over a given idx. Note that
  ///        you can call this function again for a same index pair, but you
  ///        will break assumptions if you set it to a higher weight.
  /// \param[in] weight the weight for assignment of job idx to worker jdx
  /// \param[in] idx the index of the job
  /// \param[in] jdx the index of the worker
  /// \throw std::out_of_range if idx or jdx are outside of range specified by set_size()
  void set_weight(const float32_t weight, const index_t idx, const index_t jdx);

  /// \brief reset book-keeping and weight matrix, must be called after
  ///        assign(), and before set_weight()
  void reset();

  /// \brief reset and set_size, equivalent to reset(); set_size(num_rows, num_cols);
  /// \param[in] num_rows number of rows/jobs
  /// \param[in] num_cols number of columns/workers
  void reset(const index_t num_rows, const index_t num_cols);

  /// \brief compute minimum cost assignment
  /// \return whether or not it was successful. If assignment was unsuccessful (for example, the
  ///         case when a row has no valid assignments), then get_assignment() may return
  ///         UNASSIGNED
  /// \throw std::exception if some unspecified error happens internally, should never happen
  bool8_t assign();

  // every row/job is guaranteed to have a worker
  /// \brief dictate what the assignment for a given row/task is, should be called after assign().
  ///        If assign() returned true, then every job/row is guaranteed to have a worker/column.
  ///        If assign() returned false, then a row may not have a worker/column
  /// \param[in] idx the index for the task, starting at 0
  /// \return the index for the assigned job, starting at 0
  ///         UNASSIGNED if assign() returned false and the idx job has no possible assignments
  /// \throw std::range_error if idx is out of bounds
  index_t get_assignment(const index_t idx) const;

  // get unassigned workers/columns. User should know how many unassigned there will be apriori
  /// \brief says what jobs have been unassigned
  /// \param[in] idx the i'th unassigned job, starts from 0 to num_jobs - num_workers
  /// \return the index of the i'th unassigned job
  /// \throw std::range_error if idx is out of bounds (i.e. there are no unassigned rows)
  index_t get_unassigned(const index_t idx) const;

private:
  /// \brief do steps 1-3, return true if perfect assignment found
  HUNGARIAN_ASSIGNER_LOCAL bool8_t reduce_rows_and_init_zeros_and_check_result();

  /// \brief do step 5 and 3, return true if perfect assignment found
  HUNGARIAN_ASSIGNER_LOCAL bool8_t increment_starred_zeroes_and_check_result(index2_t loc);

  /// \brief step 4 and 6: find an uncovered zero, add it if necessary, throws exception if it can't
  HUNGARIAN_ASSIGNER_LOCAL index2_t prime_uncovered_zero();

  /// \brief step 6: reduce matrix by smallest uncovered value, return false if no uncovered value
  HUNGARIAN_ASSIGNER_LOCAL bool8_t add_new_zero(index2_t & loc);

  /// \brief check if assignment is complete
  HUNGARIAN_ASSIGNER_LOCAL bool8_t are_all_columns_covered() const;

  /// \brief find a zero that is uncovered by rows or columns
  HUNGARIAN_ASSIGNER_LOCAL bool8_t find_uncovered_zero(index2_t & loc) const;

  /// \brief find minimal value that is not covered, false if none found
  HUNGARIAN_ASSIGNER_LOCAL bool8_t find_minimum_uncovered_value(
    index2_t & loc,
    float32_t & min_val) const;

  /// \brief update internal bookkeeping for which rows and columns are uncovered
  HUNGARIAN_ASSIGNER_LOCAL void update_uncovered_rows_and_cols();

  Eigen::Matrix<float32_t, Capacity, Capacity> m_weight_matrix;
  Eigen::Matrix<int8_t, Capacity, Capacity> m_mark_matrix;
  Eigen::Matrix<index_t, Capacity, 1> m_row_min_idx;
  index_t m_num_rows;
  index_t m_num_cols;
  Eigen::Matrix<float32_t, Capacity, 1> m_row_min_weights;
  Eigen::Matrix<index_t, Capacity, 1> m_assignments;
  Eigen::Matrix<bool8_t, Capacity, 1> m_is_col_covered;
  Eigen::Matrix<bool8_t, Capacity, 1> m_is_row_covered;
  index_t m_num_uncovered_rows;
  index_t m_num_uncovered_cols;
  Eigen::Matrix<index_t, Capacity, 1> m_uncovered_rows;
  Eigen::Matrix<index_t, Capacity, 1> m_uncovered_cols;
  Eigen::Matrix<index2_t, 2 * Capacity, 1> m_augment_path;
  Eigen::Matrix<index2_t, Capacity, 1> m_primed_zero_locs;
  index_t m_num_primed_zeros;
};  // class hungarian_assigner_c

}  // namespace hungarian_assigner
}  // namespace fusion
}  // namespace autoware
#endif  // HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

/**
 * @file bdd.hpp
 *
 * @brief BDD package
 *
 * @author Mathias Soeken
 * @since  2.3
 */
#ifndef BDD_HPP
#define BDD_HPP

#include <classical/dd/dd_manager.hpp>
#include <classical/dd/bdd_fwd.hpp>

#include <boost/call_traits.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <cassert>
#include <iostream>
#include <map>
#include <memory>

namespace cirkit
{

class bdd_manager;

struct bdd
{
  using const_param_ref = boost::call_traits < bdd >::const_reference;

  bdd() : manager( nullptr ), index( 0u ) {}
  bdd( bdd_manager* manager, unsigned index )
    : manager( manager ),
      index( index ) {}
  bdd( const bdd& other ) : manager( other.manager ), index( other.index ) {}

  bdd& operator=( const bdd& other );

  unsigned var() const;
  bdd high() const;
  bdd low() const;

  inline bool is_bot() const { return index == 0u; }
  inline bool is_top() const { return index == 1u; }

  bdd operator&&( const bdd& other ) const;
  bdd operator||( const bdd& other ) const;
  bdd operator^( const bdd& other ) const;
  bdd operator!() const;
  bdd cof0( unsigned v ) const;
  bdd cof1( unsigned v ) const;
  bdd exists( const bdd& other ) const;
  bdd constrain( const bdd& other ) const;
  bdd restrict( const bdd& other ) const;
  bdd round_down( unsigned level ) const;
  bdd round_up( unsigned level ) const;
  bdd round( unsigned level ) const;

  bool equals( const bdd& other ) const;

  bdd_manager* manager;
  unsigned     index;
};

std::ostream& operator<< ( std::ostream& stream, bdd::const_param_ref bdd );

class bdd_manager : public dd_manager
{
public:
  bdd_manager( unsigned nvars, unsigned log_max_objs, bool verbose = false );
  ~bdd_manager();

  bdd_manager ( const bdd_manager& other ) = delete;
  bdd_manager& operator= ( const bdd_manager& other ) = delete;

  inline bdd bdd_bot()                { return bdd( this, 0u );     }
  inline bdd bdd_top()                { return bdd( this, 1u );     }
  inline bdd bdd_var( unsigned i )    { assert( i < nvars ); return bdd( this, i + 2u ); }
  inline bdd operator[]( unsigned i ) { assert( i < nvars ); return bdd( this, i + 2u ); }

  unsigned bdd_and( unsigned f, unsigned g );
  unsigned bdd_or( unsigned f, unsigned g );
  unsigned bdd_xor( unsigned f, unsigned g );
  unsigned bdd_not( unsigned f );
  unsigned bdd_cof0( unsigned f, unsigned v );
  unsigned bdd_cof1( unsigned f, unsigned v );
  unsigned bdd_exists( unsigned f, unsigned g );
  unsigned bdd_constrain( unsigned f, unsigned g );
  unsigned bdd_restrict( unsigned f, unsigned g );
  unsigned bdd_round_down( unsigned f, unsigned level );
  unsigned bdd_round_up( unsigned f, unsigned level );
  unsigned bdd_round( unsigned f, unsigned level );

  unsigned unique_create( unsigned var, unsigned high, unsigned low );

  static bdd_manager_ptr create( unsigned nvars, unsigned log_max_objs, bool verbose = false );

private:
  unsigned bdd_round_to( unsigned f, unsigned level, unsigned cop, unsigned to );
  unsigned bdd_round_to( unsigned f, unsigned level, unsigned cop, unsigned to, const std::map<unsigned, boost::multiprecision::uint256_t>& count_map );

public:
  friend std::ostream& operator<<( std::ostream& os, const bdd_manager& mgr );
};

}

#endif

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End: