//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//  Copyright (c) 2020-2021 Nikita Kaskov <nemo@nil.foundation>

//
//  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.
//---------------------------------------------------------------------------//

#include <boost/filesystem/path.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>

namespace nil {
    namespace filecoin {
        template<typename SchemeType>
        struct scheme_params {
            typedef SchemeType scheme_type;
        };

        template<typename CurveType>
        struct scheme_params<crypto3::zk::snark::r1cs_gg_ppzksnark<CurveType>> {
            typedef CurveType curve_type;
            typedef typename curve_type::template g1_type<> g1_type;

            typedef crypto3::zk::snark::r1cs_gg_ppzksnark<CurveType> scheme_type;
            typedef typename scheme_type::verifying_key_type verifying_key_type;

            verifying_key_type vk;

            // Elements of the form ((tau^i * t(tau)) / delta) for i between 0 and
            // m-2 inclusive. Never contains points at infinity.
            std::vector<typename g1_type::value_type> h;

            // Elements of the form (beta * u_i(tau) + alpha v_i(tau) + w_i(tau)) / delta
            // for all auxiliary inputs. Variables can never be unconstrained, so this
            // never contains points at infinity.
            std::vector<typename g1_type::value_type> l;

            // QAP "A" polynomials evaluated at tau in the Lagrange basis. Never contains
            // points at infinity: polynomials that evaluate to zero are omitted from
            // the CRS and the prover can deterministically skip their evaluation.
            std::vector<typename g1_type::value_type> a;

            // QAP "B" polynomials evaluated at tau in the Lagrange basis. Needed
            // in G1 and G2 for C/B queries, respectively. Never contains points at
            // infinity for the same reason as the "A" polynomials.
            std::vector<typename g1_type::value_type> b_g1;
            std::vector<typename g1_type::value_type> b_g2;
        };
    }    // namespace filecoin
}    // namespace nil


#include <cmath>
#include <iostream>

#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "drake/solvers/fast_qp.h"
#include "drake/solvers/gurobi_qp.h"

#define MAX_CONSTRS 1000
#define MAX_STATE 1000
#define MAX_ITER 10

// TODO(jwnimmer-tri) Someone with gurobi needs to fix these.
// NOLINTNEXTLINE(build/namespaces)
using namespace Eigen;
// NOLINTNEXTLINE(build/namespaces)
using namespace std;

// template <typename tA, typename tB, typename tC, typename tD, typename tE,
// typename tF, typename tG>
// int fastQPThatTakesQinv(vector< MatrixBase<tA>* > QinvblkDiag, const
// MatrixBase<tB>& f, const MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq,
// const MatrixBase<tE>& Ain, const MatrixBase<tF>& bin, set<int>& active,
// MatrixBase<tG>& x)
int fastQPThatTakesQinv(vector<MatrixXd*> QinvblkDiag, const VectorXd& f,
                        const MatrixXd& Aeq, const VectorXd& beq,
                        const MatrixXd& Ain, const VectorXd& bin,
                        // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                        set<int>& active, VectorXd& x) {
  int i, d;
  int iterCnt = 0;

  int M_in = bin.size();
  int M = Aeq.rows();
  int N = Aeq.cols();

  if (f.rows() != N) {
    cerr << "size of f (" << f.rows() << " by " << f.cols()
         << ") doesn't match cols of Aeq (" << Aeq.rows() << " by "
         << Aeq.cols() << ")" << endl;
    return 2;
  }
  if (beq.rows() != M) {
    cerr << "size of beq doesn't match rows of Aeq" << endl;
    return 2;
  }
  if (Ain.cols() != N) {
    cerr << "cols of Ain doesn't match cols of Aeq" << endl;
    return 2;
  }
  if (bin.rows() != Ain.rows()) {
    cerr << "bin rows doesn't match Ain rows" << endl;
    return 2;
  }
  if (x.rows() != N) {
    cerr << "x doesn't match Aeq" << endl;
    return 2;
  }
  int n_active = active.size();

  MatrixXd Aact = MatrixXd(n_active, N);
  VectorXd bact = VectorXd(n_active);

  MatrixXd QinvAteq(N, M);
  VectorXd minusQinvf(N);

  // calculate a bunch of stuff that is constant during each iteration
  int startrow = 0;
  //  for (typename vector< MatrixBase<tA>* >::iterator
  //  iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) {
  //    MatrixBase<tA> *thisQinv = *iterQinv;
  for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin();
       iterQinv != QinvblkDiag.end(); iterQinv++) {
    MatrixXd* thisQinv = *iterQinv;
    int numRow = thisQinv->rows();
    int numCol = thisQinv->cols();

    if (numRow == 1 || numCol == 1) {  // it's a vector
      d = numRow * numCol;
      if (M > 0)
        QinvAteq.block(startrow, 0, d, M) =
            thisQinv->asDiagonal() *
            Aeq.block(0, startrow, M, d)
                .transpose();  // Aeq.transpose().block(startrow, 0, d, N)
      minusQinvf.segment(startrow, d) =
          -thisQinv->cwiseProduct(f.segment(startrow, d));
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = numRow;
      if (numRow != numCol) {
        cerr << "Q is not square! " << numRow << "x" << numCol << "\n";
        return -2;
      }
      if (M > 0)
        QinvAteq.block(startrow, 0, d, M) = thisQinv->operator*(
            Aeq.block(0, startrow, M, d)
                .transpose());  // Aeq.transpose().block(startrow, 0, d, N)
      minusQinvf.segment(startrow, d) =
          -thisQinv->operator*(f.segment(startrow, d));
      startrow = startrow + d;
    }
    if (startrow > N) {
      cerr << "Q is too big!" << endl;
      return -2;
    }
  }
  if (startrow != N) {
    cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow
         << " but needed " << N << "by" << N << endl;
    return -2;
  }

  MatrixXd A;
  VectorXd b;
  MatrixXd QinvAt;
  VectorXd lam, lamIneq;
  VectorXd violated(M_in);
  VectorXd violation;

  while (1) {
    iterCnt++;

    n_active = active.size();
    Aact.resize(n_active, N);
    bact.resize(n_active);

    i = 0;
    for (set<int>::iterator iter = active.begin(); iter != active.end();
         iter++) {
      if (*iter < 0 || *iter >= Ain.rows()) {
        return -3;  // active set is invalid.  exit quietly, because this is
                    // expected behavior in normal operation (e.g. it means I
                    // should immediately kick out to Gurobi)
      }
      Aact.row(i) = Ain.row(*iter);
      bact(i++) = bin(*iter);
    }

    A.resize(Aeq.rows() + Aact.rows(), N);
    b.resize(beq.size() + bact.size());
    A << Aeq, Aact;
    b << beq, bact;

    if (A.rows() > 0) {
      // Solve H * [x;lam] = [-f;b] using Schur complements, H = [Q, At';A, 0];
      QinvAt.resize(QinvAteq.rows(), QinvAteq.cols() + Aact.rows());

      if (n_active > 0) {
        startrow = 0;
        for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin();
             iterQinv != QinvblkDiag.end(); iterQinv++) {
          MatrixXd* thisQinv = (*iterQinv);
          d = thisQinv->rows();
          int numCol = thisQinv->cols();

          if (numCol == 1) {  // it's a vector
            QinvAt.block(startrow, 0, d, M + n_active)
                << QinvAteq.block(startrow, 0, d, M),
                thisQinv->asDiagonal() *
                    Aact.block(0, startrow, n_active, d).transpose();
          } else {  // it's a matrix
            QinvAt.block(startrow, 0, d, M + n_active)
                << QinvAteq.block(startrow, 0, d, M),
                thisQinv->operator*(
                    Aact.block(0, startrow, n_active, d).transpose());
          }

          startrow = startrow + d;
        }
      } else {
        QinvAt = QinvAteq;
      }

      lam.resize(QinvAt.cols());
      lam =
          -(A * QinvAt).ldlt().solve(b + (f.transpose() * QinvAt).transpose());
      x = minusQinvf - QinvAt * lam;
      lamIneq = lam.tail(lam.size() - M);
    } else {
      x = minusQinvf;
      lamIneq.resize(0);
    }

    if (Ain.rows() == 0) {
      active.clear();
      break;
    }

    set<int> new_active;

    violation = Ain * x - bin;
    for (i = 0; i < M_in; i++)
      if (violation(i) >= 1e-6) new_active.insert(i);

    bool all_pos_mults = true;
    for (i = 0; i < n_active; i++) {
      if (lamIneq(i) < 0) {
        all_pos_mults = false;
        break;
      }
    }
    if (new_active.empty() && all_pos_mults) {
      // existing active was AOK
      break;
    }

    i = 0;
    set<int>::iterator iter = active.begin(), tmp;
    while (iter != active.end()) {  // to accommodating inloop erase
      tmp = iter++;
      if (lamIneq(i++) < 0) {
        active.erase(tmp);
      }
    }
    active.insert(new_active.begin(), new_active.end());

    if (iterCnt > MAX_ITER) {
      // Default to calling this method
      //      cout << "FastQP max iter reached." << endl;
      //       mexErrMsgIdAndTxt("Drake:approximateIKmex:Error", "Max iter
      //       reached. Problem is likely infeasible");
      return -1;
    }
  }
  return iterCnt;
}

// template <typename tA, typename tB, typename tC, typename tD, typename tE,
// typename tF, typename tG>
// int fastQP(vector< MatrixBase<tA>* > QblkDiag, const MatrixBase<tB>& f, const
// MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq, const MatrixBase<tE>& Ain,
// const MatrixBase<tF>& bin, set<int>& active, MatrixBase<tG>& x)
int fastQP(vector<MatrixXd*> QblkDiag, const VectorXd& f, const MatrixXd& Aeq,
           const VectorXd& beq, const MatrixXd& Ain, const VectorXd& bin,
           // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
           set<int>& active, VectorXd& x) {
  /* min 1/2 * x'QblkDiag'x + f'x s.t A x = b, Ain x <= bin
   * using active set method.  Iterative solve a linearly constrained
   * quadratic minimization problem where linear constraints include
   * Ain(active,:)x == bin(active).  Quit if all dual variables associated
   * with these equations are positive (i.e. they satisfy KKT conditions).
   *
   * Note:
   * fails if QP is infeasible.
   * active == initial rows of Ain to treat as equations.
   * Frank Permenter - June 6th 2013
   *
   * @retval  if feasible then iterCnt, else -1 for infeasible, -2 for input
   *error
   */

  int N = f.rows();

  MatrixXd* Qinv = new MatrixXd[QblkDiag.size()];
  vector<MatrixXd*> Qinvmap;

#define REG 1e-13
  // calculate a bunch of stuff that is constant during each iteration
  int startrow = 0;
  // typedef typename vector< MatrixBase<tA> >::iterator Qiterator;

  int i = 0;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* thisQ = *iterQ;
    int numRow = thisQ->rows();
    int numCol = thisQ->cols();

    if (numCol == 1) {  // it's a vector
      VectorXd Qdiag_mod =
          thisQ->operator+(VectorXd::Constant(numRow, REG));  // regularize
      Qinv[i] = Qdiag_mod.cwiseInverse();
      Qinvmap.push_back(&Qinv[i]);
      startrow = startrow + numRow;
    } else {  // potentially dense matrix
      if (numRow != numCol) {
        if (numRow == 1)
          cerr << "diagonal Q's must be set as column vectors" << endl;
        else
          cerr << "Q is not square! " << numRow << "x" << numCol << endl;
        return -2;
      }

      MatrixXd Q_mod =
          thisQ->operator+(REG * MatrixXd::Identity(numRow, numRow));
      Qinv[i] = Q_mod.inverse();
      Qinvmap.push_back(&Qinv[i]);
      startrow = startrow + numRow;
    }
    // cout << "Qinv{" << i << "} = " << Qinv[i] << endl;
    if (startrow > N) {
      cerr << "Q is too big!" << endl;
      return -2;
    }
    i++;
  }
  if (startrow != N) {
    cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow
         << " but needed " << N << "by" << N << endl;
    return -2;
  }

  int info = fastQPThatTakesQinv(Qinvmap, f, Aeq, beq, Ain, bin, active, x);

  delete[] Qinv;
  return info;
}

/* Example call (allocate inequality matrix, call function, resize inequalites:
  VectorXd binBnd = VectorXd(2*N);
  AinBnd.setZero();
  int numIneq = boundToIneq(ub, lb, AinBnd, binBnd);
  AinBnd.resize(numIneq, N);
  binBnd.resize(numIneq);
*/
/*
int boundToIneq(const VectorXd& uB, const VectorXd& lB, MatrixXd& Ain, VectorXd&
bin)
{
    int rCnt = 0;
    int cCnt = 0;

    if (uB.rows()+lB.rows() > A.rows() ) {
        cerr << "not enough memory allocated";
    }

    if (uB.rows()+lB.rows() > b.rows() ) {
        cerr << "not enough memory allocated";
    }

    for (int i = 0; i < lB.rows(); i++ ) {
        if (!isinf(lB(i))) {
            cout << lB(i);
            cout << i;
            Ain(rCnt, cCnt++) = -1;// lB(i);
            bin(rCnt++) = -lB(i);
        }
    }
    cCnt = 0;
    for (int i = 0; i < uB.rows(); i++ ) {
        if (!isinf(uB(i))) {
            Ain(rCnt, cCnt++) = 1;// uB(i);
            bin(rCnt++) = uB(i);
        }
    }

    // resizing inside function all causes exception (why??)
    // A.resize(rCnt, uB.rows());
    return rCnt;
}
*/

template <typename DerivedA, typename DerivedB>
int myGRBaddconstrs(GRBmodel* model, MatrixBase<DerivedA> const& A,
                    MatrixBase<DerivedB> const& b, char sense,
                    double sparseness_threshold = 1e-14) {
  int i, j, nnz, error = 0;
  /*
    // todo: it seems like I should just be able to do something like this:
    SparseMatrix<double, RowMajor> sparseAeq(Aeq.sparseView());
    sparseAeq.makeCompressed();
    error = GRBaddconstrs(
        model, nq_con, sparseAeq.nonZeros(), sparseAeq.InnerIndices(),
        sparseAeq.OuterStarts(), sparseAeq.Values(), beq.data(), NULL);
  */

  int* cind = new int[A.cols()];
  double* cval = new double[A.cols()];
  for (i = 0; i < A.rows(); i++) {
    nnz = 0;
    for (j = 0; j < A.cols(); j++) {
      if (abs(A(i, j)) > sparseness_threshold) {
        cval[nnz] = A(i, j);
        cind[nnz++] = j;
      }
    }
    error = GRBaddconstr(model, nnz, cind, cval, sense, b(i), NULL);
    if (error) break;
  }

  delete[] cind;
  delete[] cval;
  return error;
}

// template <typename tA, typename tB, typename tC, typename tD, typename tE>
// GRBmodel* gurobiQP(GRBenv *env, vector< MatrixBase<tA>* > QblkDiag, VectorXd&
// f, const MatrixBase<tB>& Aeq, const MatrixBase<tC>& beq, const
// MatrixBase<tD>& Ain, const MatrixBase<tE>& bin, VectorXd& lb, VectorXd& ub,
// set<int>& active, VectorXd& x)
GRBmodel* gurobiQP(GRBenv* env, vector<MatrixXd*> QblkDiag,
                   // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                   VectorXd& f,
                   const MatrixXd& Aeq, const VectorXd& beq,
                   const MatrixXd& Ain, const VectorXd& bin,
                   // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                   VectorXd& lb, VectorXd& ub, set<int>& active, VectorXd& x,
                   double active_set_slack_tolerance) {
  // Note: f, lb, and ub are VectorXd instead of const MatrixBase templates
  // because i want to be able to call f.data() on them

  // NOTE:  this allocates memory for a new GRBmodel and returns it. (you should
  // delete this object when you're done with it)
  // NOTE:  by convention here, the active set indices correspond to Ain, bin
  // first, then lb, then ub.

  GRBmodel* model = NULL;

  int method;
  GRBgetintparam(env, "method", &method);

  int i, j, nparams = f.rows(), Qi, Qj;
  double* lbdata = NULL, * ubdata = NULL;
  if (lb.rows() == nparams) lbdata = lb.data();
  if (ub.rows() == nparams) ubdata = ub.data();
  CGE(GRBnewmodel(env, &model, "QP", nparams, NULL, lbdata, ubdata, NULL, NULL),
      env);

  int startrow = 0, d;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* Q = *iterQ;

    // WARNING:  If there are no constraints, then Gurobi clearly solves a
    // different problem: min 1/2 x'Qx + f'x
    // This is very strange; see the solveWGUROBI method in QuadraticProgram
    if (method == 2)  //&& (Aeq.rows()+Ain.rows()>0))
      *Q = .5 * (*Q);

    if (Q->rows() == 1 || Q->cols() == 1) {  // it's a vector
      d = Q->rows() * Q->cols();
      for (i = 0; i < d; i++) {
        Qi = i + startrow;
        double& qval = Q->operator()(i);
        CGE(GRBaddqpterms(model, 1, &Qi, &Qi, &qval), env);
      }
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = Q->rows();
      if (d != Q->cols()) {
        cerr << "Q is not square! " << Q->rows() << "x" << Q->cols() << "\n";
        return NULL;
      }

      for (i = 0; i < d; i++)
        for (j = 0; j < d; j++) {
          Qi = i + startrow;
          Qj = j + startrow;
          double& qval = Q->operator()(i, j);
          CGE(GRBaddqpterms(model, 1, &Qi, &Qj, &qval), env);
        }
      startrow = startrow + d;
    }
    if (startrow > nparams) {
      cerr << "Q is too big!" << endl;
      return NULL;
    }
  }

  CGE(GRBsetdblattrarray(model, "Obj", 0, nparams, f.data()), env);

  if (Aeq.rows() > 0)
    CGE(myGRBaddconstrs(model, Aeq, beq, GRB_EQUAL, 1e-18), env);
  if (Ain.rows() > 0)
    CGE(myGRBaddconstrs(model, Ain, bin, GRB_LESS_EQUAL, 1e-18), env);

  CGE(GRBupdatemodel(model), env);
  CGE(GRBoptimize(model), env);

  CGE(GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nparams, x.data()), env);

  VectorXd slack(Ain.rows());
  CGE(GRBgetdblattrarray(model, "Slack", Aeq.rows(), Ain.rows(), slack.data()),
      env);

  int offset = 0;
  active.clear();
  for (int k = 0; k < Ain.rows(); k++) {
    if (slack(k) < active_set_slack_tolerance) active.insert(k);
  }
  offset = Ain.rows();
  if (lb.rows() == nparams) {
    for (int k = 0; k < nparams; k++) {
      if (x(k) - lb(k) < active_set_slack_tolerance) active.insert(offset + k);
    }
  }
  if (ub.rows() == nparams) {
    for (int k = 0; k < nparams; k++) {
      if (ub(k) - x(k) < active_set_slack_tolerance) {
        active.insert(offset + k + nparams);
      }
    }
  }

  return model;
}

GRBmodel* gurobiActiveSetQP(GRBenv* env, vector<MatrixXd*> QblkDiag,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& f,
                            const MatrixXd& Aeq,
                            const VectorXd& beq, const MatrixXd& Ain,
                            const VectorXd& bin,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& lb, VectorXd& ub,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            int*& vbasis, int vbasis_len,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            int*& cbasis, int cbasis_len,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& x) {
  // NOTE:  this allocates memory for a new GRBmodel and returns it. (you should
  // delete this object when you're done with it)
  // NOTE:  by convention here, the active set indices correspond to Ain, bin
  // first, then lb, then ub.
  GRBmodel* model = NULL;

  int method;
  GRBgetintparam(env, "method", &method);
  if (!(method == 0 || method == 1)) {
    cerr << "gurobiActiveSetQP: method should be 0 or 1" << endl;
    return NULL;
  }

  int i, j, nparams = f.rows(), Qi, Qj;
  double* lbdata = NULL, * ubdata = NULL;
  if (lb.rows() == nparams) lbdata = lb.data();
  if (ub.rows() == nparams) ubdata = ub.data();
  CGE(GRBnewmodel(env, &model, "QP", nparams, NULL, lbdata, ubdata, NULL, NULL),
      env);

  int startrow = 0, d;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* Q = *iterQ;

    *Q = .5 * (*Q);

    if (Q->rows() == 1 || Q->cols() == 1) {  // it's a vector
      d = Q->rows() * Q->cols();
      for (i = 0; i < d; i++) {
        Qi = i + startrow;
        double& qval = Q->operator()(i);
        CGE(GRBaddqpterms(model, 1, &Qi, &Qi, &qval), env);
      }
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = Q->rows();
      if (d != Q->cols()) {
        cerr << "Q is not square! " << Q->rows() << "x" << Q->cols() << "\n";
        return NULL;
      }

      for (i = 0; i < d; i++)
        for (j = 0; j < d; j++) {
          Qi = i + startrow;
          Qj = j + startrow;
          double& qval = Q->operator()(i, j);
          CGE(GRBaddqpterms(model, 1, &Qi, &Qj, &qval), env);
        }
      startrow = startrow + d;
    }
    if (startrow > nparams) {
      cerr << "Q is too big!" << endl;
      return NULL;
    }
  }

  CGE(GRBsetdblattrarray(model, "Obj", 0, nparams, f.data()), env);

  if (Aeq.rows() > 0)
    CGE(myGRBaddconstrs(model, Aeq, beq, GRB_EQUAL, 1e-18), env);
  if (Ain.rows() > 0)
    CGE(myGRBaddconstrs(model, Ain, bin, GRB_LESS_EQUAL, 1e-18), env);

  CGE(GRBupdatemodel(model), env);

  int numvars;
  CGE(GRBgetintattr(model, "NumVars", &numvars), env);
  if (numvars == vbasis_len) {
    CGE(GRBsetintattrarray(model, "VBasis", 0, numvars, vbasis), env);
  } else {
    delete[] vbasis;
    vbasis = new int[numvars];
  }

  int numconstr;
  CGE(GRBgetintattr(model, "NumConstrs", &numconstr), env);
  if (numconstr == cbasis_len) {
    CGE(GRBsetintattrarray(model, "CBasis", 0, numconstr, cbasis), env);
  } else {
    delete[] cbasis;
    cbasis = new int[numconstr];
  }
  CGE(GRBoptimize(model), env);

  CGE(GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nparams, x.data()), env);

  CGE(GRBgetintattrarray(model, "VBasis", 0, numvars, vbasis), env);
  CGE(GRBgetintattrarray(model, "CBasis", 0, numconstr, cbasis), env);

  return model;
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <iostream>
#include <fstream>
#include <map>
#include <numeric>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <boost/program_options.hpp>
#include <dai/util.h>
#include <dai/alldai.h>

using namespace std;
using namespace dai;
namespace po = boost::program_options;

std::vector<Real> calcDists( const vector<Factor> &x, const vector<Factor> &y ) {
    vector<Real> errs;
    errs.reserve( x.size() );
    DAI_ASSERT( x.size() == y.size() );
    for( size_t i = 0; i < x.size(); i++ )
        errs.push_back( dist( x[i], y[i], DISTTV ) );
    return errs;
}

/// Wrapper class for DAI approximate inference algorithms
class TestDAI {
    protected:
        /// Stores a pointer to an InfAlg object, managed by this class
        InfAlg          *obj;
        /// Stores the name of the InfAlg algorithm
        string          name;
        /// Stores the total variation distances of the variable marginals
        vector<Real>    varErr;
        /// Stores the total variation distances of the factor marginals
        vector<Real>    facErr;

    public:
        /// Stores the variable marginals
        vector<Factor>  varMarginals;
        /// Stores the factor marginals
        vector<Factor>  facMarginals;
        /// Stores all marginals
        vector<Factor>  allMarginals;
        /// Stores the logarithm of the partition sum
        Real            logZ;
        /// Stores the maximum difference in the last iteration
        Real            maxdiff;
        /// Stores the computation time (in seconds)
        double          time;
        /// Stores the number of iterations needed
        size_t          iters;
        /// Does the InfAlg support logZ()?
        bool            has_logZ;
        /// Does the InfAlg support maxDiff()?
        bool            has_maxdiff;
        /// Does the InfAlg support Iterations()?
        bool            has_iters;

        /// Construct from factor graph \a fg, name \a _name, and set of properties \a opts
        TestDAI( const FactorGraph &fg, const string &_name, const PropertySet &opts ) : obj(NULL), name(_name), varErr(), facErr(), varMarginals(), facMarginals(), allMarginals(), logZ(0.0), maxdiff(0.0), time(0), iters(0U), has_logZ(false), has_maxdiff(false), has_iters(false) {
            double tic = toc();

            if( name == "LDPC" ) {
                // special case: simulating a Low Density Parity Check code
                Real zero[2] = {1.0, 0.0};
                for( size_t i = 0; i < fg.nrVars(); i++ )
                    varMarginals.push_back( Factor(fg.var(i), zero) );
                allMarginals = varMarginals;
                logZ = 0.0;
                maxdiff = 0.0;
                iters = 1;
                has_logZ = false;
                has_maxdiff = false;
                has_iters = false;
            } else
                // create a corresponding InfAlg object
                obj = newInfAlg( name, fg, opts );

            // Add the time needed to create the object
            time += toc() - tic;
        }

        /// Destructor
        ~TestDAI() {
            if( obj != NULL )
                delete obj;
        }

        /// Identify
        string identify() const {
            if( obj != NULL )
                return obj->identify();
            else
                return "NULL";
        }

        /// Run the algorithm and store its results
        void doDAI() {
            double tic = toc();
            if( obj != NULL ) {
                // Initialize
                obj->init();
                // Run
                obj->run();
                // Record the time
                time += toc() - tic;

                // Store logarithm of the partition sum (if supported)
                try {
                    logZ = obj->logZ();
                    has_logZ = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_logZ = false;
                    else
                        throw;
                }

                // Store maximum difference encountered in last iteration (if supported)
                try {
                    maxdiff = obj->maxDiff();
                    has_maxdiff = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_maxdiff = false;
                    else
                        throw;
                }

                // Store number of iterations needed (if supported)
                try {
                    iters = obj->Iterations();
                    has_iters = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_iters = false;
                    else
                        throw;
                }

                // Store variable marginals
                varMarginals.clear();
                for( size_t i = 0; i < obj->fg().nrVars(); i++ )
                    varMarginals.push_back( obj->beliefV( i ) );

                // Store factor marginals
                facMarginals.clear();
                for( size_t I = 0; I < obj->fg().nrFactors(); I++ )
                    try {
                        facMarginals.push_back( obj->beliefF( I ) );
                    } catch( Exception &e ) {
                        if( e.getCode() == Exception::BELIEF_NOT_AVAILABLE )
                            facMarginals.push_back( Factor( obj->fg().factor(I).vars(), INFINITY ) );
                        else
                            throw;
                    }

                // Store all marginals calculated by the method
                allMarginals = obj->beliefs();
            };
        }

        /// Calculate total variation distance of variable and factor marginals with respect to those in \a varMargs and \a facMargs
        void calcErrors( const vector<Factor>& varMargs, const vector<Factor>& facMargs ) {
            varErr = calcDists( varMarginals, varMargs );
            facErr = calcDists( facMarginals, facMargs );
        }

        /// Return maximum variable error
        Real maxVarErr() {
            return( *max_element( varErr.begin(), varErr.end() ) );
        }

        /// Return average variable error
        Real avgVarErr() {
            return( accumulate( varErr.begin(), varErr.end(), 0.0 ) / varErr.size() );
        }

        /// Return maximum factor error
        Real maxFacErr() {
            return( *max_element( facErr.begin(), facErr.end() ) );
        }

        /// Return average factor error
        Real avgFacErr() {
            return( accumulate( facErr.begin(), facErr.end(), 0.0 ) / facErr.size() );
        }
};

/// Clips a real number: if the absolute value of \a x is less than \a minabs, return \a minabs, else return \a x
Real clipReal( Real x, Real minabs ) {
    if( abs(x) < minabs )
        return minabs;
    else
        return x;
}

/// Which marginals to outpu (none, only variable, only factor, variable and factor, all)
DAI_ENUM(MarginalsOutputType,NONE,VAR,FAC,VARFAC,ALL);

/// Main function
int main( int argc, char *argv[] ) {
    // Variables to store command line options
    // Filename of factor graph
    string filename;
    // Filename for aliases
    string aliases;
    // Approximate Inference methods to use
    vector<string> methods;
    // Which marginals to output
    MarginalsOutputType marginals;
    // Output number of iterations?
    bool report_iters = true;
    // Output calculation time?
    bool report_time = true;

    // Define required command line options
    po::options_description opts_required("Required options");
    opts_required.add_options()
        ("filename", po::value< string >(&filename), "Filename of factor graph")
        ("methods", po::value< vector<string> >(&methods)->multitoken(), "DAI methods to perform")
    ;

    // Define allowed command line options
    po::options_description opts_optional("Allowed options");
    opts_optional.add_options()
        ("help", "Produce help message")
        ("aliases", po::value< string >(&aliases), "Filename for aliases")
        ("marginals", po::value< MarginalsOutputType >(&marginals), "Output marginals? (NONE/VAR/FAC/VARFAC/ALL, default=NONE)")
        ("report-time", po::value< bool >(&report_time), "Output calculation time (default==1)?")
        ("report-iters", po::value< bool >(&report_iters), "Output iterations needed (default==1)?")
    ;

    // Define all command line options
    po::options_description cmdline_options;
    cmdline_options.add(opts_required).add(opts_optional);

    // Parse command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    po::notify(vm);

    // Display help message if necessary
    if( vm.count("help") || !(vm.count("filename") && vm.count("methods")) ) {
        cout << "This program is part of libDAI - http://www.libdai.org/" << endl << endl;
        cout << "Usage: ./testdai --filename <filename.fg> --methods <method1> [<method2> <method3> ...]" << endl << endl;
        cout << "Reads factor graph <filename.fg> and performs the approximate inference algorithms" << endl;
        cout << "<method*>, reporting for each method:" << endl;
        cout << "  o the calculation time needed, in seconds (if report-time == 1);" << endl;
        cout << "  o the number of iterations needed (if report-iters == 1);" << endl;
        cout << "  o the maximum (over all variables) total variation error in the variable marginals;" << endl;
        cout << "  o the average (over all variables) total variation error in the variable marginals;" << endl;
        cout << "  o the maximum (over all factors) total variation error in the factor marginals;" << endl;
        cout << "  o the average (over all factors) total variation error in the factor marginals;" << endl;
        cout << "  o the error (difference) of the logarithm of the partition sums;" << endl << endl;
        cout << "All errors are calculated by comparing the results of the current method with" << endl;
        cout << "the results of the first method (the base method). If marginals==VAR, additional" << endl;
        cout << "output consists of the variable marginals, if marginals==FAC, the factor marginals" << endl;
        cout << "if marginals==VARFAC, both variable and factor marginals, and if marginals==ALL, all" << endl;
        cout << "marginals calculated by the method are reported." << endl << endl;
        cout << "<method*> should be a list of one or more methods, seperated by spaces, in the format:" << endl << endl;
        cout << "    name[key1=val1,key2=val2,key3=val3,...,keyn=valn]" << endl << endl;
        cout << "where name should be the name of an algorithm in libDAI (or an alias, if an alias" << endl;
        cout << "filename is provided), followed by a list of properties (surrounded by rectangular" << endl;
        cout << "brackets), where each property consists of a key=value pair and the properties are" << endl;
        cout << "seperated by commas. If an alias file is specified, alias substitution is performed." << endl;
        cout << "This is done by looking up the name in the alias file and substituting the alias" << endl;
        cout << "by its corresponding method as defined in the alias file. Properties are parsed from" << endl;
        cout << "left to right, so if a property occurs repeatedly, the right-most value is used." << endl << endl;
        cout << opts_required << opts_optional << endl;
#ifdef DAI_DEBUG
        cout << "Note: this is a debugging build of libDAI." << endl << endl;
#endif
        cout << "Example:  ./testdai --filename testfast.fg --aliases aliases.conf --methods JTREE_HUGIN BP_SEQFIX BP_PARALL[maxiter=5]" << endl;
        return 1;
    }

    try {
        // Read aliases
        map<string,string> Aliases;
        if( !aliases.empty() )
            Aliases = readAliasesFile( aliases );

        // Read factor graph
        FactorGraph fg;
        fg.ReadFromFile( filename.c_str() );

        // Declare variables used for storing variable factor marginals and log partition sum of base method
        vector<Factor> varMarginals0;
        vector<Factor> facMarginals0;
        Real logZ0 = 0.0;

        // Output header
        cout.setf( ios_base::scientific );
        cout.precision( 3 );
        cout << "# " << filename << endl;
        cout.width( 39 );
        cout << left << "# METHOD" << "\t";
        if( report_time )
            cout << right << "SECONDS  " << "\t";
        if( report_iters )
            cout << "ITERS" << "\t";
        cout << "MAX VAR ERR" << "\t";
        cout << "AVG VAR ERR" << "\t";
        cout << "MAX FAC ERR" << "\t";
        cout << "AVG FAC ERR" << "\t";
        cout << "LOGZ ERROR" << "\t";
        cout << "MAXDIFF" << "\t";
        cout << endl;

        // For each method...
        for( size_t m = 0; m < methods.size(); m++ ) {
            // Parse method
            pair<string, PropertySet> meth = parseNameProperties( methods[m], Aliases );

            // Construct object for running the method
            TestDAI testdai(fg, meth.first, meth.second );

            // Run the method
            testdai.doDAI();

            // For the base method, store its variable marginals and logarithm of the partition sum
            if( m == 0 ) {
                varMarginals0 = testdai.varMarginals;
                facMarginals0 = testdai.facMarginals;
                logZ0 = testdai.logZ;
            }

            // Calculate errors relative to base method
            testdai.calcErrors( varMarginals0, facMarginals0 );

            // Output method name
            cout.width( 39 );
            cout << left << methods[m] << "\t";
            // Output calculation time, if requested
            if( report_time )
                cout << right << testdai.time << "\t";
            // Output number of iterations, if requested
            if( report_iters ) {
                if( testdai.has_iters ) {
                    cout << testdai.iters << "\t";
                } else {
                    cout << "N/A  \t";
                }
            }

            // If this is not the base method
            if( m > 0 ) {
                cout.setf( ios_base::scientific );
                cout.precision( 3 );

                // Output maximum error in variable marginals
                Real mev = clipReal( testdai.maxVarErr(), 1e-9 );
                cout << mev << "\t";

                // Output average error in variable marginals
                Real aev = clipReal( testdai.avgVarErr(), 1e-9 );
                cout << aev << "\t";

                // Output maximum error in factor marginals
                Real mef = clipReal( testdai.maxFacErr(), 1e-9 );
                if( mef == INFINITY )
                    cout << "N/A       \t";
                else
                    cout << mef << "\t";

                // Output average error in factor marginals
                Real aef = clipReal( testdai.avgFacErr(), 1e-9 );
                if( aef == INFINITY )
                    cout << "N/A       \t";
                else
                    cout << aef << "\t";

                // Output error in log partition sum
                if( testdai.has_logZ ) {
                    cout.setf( ios::showpos );
                    Real le = clipReal( testdai.logZ - logZ0, 1e-9 );
                    cout << le << "\t";
                    cout.unsetf( ios::showpos );
                } else
                    cout << "N/A       \t";

                // Output maximum difference in last iteration
                if( testdai.has_maxdiff ) {
                    Real md = clipReal( testdai.maxdiff, 1e-9 );
                    if( dai::isnan( mev ) )
                        md = mev;
                    if( dai::isnan( aev ) )
                        md = aev;
                    if( md == INFINITY )
                        md = 1.0;
                    cout << md << "\t";
                } else
                    cout << "N/A    \t";
            }
            cout << endl;

            // Output marginals, if requested
            if( marginals == MarginalsOutputType::VAR || marginals == MarginalsOutputType::VARFAC )
                for( size_t i = 0; i < testdai.varMarginals.size(); i++ )
                    cout << "# " << testdai.varMarginals[i] << endl;
            if( marginals == MarginalsOutputType::FAC || marginals == MarginalsOutputType::VARFAC )
                for( size_t I = 0; I < testdai.facMarginals.size(); I++ )
                    cout << "# " << testdai.facMarginals[I] << endl;
            if( marginals == MarginalsOutputType::ALL )
                for( size_t I = 0; I < testdai.allMarginals.size(); I++ )
                    cout << "# " << testdai.allMarginals[I] << endl;
        }

        return 0;
    } catch( string &s ) {
        // Abort with error message
        cerr << "Exception: " << s << endl;
        return 2;
    }
}


#ifndef FSTCRITERIONMULTINOMBHATTACHARYYA_H
#define FSTCRITERIONMULTINOMBHATTACHARYYA_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    criterion_multinom_bhattacharyya.hpp
   \brief   Implements Bhattacharyya distance based on multinomial model to serve as feature selection criterion
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include "error.hpp"
#include "global.hpp"
#include "criterion_multinom.hpp"
#include "model_multinom.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

//! Implements Bhattacharyya distance based on multinomial model to serve as feature selection criterion
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Multinomial_Bhattacharyya : public Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Multinomial_Bhattacharyya() {_IB_computed=false; notify("Criterion_Multinomial_Bhattacharyya constructor.");}
	virtual ~Criterion_Multinomial_Bhattacharyya() {notify("Criterion_Multinomial_Bhattacharyya destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);

	Criterion_Multinomial_Bhattacharyya* clone() const;
	Criterion_Multinomial_Bhattacharyya* sharing_clone() const {throw fst_error("Criterion_Multinomial_Bhattacharyya::sharing_clone() not supported, use Criterion_Multinomial_Bhattacharyya::clone() instead.");}
	Criterion_Multinomial_Bhattacharyya* stateless_clone() const {throw fst_error("Criterion_Multinomial_Bhattacharyya::stateless_clone() not supported, use Criterion_Multinomial_Bhattacharyya::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Criterion_Multinomial_Bhattacharyya()"; return os;}
private:
	Criterion_Multinomial_Bhattacharyya(const Criterion_Multinomial_Bhattacharyya& cmb); // copy-constructor
private:
	bool _IB_computed;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Criterion_Multinomial_Bhattacharyya(const Criterion_Multinomial_Bhattacharyya& cmb) :
	Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(cmb),
	_IB_computed(cmb._IB_computed)
{
	notify("Criterion_Multinomial_Bhattacharyya copy-constructor.");
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Multinomial_Bhattacharyya::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(sub);
	if(sub->get_d_raw()==0) return false;
	DIMTYPE _d = sub->get_d_raw();
	DIMTYPE f;

	if(_d==1) {// general form of Bhattacharyya unusable, return Indivdual Bhattacharyya
		if(!_IB_computed) {
			parent::_model->denarrow();
			parent::_model->compute_theta();
			parent::_model->compute_IB();
			_IB_computed=true;
		}
		if(!sub->getFirstFeature(f)) return false; assert(f>=0 && f<parent::_model->get_n());
		result = parent::_model->get_IB()[f]; // individual Bhatt - higher values denote better terms
		bool b=sub->getNextFeature(f); // just to finish the loop inside sub (to prevent future asserts in sub->set_forward_mode etc.)
		assert(b==false);
	} else {
		parent::_model->narrow_to(sub);
		parent::_model->compute_theta();
		REALTYPE doc_avg_length = parent::_model->get_doc_avg_length(); // valid after compute_theta() call
		DIMTYPE _classes=parent::_model->get_classes();

		REALTYPE *tmpoint1, *tmpoint2;
		RETURNTYPE thetasum;
		RETURNTYPE value=0.0;
		DIMTYPE c1,c2;
#ifdef DEBUG
		{
			ostringstream sos; sos << "d="<<_d<<", doc_avg_length="<<doc_avg_length<<std::endl;
			syncout::print(std::cout,sos);
		}
#endif
		for(c1=0;c1<_classes;c1++) for(c2=c1+1;c2<_classes;c2++) // for class pair c1,c2
		{
			tmpoint1=&(parent::_model->get_theta()[c1*_d]);
			tmpoint2=&(parent::_model->get_theta()[c2*_d]);
			thetasum=0.0;
			for(f=0;f<_d;f++)
			{
				thetasum+=sqrt(tmpoint1[f]*tmpoint2[f]);
#ifdef DEBUG
				{
					ostringstream sos; sos << "classes<"<<c1<<","<<c2<<">: feat "<<f<<".thetasum="<<thetasum<<", tmpoint1[i="<<f<<"]="<< tmpoint1[f]<< ", tmpoint2[i="<<f<<"]="<< tmpoint2[f]<<std::endl;
					syncout::print(std::cout,sos);
				}
#endif
			}
			value+= (double)log(thetasum) * parent::_model->get_Pc(c1) * parent::_model->get_Pc(c2); // weighted
		}
		result = (-doc_avg_length)*value;
#ifdef DEBUG
		{
			ostringstream sos; sos << "result="<<result<<std::endl<<std::endl;
			syncout::print(std::cout,sos);
		}
#endif
	}
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Criterion_Multinomial_Bhattacharyya::initialize().");
	parent::initialize(da);
	_IB_computed=false;
	return true;
}

//----------------------------------------------------------------------------

/*! \brief Implements individual Mutual Information based on multinomial model to serve as feature selection criterion in Best Individual Feature setting (feature ranking) only
    \note Can be used to evaluate single features only ! */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Multinomial_MI : public Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Multinomial_MI() {_MI_computed=false; notify("Criterion_Multinomial_MI constructor.");}
	virtual ~Criterion_Multinomial_MI() {notify("Criterion_Multinomial_MI destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);
private:
	bool _MI_computed;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_MI<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Multinomial_MI::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(sub);
	if(sub->get_d_raw()==0) return false;
	DIMTYPE _d = sub->get_d_raw();
	DIMTYPE f;

	assert(_d==1);

	if(!_MI_computed) {
		parent::_model->denarrow();
		parent::_model->compute_theta();
		parent::_model->compute_MI();
		_MI_computed=true;
	}
	if(!sub->getFirstFeature(f)) return false; assert(f>=0 && f<parent::_model->get_n());
	result = parent::_model->get_MI()[f]; // individual Bhatt - higher values denote better terms
	bool b=sub->getNextFeature(f); // just to finish the loop inside sub (to prevent future asserts in sub->set_forward_mode etc.)
	assert(b==false);
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_MI<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Criterion_Multinomial_MI::initialize().");
	parent::initialize(da);
	_MI_computed=false;
	return true;
}

} // namespace
#endif // FSTCRITERIONMULTINOMBHATTACHARYYA_H ///:~


/* Copyright (C) 2017 IBM Corp.
 *  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.
 */

/* TDMatrixParams.cpp - Parameters for trapdoor sampling
 */
#include <stdexcept>
#include <NTL/vector.h>
#include "utils/tools.h"
#include "utils/timing.h"
#include "TDMatrixParams.h"
#include <stdexcept>
#include <sys/stat.h>
#include <unistd.h>

//#define DEBUGPRINT
//#define DEBUG

NTL_CLIENT

// A static table of small co-prime factors (6-7.5 bits)
long TDMatrixParams::smallFactors[TDMATRIX_NUM_SMALL_FACTORS]
    = { TDMATRIX_SMALL_FACTORS }; // TDMATRIX_SMALL_FACTORS list in TDMatrix.h

// Calculate the sigmaX value. This value must be large enough so
// that sigmaX*I - r*maxFactor * (R/I)*(R^t|I) is positive definite.
// We compute an upper bound s on the singular values of R, then set
// sigmaX = r*maxFactor * s^2.
long getsigmaXVal(long wLen, long mBar, long maxFactor, long r)
{
  double s = r*(sqrt(wLen)+sqrt(mBar)+6);
  double sigmaZ = r*maxFactor;
  long val = ceil(sigmaZ*sigmaZ * s*s);

#ifdef DEBUG
  cout << "sigmaX val=" << val << endl;
#endif // DEBUG

  return val;
}

// lower bound the dimension to get sec bits of security
long mBound(long qBits, long errBits, long sec)
{
  return ceil((qBits-errBits)*(sec+110)/7.2);
}

long mbarBound(long qBits, long n, long sec)
{
  return ceil((2+sqrt(sec))*sqrt(n*qBits));
}

long get_qBits(long kFactors, long e)
{
  assert(kFactors>0 && e>0);
  if (kFactors > TDMATRIX_NUM_SMALL_FACTORS)
    kFactors = TDMATRIX_NUM_SMALL_FACTORS;

  double logProd = log(TDMatrixParams::smallFactors[0]);
  for (long i=1; i<kFactors; i++)
    logProd += log(TDMatrixParams::smallFactors[i]);

  return ceil(logProd*e/log(2.0));
}

// For each factor f_i, initialize zz_p context for F_i=f_i^e, and also
// compute f_i^{-1} mod F_j for all i<j. Returns max the largest factor
static long setFactors(mat_zz_p& fInv, Vec<zz_pContext>& zzp_context,
                       const vec_l factors, long e)
{
    FHE_TIMER_START;
    long kFactors = factors.length();

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    zzp_context.SetLength(kFactors);
    fInv.SetDims(kFactors,kFactors);

    NTL::zz_pPush ppush; // backup NTL's current modulus
    long maxFactor = 0;
    for (long j=kFactors-1; j>=0; j--)
    {
        if (factors[j]>maxFactor) maxFactor = factors[j];

        long f2e = NTL::power_long(factors[j],e);
        // FIXME: check that this fits in a single-precision integer
        NTL::zz_p::init(f2e);
        zzp_context[j].save(); // save the current zz_p::modulus()
        for (long i=j-1; i>=0; i--)
        {
            fInv[i][j] = NTL::inv(conv<zz_p>(factors[i])); // f_i^{-1} mod F_j
        }
    }
    return maxFactor;
}

// Initialize the parameters for a modulus q with kk factors,
// each factor is an e-th power of an even smaller number.
void TDMatrixParams::init(long nn, long kk, long ee, long mm, long rr)
{
    FHE_TIMER_START;

    assert(kk <= TDMATRIX_NUM_SMALL_FACTORS);
    factors.SetLength(kk);
    for (long i=0; i<kk; i++)
      factors[i] = smallFactors[i];
    kFactors = kk;

    e = ee;
    r = rr;
    n = nn;
    wLen = nn*kk*ee;

    if (mm > 0) { // caller supplied a value for m
      m = mm;
      mBar = m - wLen;
      if (mBar < nn) { // check that mBar is not too tiny
        mBar = nn;
	m = mBar + wLen;
      }
    }
    else { // compute m, mBar using security formulas
      long qBits = get_qBits(kk,ee);
      m = mBound(qBits, /*errBits=*/7, /*sec=*/80);
      mBar = mbarBound(qBits, n, /*sec=*/80);
      if (m < mBar+wLen) m = mBar+wLen;
      else               mBar = m - wLen;
    }

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    maxFactor = setFactors(fInv, zzp_context, factors, e);

    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);//ceil(2*r*r*r*m*maxFactor);
    // The Gaussian parameter from which we can sample with a trapdoor

    //initialize stash
    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);

    cout << "TDMatrixParams::init: sigmaX="<<sigmaX<<", mBar="<<mBar;
    cout << ", |q|="<<NTL::NumBits(this->getQ())<<endl;
}

//returns the produce of all the factors to the power of e, output = product(factors)^e
ZZ TDMatrixParams::getQ() const
{
    FHE_TIMER_START;
    ZZ q = to_ZZ(1L);
    for (long i=0; i<factors.length(); i++) // product of factors
        q *= factors[i];

    return NTL::power(q,e);  // return product^e
}

//outputs the different parameters in the class p into the stream s
ostream& operator<<(ostream &s, const TDMatrixParams& p)
{
    s << "[" << p.n        << " "
      << p.m        << " "
      << p.mBar     << " "
      << p.e        << " "
      << p.r        << "\n "
      << p.factors  << "]";
    return s;
}

//This function gets an input streams and sets the different parameters in p from this stream
istream& operator>>(istream &s, TDMatrixParams& p)
{
    seekPastChar(s, '[');  // this function is defined in tools.cpp
    s >> p.n;
    s >> p.m;
    s >> p.mBar;
    s >> p.e;
    s >> p.r;
    s >> p.factors;

    p.wLen = p.n * p.kFactors * p.e;

    p.maxFactor = setFactors(p.fInv, p.zzp_context, p.factors, p.e);
    p.sigmaX = getsigmaXVal(p.wLen, p.mBar, p.maxFactor, p.r);

    seekPastChar(s, ']');  // this function is defined in tools.cpp
    return s;
}

// binary I/O - write all the variables to the file. The handle of the open file is an input to the function The function returns the number of items written
long TDMatrixParams::writeToFile(FILE* handle) const
{
    FHE_TIMER_START;
    #ifdef DEBUGPRINT
    char cwd[1024];
    getcwd(cwd, sizeof(cwd));
    cout << "write TDMatrixParams, current directory = " << cwd << endl;
    #endif

    long count = fwrite(&n, sizeof(n), 1, handle);
    count += fwrite(&m, sizeof(m), 1, handle);
    count += fwrite(&mBar, sizeof(mBar), 1, handle);
    count += fwrite(&e, sizeof(e), 1, handle);
    count += fwrite(&r, sizeof(r), 1, handle);
    count += fwrite(&kFactors, sizeof(kFactors), 1, handle);
    count += fwrite(factors.elts(), sizeof(long), factors.length(), handle);

    return count;
}

//Binary IO - reads the class parameters from the file. The open file handle
//is provided as input to the function, and the number of items read is
//returned by the function.
long TDMatrixParams::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = 0;

#ifdef DEBUGPRINT
    char cwd[1024];
    getcwd(cwd, sizeof(cwd));
    cout << "read TDMatrixParams from current directory = " << cwd << endl;
#endif

    count += fread(&n, sizeof(n), 1, handle);
    count += fread(&m, sizeof(m), 1, handle);
    count += fread(&mBar, sizeof(mBar), 1, handle);
    count += fread(&e, sizeof(e), 1, handle);
    count += fread(&r, sizeof(r), 1, handle);
    count += fread(&kFactors, sizeof(kFactors), 1, handle);

    factors.SetLength(kFactors);
    count += fread(factors.elts(), sizeof(long), factors.length(), handle);

    //initialize stash
    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);

    wLen = n * kFactors * e;

    maxFactor = setFactors(fInv, zzp_context, factors, e);
    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);

    return count;
}

//check if all the variables in the two classes are equal if Yes, return true. Else, false.

bool operator==(const TDMatrixParams& p, const TDMatrixParams& q)
{
    return (p.n == q.n && p.m == q.m && p.mBar == q.mBar && p.e == q.e
            && p.r == q.r && p.wLen == q.wLen && p.kFactors == q.kFactors
            && p.factors == q.factors);
}

#if 0
// Deprecated, kept for debuging purposes
TDMatrixParams::TDMatrixParams(vec_l &vFactors, long lm,long rSigma,long llogQ,long lq, long nIn, long eL)
{
    FHE_TIMER_START;
    factors = vFactors;
    r = rSigma;
    kFactors = llogQ;
    n = nIn;
    m = lm;//SMS.NumCols();
    mBar = m - n*(kFactors*eL);
    wLen = n*kFactors*eL;
    e = eL;

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    maxFactor = setFactors(fInv, zzp_context, factors, e);

    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);
    // The Gaussian parameter from which we can sample with a trapdoor

    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);
}
#endif


#include <iostream>
#include "PhaseSpace.h"
#include <math.h>
#include <vector>
#include "statistics.h"
//#include "data_processing.h"
//#include "using_gnuplot.h"
//#include "cpp_dec_float.hpp"
//#include "gnuplot_i.hpp"
//#include "gnuplot-iostream.h"
//#include <boost/tuple/tuple.hpp>
using namespace std;

PhaseSpace::PhaseSpace(double hWidthC, double hHeightC, double VzDistC, double zDistC, double chirpC, double bC, double pulseEnergyC, double intensityMultiplierC,
					   double hDepthC, double hDepthVelC, double VxDistC, double xDistC, double chirpTC, double bTC, double VzCC, double zCC, double VxCC, double xCC)
	: hWidth(hWidthC), hHeight(hHeightC), VzDist(VzDistC), zDist(zDistC), chirp(chirpC), b(bC), pulseEnergy(pulseEnergyC), intensityMultiplier(intensityMultiplierC),
	  hDepth(hDepthC), hDepthVel(hDepthVelC), VxDist(VxDistC), xDist(xDistC), chirpT(chirpTC), bT(bTC), VzC(VzCC), zC(zCC), VxC(VxCC), xC(zCC) {}

PhaseSpace::PhaseSpace(vector<PhaseSpace> spaces) : hWidth(0), hHeight(0), VzDist(0), zDist(0), chirp(0), b(0), pulseEnergy(0), intensityMultiplier(0), hDepth(0), hDepthVel(0), VxDist(0), xDist(0), chirpT(0), bT(0), VzC(0), zC(0), VxC(0), xC(0)
{
	for (int i = 0; i < splitNumber; i++)
	{
		VzC += spaces[i].VzC * spaces[i].intensityMultiplier;
		//DEBUGGING cout << spaces[i].intensityMultiplier << endl;
		zC += spaces[i].zC * spaces[i].intensityMultiplier;
		xC += spaces[i].xC * spaces[i].intensityMultiplier;
		intensityMultiplier += spaces[i].intensityMultiplier;
		pulseEnergy += spaces[i].pulseEnergy;
		//spaces[i].print();
	}
	//this.VzC = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.VzC"(spaces))*this.intensityRatio;
	//int maxSize = sizeof(spaces[]);

	//for (PhaseSpace space : spaces[splitNumber]){
	//	VzC += space.VzC*space.intensityMultiplier;
	//	zC += space.zC*space.intensityMultiplier;
	//	xC += space.xC*space.intensityMultiplier;
	//}
	//this.zC = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.zC"(spaces));

	//Recalculation from single variable change
	//HWIDTH BASED RECALCULATION
	//hWidth = spaces[0].hWidth; (Shown not to be accurate/gives wrong results)
	hWidth = originalHWidth + (spaces[0].hWidth - originalHWidth) * splitNumber;
	hHeight = spaces[0].hHeight * splitNumber; //If you add a * 1/chirp here sometimes it doesn't process it for some reason, a 1/chirp isn't needed here anyway but its an odd mystery why its only sometimes processed
	zDist = spaces[0].zDist;
	VzDist = zDist * hHeight / hWidth;
	chirp = VzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	b = chirp * pow(zDist / VzDist, 2);
	hDepth = spaces[0].hDepth;
	hDepthVel = spaces[0].hDepthVel;
	VxDist = spaces[0].VxDist;
	xDist = spaces[0].xDist;
	chirpT = spaces[0].chirpT;
	bT = spaces[0].bT;

	//pulseEnergy = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.totalPulseEnergy"(spaces));
	//intensityMultiplier = taskPool.reduce!"a + b"(0.0, std.algorithm.map!"a.intensityRatio"(spaces));
}

vector<PhaseSpace> PhaseSpace::split()
{
	vector<PhaseSpace> splitSpaces;
	originalHWidth = hWidth;
	//phaseSpaces.length = to!int(spaces);
	double intensityMultipliers[splitNumber];
	for (int i = 0; i < splitNumber; i++)
	{
		intensityMultipliers[i] = get_intensity(splitNumber, i + 1);
	}
	double splitHHeight = 0;
	double splitVzDist = 0;
	double splitB = 0;
	double splitZDist = 0;
	double splitChirp = 0;
	splitHHeight = hHeight / splitNumber;
	splitVzDist = VzDist / splitNumber;
	//b = chirp * pow(zDist / VzDist, 2);
	splitB = zDist * sqrt((1 / pow(splitVzDist, 2)) - (1 / pow(splitHHeight, 2)));
	splitZDist = splitB / (sqrt((1 / pow(splitVzDist, 2)) - (1 / pow(splitHHeight, 2))));
	splitChirp = splitVzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	//i, ref elem; phaseSpaces

	for (int j = 0; j < splitNumber; j++)
	{
		splitSpaces.push_back(PhaseSpace(hWidth, splitHHeight, splitVzDist, splitZDist, splitChirp, splitB, pulseEnergy * intensityMultipliers[j], intensityMultipliers[j],
										 hDepth, hDepthVel, VxDist, xDist, chirpT, bT, hHeight - (hHeight * 2 / splitNumber) * (double(j) + 0.5), (hHeight - (hHeight * 2 / splitNumber) * (double(j) + 0.5)) / chirp, VxC, xC));
	}
	phaseSpaces += splitNumber;
	return splitSpaces;
}

vector<PhaseSpace> PhaseSpace::shatter(vector<vector<double>> spectroTable)
{
	int spaces = spectroTable.size();
	vector<PhaseSpace> shatteredPulses;
	double newVzDist = VzDist / spaces;
	double newChirp = newVzDist * sqrt((1 / pow(zDist, 2)) - (1 / pow(hWidth, 2)));
	double newB = newChirp * pow(zDist / newVzDist, 2);
	for (int i = 0; i < spaces; i++)
	{
		//cout << spectroTable[i][1] << endl;
		shatteredPulses.push_back(PhaseSpace(hWidth, hHeight / spaces, newVzDist, zDist, newChirp, newB, pulseEnergy * spectroTable[i][1 / baseTotal], intensityMultiplier * spectroTable[i][1],
											 hDepth, hDepthVel, VxDist, xDist, chirpT, bT, hHeight + (spectroTable[i][0] / 1117), zC, VxC, xC));
	}
	return shatteredPulses;
}

double PhaseSpace::intensity(double x, double y)
{
	double negTwohWidthsq = -2 * hWidth * hWidth;
	double twoVzIntDistsq = 2 * VzDist * VzDist;
	double twoPIhWidthVzIntDist = 2 * M_PI * (hWidth * VzDist);
	return exp((x * x / (negTwohWidthsq)) - ((y - chirp * x) * (y - chirp * x) / (twoVzIntDistsq))) / (twoPIhWidthVzIntDist);
}

double PhaseSpace::get_intensity(double numSections, double sectionNum)
{
	//Gets intensity % proportionally to 1 (like if its gets .5 its 50% of total intensity)
	//search with xSearch & ySearch = +- 5.803*hWidth or hHeight to get the total intensity of the phase space (equal to 1)
	double ySearchLB = -catchFactor * hHeight + ((catchFactor * hHeight * 2.0 / numSections) * (sectionNum - 1));
	double ySearchUB = catchFactor * hHeight - (catchFactor * hHeight * 2.0 / numSections) * (numSections - sectionNum);
	double xSearchLB = -catchFactor * hWidth;
	double xSearchUB = catchFactor * hWidth;

	//Convert to intensity_integration parameters
	double xHalfRange = (xSearchUB - xSearchLB) / 2;
	double yHalfRange = (ySearchUB - ySearchLB) / 2;

	double xOffset = (xSearchUB + xSearchLB) / 2;
	double yOffset = (ySearchUB + ySearchLB) / 2;

	return intensity_integration(xHalfRange, yHalfRange, xOffset, yOffset);
}

double PhaseSpace::x_integration(double xLeftLim, double xRightLim)
{
	double accuracyY = 2 * hHeight / 199;
	double accuracyX = (xRightLim - xLeftLim) / 199;
	double x = xLeftLim;
	double y = -hHeight + accuracyY / 2;

	double intensityValue = 0;

	while (y < hHeight)
	{
		while (x < xRightLim)
		{
			intensityValue += accuracyX * accuracyY * intensity(x - xC, y);
			x += accuracyX;
		}
		y += accuracyY;
		double x = xLeftLim;
	}
	return intensityValue;
}

double PhaseSpace::intensity_integration(double xHalfRange, double yHalfRange, double xOffset, double yOffset)
{
	double accuracyY = 2 * yHalfRange / 199;
	double accuracyX = 2 * xHalfRange / 199;
	double x = -xHalfRange + xOffset + accuracyX / 2;
	double y = -yHalfRange + yOffset + accuracyY / 2;

	double intensityValue = 0;

	while (y < yHalfRange + yOffset)
	{
		while (x < xHalfRange + xOffset)
		{
			intensityValue += accuracyX * accuracyY * intensity(x, y);
			x += accuracyX;
		}
		y += accuracyY;
		x = -xHalfRange + xOffset + accuracyX / 2;
	}
	return intensityValue;
}

void PhaseSpace::grid_integration(double xHalfRange, double yHalfRange, double xOffset, double yOffset, double grid[modelingXRange][modelingYRange], double xGridHalfRange, double yGridHalfRange)
{
	double accuracyY = 2 * yHalfRange / 199;
	double accuracyX = 2 * xHalfRange / 199;
	double x = -xHalfRange + xOffset + accuracyX / 2;
	double y = -yHalfRange + yOffset + accuracyY / 2;

	double intensityValue = 0;

	while (y < yHalfRange + yOffset)
	{
		while (x < xHalfRange + xOffset)
		{
			grid[int(map(x, -xGridHalfRange, xGridHalfRange, 0, double(modelingXRange) - 1) + 0.5)][int(map(y, -yGridHalfRange, yGridHalfRange, 0, double(modelingYRange) - 1) + 0.5)] += accuracyX * accuracyY * intensity(x, y);
			x += accuracyX;
		}
		y += accuracyY;
		x = -xHalfRange + xOffset + accuracyX / 2;
	}
}

PhaseSpace PhaseSpace::evolution(double dist)
{ //--To deal with processing we might need to make our own math functions. (less/more digits of accuracy)
	//A divided by 1E6 was found in D code. Reason is unknown.
	double postTime = dist / 164.35;
	b += postTime;
	bT += postTime;

	if (chirp > 0)
	{
		VzDist = sqrt(1 / ((1 / pow(hHeight, 2)) + pow((b / zDist), 2)));
	}
	if (chirpT > 0)
	{
		VxDist = sqrt(1 / ((1 / pow(hDepthVel, 2)) + pow((bT / xDist), 2)));
	}
	if (chirp < 0)
	{
		zDist = b / (sqrt((1 / pow(VzDist, 2)) - (1 / pow(hHeight, 2))));
	}
	if (chirpT < 0)
	{
		xDist = bT / (sqrt((1 / pow(VxDist, 2)) - (1 / pow(hDepthVel, 2))));
	}

	//VzDist = sqrt(1/((1/pow(hHeight,2))+pow((b/zDist),2)));
	//VxDist = sqrt(1/((1/pow(hDepthVel,2))+pow((bT/xDist),2)));

	chirp = b * pow(VzDist / zDist, 2);
	chirpT = bT * pow(VxDist / xDist, 2);
	hWidth = sqrt(1 / ((1 / pow(zDist, 2)) - pow(chirp / VzDist, 2)));
	hDepth = sqrt(1 / ((1 / pow(xDist, 2)) - pow(chirpT / VxDist, 2)));

	zC += VzC * postTime;
	xC += hDepthVel * postTime;
	return *this;
}

PhaseSpace PhaseSpace::RFLens(double power)
{
	double tempSlope = VzC / zC;
	tempSlope -= sqrt(power) * RF_LENS_COEFFICIENT;
	VzC = tempSlope * zC;

	chirp -= sqrt(power) * RF_LENS_COEFFICIENT;
	zDist = sqrt(1 / ((1 / pow(hWidth, 2)) + pow(chirp / VzDist, 2)));
	b = chirp * pow(zDist / VzDist, 2);
	hHeight = sqrt(1 / ((1 / pow(VzDist, 2)) - pow(b / zDist, 2)));
	return *this;
}

PhaseSpace PhaseSpace::mag_lens(double power)
{
	//A divided by 1E12 for power was found in D code. Reason is unknown.
	//double tempSlope = VxC / xC;
	//tempSlope -= pow(power, 2) * MAG_LENS_COEFFICIENT;
	//VxC = tempSlope * xC;
	//xC = VxC / tempSlope;

	chirpT -= pow(power, 2) * MAG_LENS_COEFFICIENT;
	xDist = sqrt(1 / ((1 / pow(hDepth, 2)) + pow(chirpT / VxDist, 2)));
	bT = chirpT * pow(xDist / VxDist, 2);
	hDepthVel = sqrt(1 / ((1 / pow(VxDist, 2)) - pow(bT / xDist, 2)));
	return *this;
}

PhaseSpace PhaseSpace::spectroscopy_function()
{
	xC = xC + 7172.99042634 * VzC;
	return *this;
}

//Accessor methods
double PhaseSpace::getHWidth() { return hWidth; }
double PhaseSpace::getHHeight() { return hHeight; }
double PhaseSpace::getVzDist() { return VzDist; }
double PhaseSpace::getZDist() { return zDist; }
double PhaseSpace::getChirp() { return chirp; }
double PhaseSpace::getB() { return b; }
double PhaseSpace::getIntensityMultiplier() { return intensityMultiplier; }
double PhaseSpace::getVzC() { return VzC; }
double PhaseSpace::getZC() { return zC; }

//Longitudinal accessor methods
double PhaseSpace::getHDepth() { return hDepth; }
double PhaseSpace::getHDepthVel() { return hDepthVel; }
double PhaseSpace::getVxDist() { return VxDist; }
double PhaseSpace::getXDist() { return xDist; }
double PhaseSpace::getChirpT() { return chirpT; }
double PhaseSpace::getBT() { return bT; }
double PhaseSpace::getVxC() { return VxC; }
double PhaseSpace::getXC() { return xC; }