/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

/**********************************************************************************************/
/*  This program is part of the Barcelona OpenMP Tasks Suite */
/*  Copyright (C) 2009 Barcelona Supercomputing Center - Centro Nacional de
 * Supercomputacion  */
/*  Copyright (C) 2009 Universitat Politecnica de Catalunya */
/*                                                                                */
/*  This program is free software; you can redistribute it and/or modify */
/*  it under the terms of the GNU General Public License as published by */
/*  the Free Software Foundation; either version 2 of the License, or */
/*  (at your option) any later version. */
/*                                                                                */
/*  This program is distributed in the hope that it will be useful, */
/*  but WITHOUT ANY WARRANTY; without even the implied warranty of */
/*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the */
/*  GNU General Public License for more details. */
/*                                                                                */
/*  You should have received a copy of the GNU General Public License */
/*  along with this program; if not, write to the Free Software */
/*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
 * USA            */
/**********************************************************************************************/

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <libgen.h>
#include "bots.h"
#include "sparselu.h"

#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/LargeArray.h"
#include "galois/graphs/FileGraph.h"
#include "galois/runtime/KDGtwoPhase.h"
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

extern char bots_arg_file[256];

/***********************************************************************
 * checkmat:
 **********************************************************************/
int checkmat(float* M, float* N) {
  int i, j;
  float r_err;

  for (i = 0; i < bots_arg_size_1; i++) {
    for (j = 0; j < bots_arg_size_1; j++) {
      r_err = M[i * bots_arg_size_1 + j] - N[i * bots_arg_size_1 + j];
      if (r_err == 0.0)
        continue;

      if (r_err < 0.0)
        r_err = -r_err;

      if (M[i * bots_arg_size_1 + j] == 0) {
        bots_message("Checking failure: A[%d][%d]=%f  B[%d][%d]=%f; \n", i, j,
                     M[i * bots_arg_size_1 + j], i, j,
                     N[i * bots_arg_size_1 + j]);
        return FALSE;
      }
      r_err = r_err / M[i * bots_arg_size_1 + j];
      if (r_err > EPSILON) {
        bots_message(
            "Checking failure: A[%d][%d]=%f  B[%d][%d]=%f; Relative Error=%f\n",
            i, j, M[i * bots_arg_size_1 + j], i, j, N[i * bots_arg_size_1 + j],
            r_err);
        return FALSE;
      }
    }
  }
  return TRUE;
}

/***********************************************************************
 * genmat:
 **********************************************************************/
static void synthetic_genmat(float* M[]) {
  int null_entry, init_val, i, j, ii, jj;
  float* p;
  int a = 0, b = 0;

  init_val = 1325;

  /* generating the structure */
  for (ii = 0; ii < bots_arg_size; ii++) {
    for (jj = 0; jj < bots_arg_size; jj++) {
      /* computing null entries */
      null_entry = FALSE;
      if ((ii < jj) && (ii % 3 != 0))
        null_entry = TRUE;
      if ((ii > jj) && (jj % 3 != 0))
        null_entry = TRUE;
      if (ii % 2 == 1)
        null_entry = TRUE;
      if (jj % 2 == 1)
        null_entry = TRUE;
      if (ii == jj)
        null_entry = FALSE;
      if (ii == jj - 1)
        null_entry = FALSE;
      if (ii - 1 == jj)
        null_entry = FALSE;
      /* allocating matrix */
      if (null_entry == FALSE) {
        a++;
        M[ii * bots_arg_size + jj] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        if (M[ii * bots_arg_size + jj] == NULL) {
          bots_message("Error: Out of memory\n");
          exit(101);
        }
        /* initializing matrix */
        p = M[ii * bots_arg_size + jj];
        for (i = 0; i < bots_arg_size_1; i++) {
          for (j = 0; j < bots_arg_size_1; j++) {
            init_val = (3125 * init_val) % 65536;
            (*p)     = (float)((init_val - 32768.0) / 16384.0);
            p++;
          }
        }
      } else {
        b++;
        M[ii * bots_arg_size + jj] = NULL;
      }
    }
  }
  bots_debug("allo = %d, no = %d, total = %d, factor = %f\n", a, b, a + b,
             (float)((float)a / (float)(a + b)));
}

// From symmetric matrix
static void structure_from_file_genmat(float* M[]) {
  int ii, jj;
  int a = 0, b;
  galois::graphs::FileGraph g;
  int num_blocks;
  unsigned max_id;

  g.fromFile(bots_arg_file);
  memset(M, 0, bots_arg_size * bots_arg_size * sizeof(*M));

  num_blocks = (g.size() + bots_arg_size_1 - 1) / bots_arg_size_1;
  max_id     = bots_arg_size_1 * bots_arg_size;

  printf("full size: %d\n", num_blocks);

  /* generating the structure */
  for (auto ii : g) {
    if (ii >= max_id)
      break;
    int bii = ii / bots_arg_size_1;
    for (auto edge : g.out_edges(ii)) {
      /* computing null entries */
      int jj = g.getEdgeDst(edge);
      if (jj >= max_id)
        continue;
      int bjj = jj / bots_arg_size_1;
      if (M[bii * bots_arg_size + bjj] == NULL) {
        a++;
        M[bii * bots_arg_size + bjj] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        memset(M[bii * bots_arg_size + bjj], 0,
               bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      }
      if (M[bii * bots_arg_size + bjj] == NULL) {
        bots_message("Error: Out of memory\n");
        exit(101);
      }
      if (M[bjj * bots_arg_size + bii] == NULL) {
        a++;
        M[bjj * bots_arg_size + bii] =
            (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
        memset(M[bjj * bots_arg_size + bii], 0,
               bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      }
      if (M[bjj * bots_arg_size + bii] == NULL) {
        bots_message("Error: Out of memory\n");
        exit(101);
      }
      M[bii * bots_arg_size + bjj]
       [(ii % bots_arg_size_1) * bots_arg_size_1 + (jj % bots_arg_size_1)] =
           g.getEdgeData<float>(edge);
      M[bjj * bots_arg_size + bii]
       [(jj % bots_arg_size_1) * bots_arg_size_1 + (ii % bots_arg_size_1)] =
           g.getEdgeData<float>(edge);
    }
  }
  // Add identity diagonal as necessary
  for (ii = 0; ii < bots_arg_size; ++ii) {
    if (M[ii * bots_arg_size + ii] == NULL) {
      a++;
      M[ii * bots_arg_size + ii] =
          (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
      memset(M[ii * bots_arg_size + ii], 0,
             bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
    }
    for (jj = 0; jj < bots_arg_size_1; ++jj) {
      if (M[ii * bots_arg_size + ii][jj * bots_arg_size_1 + jj] == 0.0)
        M[ii * bots_arg_size + ii][jj * bots_arg_size_1 + jj] = 1.0;
    }
  }
  b = num_blocks * num_blocks - a;
  bots_debug("allo = %d, no = %d, total = %d, factor = %f\n", a, b, a + b,
             (float)((float)a / (float)(a + b)));
}

void genmat(float* M[]) {
  if (strlen(bots_arg_file) == 0)
    synthetic_genmat(M);
  else
    structure_from_file_genmat(M);
}

/***********************************************************************
 * print_structure:
 **********************************************************************/
void print_structure(char* name, float* M[]) {
  int ii, jj;
  bots_message("Structure for matrix %s @ 0x%p\n", name, M);
  for (ii = 0; ii < bots_arg_size; ii++) {
    for (jj = 0; jj < bots_arg_size; jj++) {
      if (M[ii * bots_arg_size + jj] != NULL) {
        bots_message("x");
      } else
        bots_message(" ");
    }
    bots_message("\n");
  }
  bots_message("\n");
}
/***********************************************************************
 * allocate_clean_block:
 **********************************************************************/
float* allocate_clean_block() {
  int i, j;
  float *p, *q;

  p = (float*)malloc(bots_arg_size_1 * bots_arg_size_1 * sizeof(float));
  q = p;
  if (p != NULL) {
    for (i = 0; i < bots_arg_size_1; i++)
      for (j = 0; j < bots_arg_size_1; j++) {
        (*p) = 0.0;
        p++;
      }

  } else {
    bots_message("Error: Out of memory\n");
    exit(101);
  }
  return (q);
}

/***********************************************************************
 * lu0:
 **********************************************************************/
void lu0(float* diag) {
  int i, j, k;

  for (k = 0; k < bots_arg_size_1; k++)
    for (i = k + 1; i < bots_arg_size_1; i++) {
      diag[i * bots_arg_size_1 + k] =
          diag[i * bots_arg_size_1 + k] / diag[k * bots_arg_size_1 + k];
      for (j = k + 1; j < bots_arg_size_1; j++)
        diag[i * bots_arg_size_1 + j] =
            diag[i * bots_arg_size_1 + j] -
            diag[i * bots_arg_size_1 + k] * diag[k * bots_arg_size_1 + j];
    }
}

/***********************************************************************
 * bdiv:
 **********************************************************************/
void bdiv(float* diag, float* row) {
  int i, j, k;
  for (i = 0; i < bots_arg_size_1; i++)
    for (k = 0; k < bots_arg_size_1; k++) {
      row[i * bots_arg_size_1 + k] =
          row[i * bots_arg_size_1 + k] / diag[k * bots_arg_size_1 + k];
      for (j = k + 1; j < bots_arg_size_1; j++)
        row[i * bots_arg_size_1 + j] =
            row[i * bots_arg_size_1 + j] -
            row[i * bots_arg_size_1 + k] * diag[k * bots_arg_size_1 + j];
    }
}
/***********************************************************************
 * bmod:
 **********************************************************************/
void bmod(float* row, float* col, float* inner) {
  int i, j, k;
  for (i = 0; i < bots_arg_size_1; i++)
    for (j = 0; j < bots_arg_size_1; j++)
      for (k = 0; k < bots_arg_size_1; k++)
        inner[i * bots_arg_size_1 + j] =
            inner[i * bots_arg_size_1 + j] -
            row[i * bots_arg_size_1 + k] * col[k * bots_arg_size_1 + j];
}
/***********************************************************************
 * fwd:
 **********************************************************************/
void fwd(float* diag, float* col) {
  int i, j, k;
  for (j = 0; j < bots_arg_size_1; j++)
    for (k = 0; k < bots_arg_size_1; k++)
      for (i = k + 1; i < bots_arg_size_1; i++)
        col[i * bots_arg_size_1 + j] =
            col[i * bots_arg_size_1 + j] -
            diag[i * bots_arg_size_1 + k] * col[k * bots_arg_size_1 + j];
}

static galois::LargeArray<galois::runtime::Lockable> locks;

void sparselu_init(float*** pBENCH, char* pass) {
  galois::setActiveThreads(bots_arg_size_2);
  galois::substrate::getThreadPool().burnPower(bots_arg_size_2);
  galois::preAlloc(5 * bots_arg_size_2);
  galois::reportPageAlloc("MeminfoPre");
  *pBENCH = (float**)malloc(bots_arg_size * bots_arg_size * sizeof(float*));
  genmat(*pBENCH);
  print_structure(pass, *pBENCH);
  // locks.allocateInterleaved(bots_arg_size*bots_arg_size); XXX
  locks.allocateLocal(bots_arg_size * bots_arg_size);
  locks.construct();
}

void sparselu_seq_call(float** BENCH) {
  int ii, jj, kk;

  for (kk = 0; kk < bots_arg_size; kk++) {
    lu0(BENCH[kk * bots_arg_size + kk]);
    for (jj = kk + 1; jj < bots_arg_size; jj++)
      if (BENCH[kk * bots_arg_size + jj] != NULL) {
        fwd(BENCH[kk * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj]);
      }
    for (ii = kk + 1; ii < bots_arg_size; ii++)
      if (BENCH[ii * bots_arg_size + kk] != NULL) {
        bdiv(BENCH[kk * bots_arg_size + kk], BENCH[ii * bots_arg_size + kk]);
      }
    for (ii = kk + 1; ii < bots_arg_size; ii++)
      if (BENCH[ii * bots_arg_size + kk] != NULL)
        for (jj = kk + 1; jj < bots_arg_size; jj++)
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            if (BENCH[ii * bots_arg_size + jj] == NULL)
              BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
            bmod(BENCH[ii * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj],
                 BENCH[ii * bots_arg_size + jj]);
          }
  }
}

struct FwdBdiv {
  typedef int tt_does_not_need_aborts;

  float** BENCH;
  int kk;

  enum { SEEK, FWD, BDIV };

  struct Task {
    int type;
    int arg;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int arg) const { return {SEEK, arg}; }
  };

  void doSeek(const Task& t, galois::UserContext<Task>& ctx) {
    int jj = t.arg;
    if (BENCH[kk * bots_arg_size + jj] != NULL)
      ctx.push(Task{FWD, jj});
    int ii = t.arg;
    if (BENCH[ii * bots_arg_size + kk] != NULL)
      ctx.push(Task{BDIV, ii});
  }

  void doFwd(const Task& t, galois::UserContext<Task>& ctx) {
    int jj = t.arg;
    fwd(BENCH[kk * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj]);
  }

  void doBdiv(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg;
    bdiv(BENCH[kk * bots_arg_size + kk], BENCH[ii * bots_arg_size + kk]);
  }

  void operator()(const Task& t, galois::UserContext<Task>& ctx) {
    switch (t.type) {
    case SEEK:
      return doSeek(t, ctx);
    case FWD:
      return doFwd(t, ctx);
    case BDIV:
      return doBdiv(t, ctx);
    default:
      abort();
    }
  }
};

struct Bmod {
  typedef int tt_does_not_need_aborts;

  float** BENCH;
  int kk;

  enum { SEEK, BMOD };

  struct Task {
    int type;
    int arg1;
    int arg2;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int arg) const { return {SEEK, arg, arg}; }
  };

  void doSeek(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg1;
    if (BENCH[ii * bots_arg_size + kk] != NULL)
      for (int jj = kk + 1; jj < bots_arg_size; jj++)
        if (BENCH[kk * bots_arg_size + jj] != NULL)
          ctx.push(Task{BMOD, ii, jj});
  }

  void doBmod(const Task& t, galois::UserContext<Task>& ctx) {
    int ii = t.arg1;
    int jj = t.arg2;
    if (BENCH[ii * bots_arg_size + jj] == NULL)
      BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
    bmod(BENCH[ii * bots_arg_size + kk], BENCH[kk * bots_arg_size + jj],
         BENCH[ii * bots_arg_size + jj]);
  }

  void operator()(const Task& t, galois::UserContext<Task>& ctx) {
    switch (t.type) {
    case SEEK:
      return doSeek(t, ctx);
    case BMOD:
      return doBmod(t, ctx);
    default:
      abort();
    }
  }
};

struct FwdBdivBmod {
  enum { LU0 = 0, FWD = 1, BDIV = 2, BMOD = 3 };

  struct Task {
    int type;
    int kk;
    int arg0;
    int arg1;
  };

  struct Initializer : public std::unary_function<int, Task> {
    Task operator()(int kk) const { return {LU0, kk, 0, 0}; }
  };

  struct Comparator {
    bool operator()(const Task& t1, const Task& t2) const {
      // Lexicographic (kk, type, arg0, arg1)
      if (t1.kk == t2.kk)
        if (t1.type == t2.type)
          if (t1.arg0 == t2.arg0)
            return t1.arg1 < t2.arg1;
          else
            return t1.arg0 < t2.arg0;
        else
          return t1.type < t2.type;
      else
        return t1.kk < t2.kk;
    }
  };

  struct NeighborhoodVisitor {
    static const unsigned CHUNK_SIZE = 4;
    float** BENCH;

    typedef int tt_does_not_need_push;

    void acquire(int ii, int jj) {
      galois::runtime::acquire(&locks[ii * bots_arg_size + jj],
                               galois::MethodFlag::WRITE);
    }

    void operator()(const Task& t, galois::UserContext<Task>&) {
      int ii, jj, kk = t.kk;
      switch (t.type) {
      case LU0:
        acquire(kk, kk);
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            acquire(kk, jj);
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, kk);
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, kk);
            for (jj = kk + 1; jj < bots_arg_size; jj++) {
              if (BENCH[kk * bots_arg_size + jj] != NULL) {
                acquire(kk, jj);
                acquire(ii, jj);
              }
            }
          }
        }
        return;
      case FWD:
        acquire(kk, t.arg0);
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            acquire(ii, t.arg0);
          }
        }
        return;
      case BDIV:
        acquire(t.arg0, kk);
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            acquire(t.arg0, jj);
          }
        }
        return;
      case BMOD:
        acquire(t.arg0, t.arg1);
        return;
      }
    }
  };

  struct Process {
    static const int CHUNK_SIZE = 1;

    typedef int tt_does_not_need_aborts;

    float** BENCH;

    void operator()(const Task& t, galois::UserContext<Task>& ctx) {
      int ii, jj, kk = t.kk;

      switch (t.type) {
      case LU0:
        lu0(BENCH[kk * bots_arg_size + kk]);
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            ctx.push(Task{BDIV, kk, ii, 0});
          }
        }
        for (jj = kk + 1; jj < bots_arg_size; jj++) {
          if (BENCH[kk * bots_arg_size + jj] != NULL) {
            ctx.push(Task{FWD, kk, jj, 0});
          }
        }
        for (ii = kk + 1; ii < bots_arg_size; ii++) {
          if (BENCH[ii * bots_arg_size + kk] != NULL) {
            for (jj = kk + 1; jj < bots_arg_size; jj++) {
              if (BENCH[kk * bots_arg_size + jj] != NULL) {
                if (BENCH[ii * bots_arg_size + jj] == NULL)
                  BENCH[ii * bots_arg_size + jj] = allocate_clean_block();
                ctx.push(Task{BMOD, kk, ii, jj});
              }
            }
          }
        }
        return;
      case FWD:
        return fwd(BENCH[kk * bots_arg_size + kk],
                   BENCH[kk * bots_arg_size + t.arg0]);
      case BDIV:
        return bdiv(BENCH[kk * bots_arg_size + kk],
                    BENCH[t.arg0 * bots_arg_size + kk]);
      case BMOD:
        return bmod(BENCH[t.arg0 * bots_arg_size + kk],
                    BENCH[kk * bots_arg_size + t.arg1],
                    BENCH[t.arg0 * bots_arg_size + t.arg1]);
      }
    }
  };
};

static void bs_algo(float** BENCH) {
  int ii, jj, kk;

  namespace ww = galois::worklists;
  typedef ww::StableIterator<>::with_container<ww::PerSocketChunkLIFO<1>>::type
      WL;

  for (kk = 0; kk < bots_arg_size; kk++) {
    lu0(BENCH[kk * bots_arg_size + kk]);

    galois::for_each(
        boost::transform_iterator<FwdBdiv::Initializer,
                                  boost::counting_iterator<int>>(kk + 1),
        boost::transform_iterator<FwdBdiv::Initializer,
                                  boost::counting_iterator<int>>(bots_arg_size),
        FwdBdiv{BENCH, kk}, galois::wl<WL>());

    galois::for_each(
        boost::transform_iterator<Bmod::Initializer,
                                  boost::counting_iterator<int>>(kk + 1),
        boost::transform_iterator<Bmod::Initializer,
                                  boost::counting_iterator<int>>(bots_arg_size),
        Bmod{BENCH, kk}, galois::wl<WL>());
  }
}

static void ikdg_algo(float** BENCH) {
  int kk;
  typedef boost::transform_iterator<FwdBdivBmod::Initializer,
                                    boost::counting_iterator<int>>
      TI;

  galois::setDoAllImpl(galois::DOALL_COUPLED);
  if (galois::getDoAllImpl() != galois::DOALL_COUPLED) {
    std::abort();
  }

  galois::runtime::for_each_ordered_ikdg(
      galois::runtime::makeStandardRange(TI(0), TI(bots_arg_size)),
      FwdBdivBmod::Comparator{}, FwdBdivBmod::NeighborhoodVisitor{BENCH},
      FwdBdivBmod::Process{BENCH},
      std::make_tuple(galois::loopname("sparselu-ikdg")));
}

void sparselu_par_call(float** BENCH) {
  galois::StatManager manager;
  galois::StatTimer T;

  T.start();
  bots_message(
      "Computing SparseLU Factorization (%dx%d matrix with %dx%d blocks) ",
      bots_arg_size, bots_arg_size, bots_arg_size_1, bots_arg_size_1);
  if (bots_app_cutoff_value_1 == 0)
    bs_algo(BENCH);
  else
    ikdg_algo(BENCH);
  bots_message(" completed!\n");
  galois::reportPageAlloc("MeminfoPost");
  T.stop();
}

void sparselu_fini(float** BENCH, char* pass) {
  galois::substrate::getThreadPool().beKind();
  print_structure(pass, BENCH);
  locks.destroy();
  locks.deallocate();
}

int sparselu_check(float** SEQ, float** BENCH) {
  int ii, jj, ok = 1;

  for (ii = 0; ((ii < bots_arg_size) && ok); ii++) {
    for (jj = 0; ((jj < bots_arg_size) && ok); jj++) {
      if (SEQ[ii * bots_arg_size + jj] == NULL &&
          BENCH[ii * bots_arg_size + jj] != NULL)
        ok = FALSE;
      if (SEQ[ii * bots_arg_size + jj] != NULL &&
          BENCH[ii * bots_arg_size + jj] == NULL)
        ok = FALSE;
      if (SEQ[ii * bots_arg_size + jj] != NULL &&
          BENCH[ii * bots_arg_size + jj] != NULL)
        ok = checkmat(SEQ[ii * bots_arg_size + jj],
                      BENCH[ii * bots_arg_size + jj]);
    }
  }
  if (ok)
    return BOTS_RESULT_SUCCESSFUL;
  else
    return BOTS_RESULT_UNSUCCESSFUL;
}


// Enable C++11 via this plugin (Rcpp 0.10.3 or later)
// [[Rcpp::plugins(cpp11)]]

// [[Rcpp::depends(BH)]]

#include <Rcpp.h>
using namespace Rcpp;

#define _USE_MATH_DEFINES
#include <math.h>

#include <random>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <functional>
#include <iterator>
#include <string>
#include <unordered_map>
#include <boost/functional/hash.hpp>

typedef std::vector<int> sequence;

sequence subseq(const sequence &x, unsigned int first, unsigned int last) {
  if (last >= x.size() || last < first) {
    stop("invalid subsequence indices");
  }
  int n = 1 + last - first;
  sequence res(n);
  for (int j = 0; j < n; j ++) {
    res[j] = x[j + first];
  }
  return(res);
}

sequence last_n (const sequence &x, int n) {
  if (n < 0) {
    stop("n cannot be less than 0");
  }
  int original_length = static_cast<int>(x.size());
  if (n > original_length) {
    stop("cannot excise more elements than the sequence contains");
  }
  if (n == 0) {
    sequence res(0);
    return res;
  } else {
    sequence res(n);
    for (int i = 0; i < n; i ++) {
      res[i] = x[i + original_length - n];
    }
    return res;
  }
}

void print(const sequence &x) {
  for (unsigned int j = 0; j < x.size(); j ++) {
    if (j > 0) {
      Rcout << " ";
    }
    Rcout << x[j];
  }
  Rcout << "\n";
}

void print(const std::vector<double> &x) {
  for (unsigned int j = 0; j < x.size(); j ++) {
    if (j > 0) {
      Rcout << " ";
    }
    Rcout << x[j];
  }
  Rcout << "\n";
}

void print(const std::vector<bool> &x) {
  for (unsigned int j = 0; j < x.size(); j ++) {
    if (j > 0) {
      Rcout << " ";
    }
    if (x[j]) {
      Rcout << "True";
    } else {
      Rcout << "False";
    }
  }
  Rcout << "\n";
}

RObject list_to_tibble(List x) {
  Environment pkg = Environment::namespace_env("tibble");
  Function f = pkg["as_tibble"];
  return(f(x));
}

NumericVector as_numeric_vector(std::vector<double> x) {
  int size = x.size();
  NumericVector res = NumericVector(size);
  for (int i = 0; i < size; i ++) {
    res[i] = x[i];
  }
  return res;
}

template <typename Container> // we can make this generic for any container [1]
struct container_hash {
  std::size_t operator() (Container const& c) const {
    return boost::hash_range(c.begin(), c.end());
  }
};

class record {

};

class record_simple: public record {
public:
  long int full_count = 0;
  long int up_ex_count = 0;

  record_simple() {};

  void add_1(bool full_only) {
    if (full_count >= LONG_MAX || up_ex_count >= LONG_MAX) {
      stop("cannot increment this record count any higher");
    }
    full_count ++;
    if (!full_only) {
      up_ex_count ++;
    }
  }
};

double compute_entropy(std::vector<double> x) {
  int n = x.size();
  double counter = 0;
  for (int i = 0; i < n; i ++) {
    double p = x[i];
    counter -= p * log2(p);
  }
  return counter;
}

std::vector<double> normalise_distribution(std::vector<double> &x) {
  double total = 0;
  int n = static_cast<int>(x.size());
  for (int i = 0; i < n; i ++) {
    total += x[i];
  }
  for (int i = 0; i < n; i ++) {
    x[i] = x[i] / total;
  }
  return x;
}

class record_decay: public record {
public:
  record_decay() {}
  std::vector<int> pos;
  // std::vector<double> time;
  void insert(int pos_, double time_) {
    pos.push_back(pos_);
    // time.push_back(time_);
  }
};

class symbol_prediction {
public:
  int symbol;
  int pos;
  double time;
  int model_order;
  std::vector<double> distribution;
  double information_content;

  symbol_prediction(int symbol_,
                    int pos_,
                    double time_,
                    int model_order_,
                    const std::vector<double> &distribution_) {
    int dist_size_ = distribution_.size();
    if (symbol_ > dist_size_) {
      Rcout << "symbol = " << symbol_ << ", distribution(n) = " << distribution_.size() << "\n";
      stop("observed symbol not compatible with distribution dimensions");
    }

    symbol = symbol_;
    pos = pos_;
    time = time_;
    model_order = model_order_;
    distribution = distribution_;
    information_content = - log2(distribution[symbol]);
  }
};

class sequence_prediction {
public:
  bool return_distribution;
  bool return_entropy;
  bool decay;

  std::vector<int> symbol;
  std::vector<int> pos;
  std::vector<double> time;
  std::vector<int> model_order;
  std::vector<double> information_content;
  std::vector<double> entropy;
  std::vector<std::vector<double>> distribution;

  sequence_prediction(bool return_distribution_,
                      bool return_entropy_,
                      bool decay_) {
    return_distribution = return_distribution_;
    return_entropy = return_entropy_;
    decay = decay_;
  }

  void insert(const symbol_prediction &x) {
    symbol.push_back(x.symbol);
    model_order.push_back(x.model_order);
    information_content.push_back(x.information_content);
    if (return_entropy) {
      entropy.push_back(compute_entropy(x.distribution));
    }
    if (return_distribution) {
      distribution.push_back(x.distribution);
    }
    if (decay) {
      pos.push_back(x.pos);
      time.push_back(x.time);
    }
  }

  List as_list() {
    List x = List::create(Named("symbol") = symbol);
    if (decay) {
      x.push_back(pos, "pos");
      x.push_back(time, "time");
    }
    x.push_back(model_order, "model_order");
    x.push_back(information_content, "information_content");
    if (return_entropy) {
      x.push_back(entropy, "entropy");
    }
    if (return_distribution) {
      x.push_back(distribution, "distribution");
    }
    return(x);
  }

  RObject as_tibble() {
    return(list_to_tibble(this->as_list()));
  }
};

class model_order {
public:
  int chosen;
  int longest_available;
  bool deterministic_any;
  int deterministic_shortest;
  bool deterministic_is_selected;

  model_order(int chosen_,
              int longest_available_,
              bool deterministic_any_,
              int deterministic_shortest_,
              bool deterministic_is_selected_) {
    chosen = chosen_;
    longest_available = longest_available_;
    deterministic_any = deterministic_any_;
    deterministic_shortest = deterministic_shortest_;
    deterministic_is_selected = deterministic_is_selected_;
  }
};

class ppm {
public:
  int alphabet_size;
  int order_bound;
  bool shortest_deterministic;
  bool exclusion;
  bool update_exclusion;
  std::string escape;
  double k;
  bool decay;
  bool sub_n_from_m1_dist;
  bool lambda_uses_zero_weight_symbols;
  bool debug_smooth;
  CharacterVector alphabet_levels;

  int num_observations = 0;
  std::vector<double> all_time;

  ppm(int alphabet_size_,
      int order_bound_,
      bool shortest_deterministic_,
      bool exclusion_,
      bool update_exclusion_,
      std::string escape_,
      bool decay_,
      bool sub_n_from_m1_dist_,
      bool lambda_uses_zero_weight_symbols_,
      CharacterVector alphabet_levels_
  ) {
    if (alphabet_size_ <= 0) {
      stop("alphabet size must be greater than 0");
    }

    alphabet_size = alphabet_size_;
    order_bound = order_bound_;
    shortest_deterministic = shortest_deterministic_;
    exclusion = exclusion_;
    update_exclusion = update_exclusion_;
    escape = escape_;
    k = this->get_k(escape);
    decay = decay_;
    sub_n_from_m1_dist = sub_n_from_m1_dist_;
    lambda_uses_zero_weight_symbols = lambda_uses_zero_weight_symbols_;
    alphabet_levels = alphabet_levels_;
  }

  virtual ~ ppm() {};

  // returns true if the n_gram already existed in the memory bank
  virtual bool insert(sequence x, int pos, double time, bool full_only) {
    stop("this shouldn't happen (1)");
    return true;
  };

  virtual double get_weight(const sequence &n_gram,
                            int pos,
                            double time,
                            bool update_excluded) {
    return 0.0;
  };

  double get_num_observed_symbols(int pos, double time) {
    int res = 0;
    for (int i = 0; i < this->alphabet_size; i ++) {
      sequence symbol(1, i);
      double weight = get_weight(symbol, pos, time, false);
      if (weight > 0.0) res ++;
    }
    return res;
  }

  double get_context_count(const std::vector<double> &counts,
                           const std::vector<bool> &excluded) {
    double context_count = 0;
    for (int i = 0; i < this->alphabet_size; i ++) {
      if (!excluded[i]) {
        context_count += counts[i];
      }
    }
    return context_count;
  }

  sequence_prediction model_seq(sequence x,
                                NumericVector time = NumericVector(0),
                                bool train = true,
                                bool predict = true,
                                bool return_distribution = true,
                                bool return_entropy = true,
                                bool generate = false) {
    int n = x.size();
    if (this->decay &&
        (static_cast<unsigned int>(x.size()) !=
        static_cast<unsigned int>(time.size()))) {
      stop("time must either have length 0 or have length equal to x");
    }
    if (this->all_time.size() > 0 && time.size() > 0 && time[0] < this->all_time.back()) {
      stop("a sequence may not begin before the previous sequence finished");
    }

    sequence_prediction result(return_distribution,
                               return_entropy,
                               this->decay);

    for (int i = 0; i < n; i ++) {
      int pos_i = num_observations;
      double time_i = this->decay? time[i] : 0;
      // Predict
      if (predict) {
        sequence context = (i < 1 || order_bound < 1) ? sequence() :
        subseq(x,
               std::max(0, i - order_bound),
               i - 1);
        symbol_prediction pred = predict_symbol(x[i], context, pos_i, time_i, generate);
        result.insert(pred);
        if (generate) {
          x[i] = pred.symbol;
        }
      }
      // Train
      if (train) {
        if (decay) this->all_time.push_back(time_i);
        bool full_only = false;
        for (int h = std::max(0, i - order_bound); h <= i; h ++) {
          full_only = this->insert(subseq(x, h, i), pos_i, time_i, full_only);
        }
        num_observations ++;
      }
    }
    return(result);
  }

  symbol_prediction predict_symbol(
      int symbol,
      const sequence &context,
      int pos,
      double time,
      bool generate
    ) {
    if (!generate) {
      if (symbol < 0) {
        stop("symbols must be greater than or equal to 0");
      }
      if (symbol > alphabet_size - 1) {
        stop("symbols cannot exceed (alphabet_size - 1)");
      }
    }

    model_order model_order = this->get_model_order(context, pos, time);
    std::vector<double> dist = get_probability_distribution(context,
                                                            model_order,
                                                            pos,
                                                            time);

    if (generate) {
      int n_samples = 1;
      bool replace = false;
      NumericVector probs = as_numeric_vector(dist);
      bool one_indexed = false;
      symbol = Rcpp::sample(alphabet_size, n_samples, replace, probs, one_indexed)[0];
    }

    symbol_prediction out(symbol, pos, time, model_order.chosen, dist);
    return(out);
  }

  std::vector<double> get_probability_distribution(const sequence &context,
                                                   model_order model_order,
                                                   int pos,
                                                   double time) {
    std::vector<bool> excluded(alphabet_size, false);
    std::vector<double> dist = get_smoothed_distribution(
      context,
      model_order,
      model_order.chosen,
      pos,
      time,
      excluded
    );
    return normalise_distribution(dist);
  }

  std::vector<double> get_smoothed_distribution(const sequence &context,
                                                model_order model_order,
                                                int order,
                                                int pos,
                                                double time,
                                                std::vector<bool> &excluded) {
    if (order == -1) {
      return get_order_minus_1_distribution(pos, time);
    } else {
      bool update_excluded = this->update_exclusion;
      if (order == model_order.chosen &&
          this->shortest_deterministic &&
          this->update_exclusion &&
          model_order.deterministic_is_selected) {
        update_excluded = false;
      }

      std::vector<int> n_gram = last_n(context, order);
      n_gram.resize(order + 1);

      std::vector<double> counts(this->alphabet_size);
      int num_distinct_symbols = 0;
      std::vector<bool> predicted(this->alphabet_size);

      for (int i = 0; i < this->alphabet_size; i ++) {
        n_gram[order] = i;
        counts[i] = this->get_weight(n_gram, pos, time, update_excluded);
        if (counts[i] > 0.0) {
          predicted[i] = true;
          num_distinct_symbols += 1;
        } else {
          predicted[i] = false;
        }
        counts[i] = this->modify_count(counts[i]);
      }

      // Rcout << "counts: ";
      // print(counts);

      double context_count = get_context_count(counts, excluded);
      double lambda = get_lambda(counts, context_count, num_distinct_symbols);

      std::vector<double> alphas = get_alphas(lambda, counts, context_count);

      if (this->debug_smooth) {
        Rcout << "\n*** order = " << order << " ***\n";
        Rcout << "pos = " << pos << "\n";
        Rcout << "time = " << time << "\n";
        Rcout << "model_order.chosen = " << model_order.chosen << "\n";
        // Rcout << "this->shortest_deterministic = " << this->shortest_deterministic << "\n";
        // Rcout << "this->update_exclusion = " << this->update_exclusion << "\n";
        // Rcout << "model_order.deterministic_is_selected = " << model_order.deterministic_is_selected << "\n";
        Rcout << "context = ";
        print(last_n(context, order));
        // Rcout << "update_excluded = " << update_excluded << "\n";
        Rcout << "counts = ";
        print(counts);
        Rcout << "context_count = " << context_count << "\n";
        Rcout << "lambda = " << lambda << "\n";
        Rcout << "alphas = ";
        print(alphas);
      }

      if (this->exclusion) {
        for (int i = 0; i < alphabet_size; i ++) {
          // There is a choice here:
          // do we exclude symbols that have alphas greater than 0
          // (i.e. their counts survive addition of k),
          // or do we exclude any symbol that is present in the tree at all,
          // even if adding k takes it down to 0?
          //
          // Since decay-based models don't have exclusion,
          // we only have to think about normal PPM models.
          // All of these models apart from PPM-B have k > -1,
          // in which case there is no difference between the strategies.
          // We only have to worry for PPM-B.
          //
          // Following Bunton (1996) and Pearce (2005)'s implementation,
          // we adopt the latter strategy, excluding symbols even
          // if their alphas are equal to 0, as long as they were present
          // in the tree.

          if (predicted[i]) {
            excluded[i] = true;
          }
        }
        if (this->debug_smooth) {
          Rcout << "new excluded = ";
          print(excluded);
        }
      }

      std::vector<double> lower_order_distribution = get_smoothed_distribution(
        context, model_order, order - 1, pos, time, excluded);

      std::vector<double> res(this->alphabet_size);
      for (int i = 0; i < this->alphabet_size; i ++) {
        res[i] = alphas[i] + (1 - lambda) * lower_order_distribution[i];
      }

      if (this->debug_smooth) {
        Rcout << "order " << order << " ";
        Rcout << "probability distribution = ";
        print(res);
      }

      return res;
    }
  }

  std::vector<double>get_alphas(double lambda,
                                const std::vector<double> &counts,
                                double context_count) {
    if (lambda > 0) {
      std::vector<double> res(this->alphabet_size);
      for (int i = 0; i < this->alphabet_size; i ++) {
        res[i] = lambda * counts[i] / context_count;
      }
      return res;
    } else {
      std::vector<double> res(this->alphabet_size, 0);
      return res;
    }
  }

  // The need to capture situations where the context_count is 0 is
  // introduced by Pearce (2005)'s decision to introduce exclusion
  // (see 6.2.3.3), though the thesis does not mention
  // this explicitly.
  virtual double get_lambda(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    stop("this virtual get_lambda method should never be called directly");
    return 0.0;
  }

  double get_k(const std::string &e) {
    if (e == "a") {
      return 0;
    } else if (e == "b") {
      return - 1;
    } else if (e == "c") {
      return 0;
    } else if (e == "d") {
      return - 0.5;
    } else if (e == "ax") {
      return 0;
    } else {
      stop("unrecognised escape method");
    }
  }

  double get_effective_distinct_symbols(int num_distinct_symbols,
                                        const std::vector<double> &counts) {
    if (this->lambda_uses_zero_weight_symbols) {
      return num_distinct_symbols;
    } else {
      return this->count_positive_values(counts);
    }
  }

  double lambda_a(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    if (this->debug_smooth) {
      Rcout << "lambda_a, context_count = " << context_count << "\n";
    }
    return context_count / (context_count + 1.0);
  }

  double lambda_b(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    double effective_distinct_symbols =
      this->get_effective_distinct_symbols(num_distinct_symbols,
                                           counts);

    return static_cast<double>(context_count) /
      static_cast<double>(context_count + effective_distinct_symbols);
  }

  double lambda_c(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    double effective_distinct_symbols =
      this->get_effective_distinct_symbols(num_distinct_symbols,
                                           counts);

    return static_cast<double>(context_count) /
      static_cast<double>(context_count + effective_distinct_symbols);
  }

  double lambda_d(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    double effective_distinct_symbols =
      this->get_effective_distinct_symbols(num_distinct_symbols,
                                           counts);

    return static_cast<double>(context_count) /
      (static_cast<double>(context_count + effective_distinct_symbols / 2.0));
  }

  double lambda_ax(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    // Note - there is a mistake in the reference papers,
    // Pearce & Wiggins (2004), also Pearce (2005);
    // the 1.0 is missing from the equation.
    // Our version is consistent with the context literature though,
    // and consistent with Pearce's LISP implementation.
    //
    // We generalise the definition of singletons to decayed counts between
    // 0 and 1. This is a bit hacky though, and the escape method
    // should ultimately be reconfigured for new decay functions.
    return static_cast<double>(context_count) /
      static_cast<double>(context_count + num_singletons(counts) + 1.0);
  }

  int num_singletons(const std::vector<double> &x) {
    int n = static_cast<int>(x.size());
    int res = 0;
    for (int i = 0; i < n; i ++) {
      if (x[i] > 0 && x[i] <= 1) {
        res ++;
      }
    }
    return res;
  }

  double modify_count(double count) {
    if (this->k == 0 || count == 0) {
      return count;
    } else {
      double x = count + this->k;
      if (x > 0) {
        return x;
      } else {
        return 0;
      }
    }
  }

  int count_positive_values(const std::vector<double> &x) {
    int n = static_cast<int>(x.size());
    int res = 0;
    for (int i = 0; i < n; i ++) {
      if (x[i] > 0) {
        res ++;
      }
    }
    return res;
  }

  std::vector<double> get_order_minus_1_distribution(int pos, double time) {

    // See Bunton (1996, p. 82): alpha(s0) comes from the 3-arg version of count(),
    // which does not include exclusion or subtraction
    // of the k parameter (see escape method).
    // It instead corresponds to the number of symbols that the model
    // has ever seen.

    //// Old version:
    // int num_observed_symbols = 0;
    // for (int i = 0; i < this->alphabet_size; i ++) {
    //   if (excluded[i]) {
    //     num_observed_symbols ++;
    //   }
    // }

    double denominator = this->alphabet_size + 1;

    if (this->sub_n_from_m1_dist) {
      // This is disabled for decay-based models
      double num_observed_symbols = this->get_num_observed_symbols(pos, time);
      denominator -= num_observed_symbols;
    }

    double p = 1.0 / denominator;
    std::vector<double> res(this->alphabet_size, p);

    if (this->debug_smooth) {
      Rcout << "order minus 1 distribution = ";
      print(res);
      Rcout << "\n";
    }
    return res;
  }

  model_order get_model_order(const sequence &context, int pos, double time) {
    const int longest_available = this->get_longest_context(context, pos, time);
    int chosen = longest_available;

    int det_shortest = - 1;
    int det_any = false;
    int det_is_selected = false;

    if (shortest_deterministic) {
      int det_shortest = this->get_shortest_deterministic_context(context,
                                                                  pos,
                                                                  time);
      bool det_any = det_shortest >= 0;
      if (det_any) {
        if (det_shortest < longest_available) {
          det_is_selected = true;
          chosen = det_shortest;
        }
      }
    }

    return(model_order(chosen, longest_available,
                       det_any, det_shortest, det_is_selected));
  }

  virtual int get_longest_context(sequence context, int pos, double time) {
    stop("this shouldn't happen (2)");
    return 0;
  }

  int get_shortest_deterministic_context(const sequence &context, int pos, double time) {
    int len = static_cast<int>(context.size());
    int res = -1;
    for (int order = 0; order <= std::min(len, order_bound); order ++) {
      sequence effective_context = order == 0 ? sequence() : subseq(context,
                                                         len - order, len - 1);
      if (is_deterministic_context(effective_context, pos, time)) {
        res = order;
        break;
      }
    }
    return(res);
  }

  bool is_deterministic_context(const sequence &context, int pos, double time) {
    int num_continuations = 0;
    for (int i = 0; i < alphabet_size; i ++) {
      sequence n_gram = context;
      n_gram.push_back(i);
      double weight = this->get_weight(n_gram,
                                       pos,
                                       time,
                                       false); // update exclusion
      if (weight > 0) {
        num_continuations ++;
        if (num_continuations > 1) {
          break;
        }
      }
    }
    return num_continuations == 1;
  }
};

class ppm_simple: public ppm {
public:
  std::unordered_map<sequence,
                     record_simple,
                     container_hash<sequence>> data;

  ppm_simple(
    int alphabet_size_,
    int order_bound_,
    bool shortest_deterministic_,
    bool exclusion_,
    bool update_exclusion_,
    std::string escape_,
    CharacterVector alphabet_levels_
  ) : ppm(
      alphabet_size_,
      order_bound_,
      shortest_deterministic_,
      exclusion_,
      update_exclusion_,
      escape_,
      false, // decay
      true, // sub_n_from_m1_dist
      true, // lambda_uses_zero_weight_symbols
      alphabet_levels_
      ) {
    data = {};
  }

  ~ ppm_simple() {};

  bool insert(sequence x, int pos, double time, bool full_only) {
    std::unordered_map<sequence, record_simple, container_hash<sequence>>::const_iterator target = data.find(x);
    if (target == data.end()) {
      record_simple record;
      record.add_1(full_only);
      data[x] = record;
      return false;
    } else {
      data[x].add_1(full_only);
      return true;
    }
  }

  int get_longest_context(sequence context, int pos, double time) {
    // Rcout << "get_longest_context...\n";
    int context_len = static_cast<int>(context.size());
    int upper_bound = std::min(order_bound, context_len);

    for (int order = upper_bound; order >= 0; order --) {
      // Rcout << "Checking order = " << order << "\n";
      sequence x = order == 0 ? sequence() : subseq(context,
                                         context_len - order,
                                         context_len - 1);
      // Rcout << "Truncated context = ";
      // print(x);
      // Skip this iteration if the context doesn't exist in the tree
      if (order > 0 && // we don't store 0-grams in the tree
          this->get_weight(x,
                           0, // pos - irrelevant for non-decay-based models
                           0, // time - irrelevant for non-decay-based models
                           false) // update exclusion
            == 0.0) {
        // Rcout << "Couldn't find context in the tree\n";
        continue;
      }
      // Skip this iteration if we can't find a continuation for that context
      bool any_continuation = false;
      x.resize(order + 1);
      for (int i = 0; i < this->alphabet_size; i ++) {
        x[order] = i;
        if (this->get_weight(x, 0, 0, false) > 0.0) {
          any_continuation = true;
          break;
        }
      }
      if (! any_continuation) {
        // Rcout << "Couldn't find any continuations for this context\n";
        continue;
      }
      // Rcout << "Couldn't find a problem with this context\n";
      return(order);
    }
    // Rcout << "Escaped to order = -1\n";
    return(- 1);
  }

  double get_weight(const sequence &n_gram,
                    int pos,
                    double time,
                    bool update_excluded) {
    return static_cast<double>(this->get_count(n_gram, update_excluded));
  };

  long int get_count(const sequence &x, bool update_excluded) {
    std::unordered_map<sequence, record_simple, container_hash<sequence>>::const_iterator target = data.find(x);
    if (target == data.end()) {
      return(0);
    } else if (update_excluded) {
      return target->second.up_ex_count;
    } else {
      return target->second.full_count;
    }
  }

  // The need to capture situations where the context_count is 0 is
  // introduced by Pearce (2005)'s decision to introduce exclusion
  // (see 6.2.3.3), though the thesis does not mention
  // this explicitly.
  double get_lambda(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    if (this->debug_smooth) {
      Rcout << "calling ppm_simple.get_lambda()\n";
    }
    std::string e = this->escape;
    if (context_count <= 0.0) {
      return 0.0;
    } else if (e == "a") {
      return this->lambda_a(counts, context_count, num_distinct_symbols);
    } else if (e == "b") {
      return this->lambda_b(counts, context_count, num_distinct_symbols);
    } else if (e == "c") {
      return this->lambda_c(counts, context_count, num_distinct_symbols);
    } else if (e == "d") {
      return this->lambda_d(counts, context_count, num_distinct_symbols);
    } else if (e == "ax") {
      return this->lambda_ax(counts, context_count, num_distinct_symbols);
    } else {
      stop("unrecognised escape method");
    }
  }

  List as_list() {
    int n = data.size();
    List n_gram(n);
    NumericVector full_count(n); // NumericVector deals better with v long ints
    NumericVector up_ex_count(n);

    int i = 0;
    for(auto kv : data) {
      n_gram[i] = kv.first;
      full_count[i] = kv.second.full_count;
      up_ex_count[i] = kv.second.up_ex_count;
      i ++;
    }

    List x = List::create(Named("n_gram") = n_gram,
                          Named("full_count") = full_count,
                          Named("up_ex_count") = up_ex_count);
    return(x);
  }

  RObject as_tibble() {
    return(list_to_tibble(this->as_list()));
  }

};

class ppm_decay: public ppm {
public:
  std::unordered_map<sequence, record_decay, container_hash<sequence>> data;

  double buffer_length_time;
  int buffer_length_items;
  double buffer_weight;
  bool only_learn_from_buffer;
  bool only_predict_from_buffer;
  double stm_weight;
  double stm_duration;
  double stm_half_life; // computed from stm_weight, ltm_weight, and stm_duration
  double ltm_weight;
  double ltm_half_life;
  double ltm_asymptote;
  double noise;
  double noise_mean;
  bool disable_noise;
  int seed;
  bool debug_decay;

  std::mt19937 random_engine;
  std::normal_distribution<> noise_generator;
  // std::bernoulli_distribution noise_generator;
  // std::uniform_int_distribution<> alphabet_sampler;

  ppm_decay(
    int alphabet_size_,
    int order_bound_,
    List decay_par,
    int seed,
    CharacterVector alphabet_levels_
  ) : ppm (
      alphabet_size_,
      order_bound_,
      false, // shortest_deterministic
      false, // exclusion
      false, // update_exclusion
      "a", // escape,
      true, // decay
      false, // sub_n_from_m1_dist
      false, // lambda_uses_zero_weight_symbols
      alphabet_levels_
  ) {
    data = {};
    buffer_length_time = decay_par["buffer_length_time"];
    buffer_length_items = decay_par["buffer_length_items"];
    buffer_weight = decay_par["buffer_weight"];
    only_learn_from_buffer = decay_par["only_learn_from_buffer"];
    only_predict_from_buffer = decay_par["only_predict_from_buffer"];
    stm_weight = decay_par["stm_weight"];
    stm_duration = decay_par["stm_duration"];
    ltm_weight = decay_par["ltm_weight"];
    ltm_half_life = decay_par["ltm_half_life"];
    ltm_asymptote = decay_par["ltm_asymptote"];
    noise = decay_par["noise"];
    disable_noise = false;
    debug_decay = false;

    stm_half_life = (log(2.0) * stm_duration) / (log(stm_weight / ltm_weight));

    // if (noise < 0.0) {
    //   stop("noise must be greater than or equal to zero");
    // }

    if (ltm_weight > stm_weight)
      stop("ltm_weight cannot be greater than stm_weight");

    if (ltm_weight <= 0.0)
      stop("ltm_weight must be positive");

    if (stm_weight <= 0.0)
      stop("stm_weight must be positive");

    if (stm_duration < 0)
      stop("stm_duration cannot be negative");

    if (ltm_half_life <= 0)
      stop("ltm_half_life must be positive");

    if (ltm_asymptote < 0)
      stop("ltm_asymptote must be non-negative");

    if (ltm_asymptote > ltm_weight)
      stop("ltm_asymptote cannot be greater than ltm_weight");

    if (escape != "a")
      stop("escape method must be 'a' for decay-based models");

    if (only_learn_from_buffer && buffer_length_items - 1 < order_bound)
      stop("if only_learn_from_buffer is TRUE, order bound cannot be greater than buffer_length_items - 1");

    noise_mean = noise * sqrt(2.0 / M_PI); // mean of abs(normal distribution)

    // std::random_device rd;
    std::mt19937 engine(seed);
    std::normal_distribution<> gen{0.0, noise};
    // std::bernoulli_distribution gen_bernoulli(noise);
    // std::uniform_int_distribution<int> gen_uniform_int(0, this->alphabet_size - 1);

    random_engine = engine;
    noise_generator = gen;
    // alphabet_sampler = gen_uniform_int;
  }

  ~ ppm_decay() {};

  bool insert(sequence x, int pos, double time, bool full_only) {
    // Rcout << "Original sequence: ";
    // print(x);
    // sequence noisy_x = x;
    // for (unsigned int i = 0; i < x.size(); i ++) {
    //   if (this->noise_generator(this->random_engine)) {
    //     noisy_x[i] = this->alphabet_sampler(this->random_engine);
    //   }
    // }
    // Rcout << "New sequence: ";
    // print(noisy_x);

    // Only insert the n-gram if it fit completely within the buffer.
    // Rcout << "pos_n_gram_begin = " << pos_n_gram_begin << "\n";

    if (this->only_learn_from_buffer) {
      // We skip the n-gram insertion if we find that the n-gram
      // doesn't fit in the buffer.
      // Note that we only have to check the temporal constraint;
      // the positional constraint was checked when the
      // model's order bound was originally specified.
      int n_gram_length = x.size();
      int pos_n_gram_begin = pos - n_gram_length + 1;
      double time_n_gram_begin = this->all_time.at(pos_n_gram_begin);
      if (time - time_n_gram_begin >= this->buffer_length_time)
        return true;
    }
    std::unordered_map<sequence,
                       record_decay,
                       container_hash<sequence>>::const_iterator target = data.find(x);
    if (target == data.end()) {
      record_decay record;
      record.insert(pos, time);
      data[x] = record;
      return false;
    } else {
      data[x].insert(pos, time);
      return true;
    }
  }

  int get_longest_context(sequence context, int pos, double time) {
    int max_context_size = context.size();
    if (max_context_size > this->order_bound) stop("this shouldn't happen (3)");
    if (this->only_predict_from_buffer) {
      for (int context_size = max_context_size;
           context_size > 0;
           context_size --) {
        // Rcout << "context_size = " << context_size << "\n";
        int pos_context_begin = pos - context_size;
        // Rcout << "pos_context_begin = " << pos_context_begin << "\n";
        // Rcout << "all_time = " << "\n";
        // print(this->all_time);
        double time_context_begin = this->all_time.at(pos_context_begin);
        // Rcout << "time_context_begin = " << time_context_begin << "\n";
        // Rcout << "pos = " << pos << "\n";
        // Rcout << "time = " << time << "\n";
        if (time - time_context_begin <= this->buffer_length_time)
          return context_size;
      }
      return 0;
    } else {
      return max_context_size;
    }
  }

  double get_weight(const sequence &n_gram,
                    int pos,
                    double time,
                    bool update_excluded) {
    record_decay data = this->get(n_gram);

    int N = static_cast<int>(this->all_time.size());
    int n = static_cast<int>(data.pos.size());

    double weight = 0.0;
    for (int i = 0; i < n; i ++) {
      if (this->debug_decay) {
        Rcout << "\n\nobserving from pos = " << pos << ", time = " << time << "\n";
        Rcout << "memory " << i << "/" << n << "\n";
      }

      if (data.pos[i] > pos) {
        stop("tried to predict using training data from the future");
      }
      if (data.pos[i] < 0) {
        stop("data.pos cannot be less than 0");
      }

      // Original buffer version
      // int pos_item_buffer_fails = data.pos[i] + this->buffer_length_items;
      // double temporal_buffer_fail_time = data.time[i] + this->buffer_length_time;

      int pos_item_buffer_fails = data.pos[i] +
        std::max(0, this->buffer_length_items - static_cast<int>(n_gram.size()) + 1);
      if (this->debug_decay) Rcout << "pos_item_buffer_fails = " << pos_item_buffer_fails << "\n";

      // Rcout << "pos = " << pos << ", N = " << N << "\n";

      bool item_buffer_failed = pos_item_buffer_fails <= N - 1; // <= pos;
      if (this->debug_decay) Rcout << "item_buffer_failed = " << item_buffer_failed << "\n";
      int pos_n_gram_began = data.pos[i] - static_cast<int>(n_gram.size()) + 1;

      double temporal_buffer_fail_time =
        this->all_time.at(pos_n_gram_began) + this->buffer_length_time;
      if (this->debug_decay) Rcout << "temporal_buffer_fail_time = " << temporal_buffer_fail_time << "\n";

      double buffer_fail_time;

      // Rcout << "temporal_buffer_fail_time = " << temporal_buffer_fail_time << "\n";
      // Rcout << "pos_item_buffer_fails = " << pos_item_buffer_fails << "\n";

      if (item_buffer_failed) {
        double time_when_item_buffer_failed = this->all_time.at(pos_item_buffer_fails);
        if (this->debug_decay) Rcout << "time_when_item_buffer_failed = " << time_when_item_buffer_failed << "\n";
        buffer_fail_time = std::min(time_when_item_buffer_failed,
                                    temporal_buffer_fail_time);
      } else {
        buffer_fail_time = temporal_buffer_fail_time;
      }

      double time_since_buffer_fail = time - buffer_fail_time;

      if (this->debug_decay) {
        Rcout << "buffer_fail_time = " << buffer_fail_time << "\n";
        Rcout << "time_since_buffer_fail = " << time_since_buffer_fail << "\n";
      }

      double weight_increment;

      if (time_since_buffer_fail < 0) {
        if (this->debug_decay) Rcout << "buffer didn't fail\n";
        weight_increment = this->buffer_weight;
      } else {
        if (this->debug_decay) Rcout << "buffer failed\n";
        weight_increment = this->decay_stm_ltm(time_since_buffer_fail);
      }

      if (this->debug_decay) Rcout << "weight_increment = " << weight_increment << "\n";

      weight += weight_increment;
    }

    if (!this->disable_noise) {
      // Rcout << "original weight = " << noise << "\n";
      double noise = fabs(this->noise_generator(this->random_engine));
      // Rcout << "noise = " << noise << "\n\n";
      weight += noise;
      // return std::max(0.0, weight);
    }

    if (this->debug_decay) {
      Rcout << "\ntotal weight = " << weight << "\n";
    }

    return weight;
  };

  double decay_stm_ltm(double elapsed_time) {
    bool stm = elapsed_time < this->stm_duration;
    double res;
    if (stm) {
      res =  decay_exp(this->stm_weight,
                       elapsed_time,
                       this->stm_half_life,
                       0.0);
    } else {
      res =  decay_exp(this->ltm_weight,
                       elapsed_time - this->stm_duration,
                       this->ltm_half_life,
                       this->ltm_asymptote);
    }
    // if (this->debug_decay) Rcout << "elapsed_time = " << elapsed_time << ", fraction = " << res << "\n";
    return res;
  }

  double decay_exp(double start,
                   double elapsed_time,
                   double half_life,
                   double asymptote) {
    if (half_life <= 0.0) stop("half life must be positive");
    return
      asymptote + (start - asymptote) * std::pow(2.0, - elapsed_time / half_life);
  }

  double get_lambda(const std::vector<double> &counts, double context_count, int num_distinct_symbols) {
    if (this->debug_smooth) {
      Rcout << "calling ppm_decay.get_lambda()\n";
    }
    double total_expected_noise;
    if (this->disable_noise) {
      total_expected_noise = 0.0;
    } else {
      total_expected_noise = this->noise_mean * this->alphabet_size;
    }
    double adj_context_count = std::max(context_count - total_expected_noise,
                                        0.0);

    // Rcout << "original context count = " << context_count << "\n";
    // Rcout << "total_expected_noise = " << total_expected_noise << "\n";
    // Rcout << "adj_context_count = " << adj_context_count << "\n\n";

    if (context_count <= 0.0) {
      if (this->debug_smooth) {
        // this should actually be adj_context_count, but does it matter?
        // no, because adj_context_count always <= context_count
        Rcout << "context_count <= 0.0 so lambda = 0.0\n";
      }
      return 0.0;
    } else {
      if (this->debug_smooth) {
        Rcout << "calling lambda_a...\n";
      }
      return this->lambda_a(counts, adj_context_count, -99); // last parameter ignored
    }
  }

  record_decay get(const sequence &x) {
    std::unordered_map<sequence,
                       record_decay,
                       container_hash<sequence>>::const_iterator target = data.find(x);
    if (target == data.end()) {
      record_decay blank;
      return(blank);
    } else {
      return(target->second);
    }
  }

  List as_list() {
    int n = data.size();
    List n_gram(n);
    List pos(n);
    List time(n);

    int i = 0;
    for(auto kv : data) {
      n_gram[i] = kv.first;
      pos[i] = kv.second.pos;
      time[i] = this->all_time[pos[i]];
      i ++;
    }

    List x = List::create(Named("n_gram") = n_gram,
                          Named("pos") = pos,
                          Named("time") = time);
    return(x);
  }

  RObject as_tibble() {
    return(list_to_tibble(this->as_list()));
  }

};

// ppm test_ppm() {
//   escape_a esc;
//   return ppm(10, // alphabet size
//              10, // order bound
//              true, //shortest deterministic
//              true, // exclusion
//              true, // update_exclusion
//              esc);
// }

RCPP_EXPOSED_CLASS(record_decay)
  RCPP_EXPOSED_CLASS(ppm)
  RCPP_EXPOSED_CLASS(ppm_simple)
  RCPP_EXPOSED_CLASS(ppm_decay)
  RCPP_EXPOSED_CLASS(symbol_prediction)
  RCPP_EXPOSED_CLASS(sequence_prediction)

  RCPP_MODULE(ppm) {
    class_<sequence_prediction>("sequence_prediction")
    .field("information_content", &sequence_prediction::information_content)
    .field("entropy", &sequence_prediction::entropy)
    .field("distribution", &sequence_prediction::distribution)
    .method("as_tibble", &sequence_prediction::as_tibble)
    ;

    class_<ppm>("ppm")
      // ppm class cannot be instantiated directly in R
      // .constructor<int, int, bool, bool, bool, std::string>()
         .field("alphabet_size", &ppm::alphabet_size)
         .field("order_bound", &ppm::order_bound)
         .field("shortest_deterministic", &ppm::shortest_deterministic)
         .field("exclusion", &ppm::exclusion)
         .field("update_exclusion", &ppm::update_exclusion)
         .field("escape", &ppm::escape)
         .field("all_time", &ppm::all_time)
         .field("sub_n_from_m1_dist", &ppm::sub_n_from_m1_dist)
         .field("lambda_uses_zero_weight_symbols", &ppm::lambda_uses_zero_weight_symbols)
         .field("debug_smooth", &ppm::debug_smooth)
         .field("alphabet_levels", &ppm::alphabet_levels)
         .method("model_seq", &ppm::model_seq)
      // .method("insert", &ppm::insert)
         .method("get_weight", &ppm::get_weight)
      ;

    class_<ppm_simple>("ppm_simple")
      .derives<ppm>("ppm")
      .constructor<int, int, bool, bool, bool, std::string, CharacterVector>()
      .method("get_count", &ppm_simple::get_count)
      .method("as_tibble", &ppm_simple::as_tibble)
    ;

    class_<ppm_decay>("ppm_decay")
      .derives<ppm>("ppm")
      .constructor<int, int, List, int, CharacterVector>()
      .method("get", &ppm_decay::get)
      .method("as_tibble", &ppm_decay::as_tibble)
      .method("as_list", &ppm_decay::as_list)
      .field("buffer_length_time", &ppm_decay::buffer_length_time)
      .field("buffer_length_items", &ppm_decay::buffer_length_items)
      .field("buffer_weight", &ppm_decay::buffer_weight)
      .field("only_learn_from_buffer", &ppm_decay::only_learn_from_buffer)
      .field("only_predict_from_buffer", &ppm_decay::only_predict_from_buffer)
      .field("stm_weight", &ppm_decay::stm_weight)
      .field("stm_duration", &ppm_decay::stm_duration)
      .field("stm_half_life", &ppm_decay::stm_half_life)
      .field("ltm_weight", &ppm_decay::ltm_weight)
      .field("ltm_half_life", &ppm_decay::ltm_half_life)
      .field("ltm_asymptote", &ppm_decay::ltm_asymptote)
      .field("noise", &ppm_decay::noise)
      .field("noise_mean", &ppm_decay::noise_mean)
      .field("disable_noise", &ppm_decay::disable_noise)
      .field("seed", &ppm_decay::seed)
      .field("debug_decay", &ppm_decay::debug_decay)
    ;

    class_<record_decay>("record_decay")
      .constructor()
      .field("pos", &record_decay::pos)
    // .field("time", &record_decay::   time)
       .method("insert", &record_decay::insert)
    ;
  }


/*ckwg +29
 * Copyright 2013-2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief Implementation of \link kwiver::vital::camera_intrinsics_
 *        camera_intrinsics_<T> \endlink class
 *        for \c T = { \c float, \c double }
 */

#include <vital/types/camera_intrinsics.h>
#include <vital/io/eigen_io.h>
#include <vital/math_constants.h>
#include <Eigen/Dense>

#include <cmath>
#include <iomanip>

namespace kwiver {
namespace vital {

/// Convert to a 3x3 calibration matrix
matrix_3x3d
camera_intrinsics
::as_matrix() const
{
  matrix_3x3d K;
  const double f = this->focal_length();
  const vector_2d pp = this->principal_point();
  K << f, this->skew(), pp.x(),
    0, f / this->aspect_ratio(), pp.y(),
    0, 0, 1;
  return K;
}

/// Map normalized image coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_2d& point ) const
{
  // apply radial and tangential distortion if coefficients are provided
  const vector_2d pt = this->distort( point );
  const vector_2d pp = this->principal_point();
  const double f = this->focal_length();

  return vector_2d( pt.x() * f + pt.y() * this->skew() + pp.x(),
                    pt.y() * f / this->aspect_ratio() + pp.y() );
}

/// Map a 3D point in camera coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_3d& norm_hpt ) const
{
  return this->map( vector_2d( norm_hpt[0] / norm_hpt[2],
                               norm_hpt[1] / norm_hpt[2] ) );
}

/// Unmap actual image coordinates back into normalized image coordinates
vector_2d
camera_intrinsics
::unmap( const vector_2d& pt ) const
{
  const double f = this->focal_length();
  const vector_2d p0 = pt - this->principal_point();
  const double y = p0.y() * this->aspect_ratio() / f;
  const double x = ( p0.x() - y * this->skew() ) / f;

  return this->undistort( vector_2d( x, y ) );
}

/// Check if a 3D point in camera coordinates can map into image coordinates
bool
camera_intrinsics
::is_map_valid(const vector_3d& norm_hpt) const
{
  return this->is_map_valid(vector_2d(norm_hpt[0] / norm_hpt[2],
                                      norm_hpt[1] / norm_hpt[2]));
}

namespace // anonymous namespace
{

/// Compute the radial distortion scaling
/** Distortion scaling is a function of the squared radius \p r2
 *  and the distortion parameters \p d
 */
template < typename T >
T
radial_distortion_scale( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T scale = T( 1 );

  if ( d.rows() > 0 )
  {
    scale += r2 * d[0];
    if ( d.rows() > 1 )
    {
      const T r4 = r2 * r2;
      scale += r4 * d[1];
      if ( d.rows() > 4 )
      {
        const T r6 = r2 * r4;
        scale += r6 * d[4];
        if ( d.rows() > 7 )
        {
          scale /= T( 1 ) + r2 * d[5] + r4 * d[6] + r6 * d[7];
        }
      }
    }
  }
  return scale;
}

/// Compute radial distortion as a scaling and offset
/** For a point \p pt and distortion coefficients \p d compute
 *  a scale and offset such that distortion can be applied as
 *  \code
 *    distorted_pt = pt * scale + offset;
 *  \endcode
 */
template < typename T >
void
distortion_scale_offset( const Eigen::Matrix< T, 2, 1 >& pt,
                         const Eigen::VectorXd& d,
                         T& scale, Eigen::Matrix< T, 2, 1 >& offset )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T r2 = x2 + y2;

  scale = radial_distortion_scale( r2, d );
  offset = Eigen::Matrix< T, 2, 1 > ( T( 0 ), T( 0 ) );
  if ( d.rows() > 3 )
  {
    const T two_xy = 2 * pt.x() * pt.y();
    offset = Eigen::Matrix< T, 2, 1 > ( d[2] * two_xy + d[3] * ( r2 + 2 * x2 ),
                                        d[3] * two_xy + d[2] * ( r2 + 2 * y2 ) );
  }
}

/// Compute the derivative of the radial distortion as a function of \p r2
template < typename T >
T
radial_distortion_deriv( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T deriv = T( 0 );

  if ( d.rows() > 0 )
  {
    deriv += d[0];
    if ( d.rows() > 1 )
    {
      deriv += 2 * d[1] * r2;
      if ( d.rows() > 4 )
      {
        const T r4 = r2 * r2;
        deriv += 3 * d[4] * r4;
        if ( d.rows() > 7 )
        {
          const T r6 = r4 * r2;
          const T a1 = T( 1 ) / ( d[5] * r2 + d[6] * r4 + d[7] * r6 + T( 1 ) );
          const T a2 = d[5] + 2 * d[6] * r2 + 3 * d[7] * r4;
          deriv -= a2 * a1 * ( d[0] * r2 + d[1] * r4 + d[4] * r6 + T( 1 ) );
          deriv *= a1;
        }
      }
    }
  }
  return deriv;
}

/// Compute the Jacobian of the distortion at a point
template < typename T >
Eigen::Matrix< T, 2, 2 >
distortion_jacobian( const Eigen::Matrix< T, 2, 1 >& pt,
                     const Eigen::VectorXd& d )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T xy = pt.x() * pt.y();
  const T r2 = x2 + y2;
  const T d_scale = 2 * radial_distortion_deriv( r2, d );
  const T scale = radial_distortion_scale( r2, d );
  Eigen::Matrix< T, 2, 2 > J;

  J << d_scale * x2 + scale, d_scale * xy,
    d_scale * xy, d_scale * y2 + scale;
  // add tangential distortion jacobian
  if ( d.rows() > 3 )
  {
    const T axy = 2 * ( d[2] * pt.x() + d[3] * pt.y() );
    const T ay = 2 * d[2] * pt.y();
    const T ax = 2 * d[3] * pt.x();
    J( 0, 0 ) += ay + 3 * ax;
    J( 0, 1 ) += axy;
    J( 1, 0 ) += axy;
    J( 0, 0 ) += 3 * ay + ax;
  }
  return J;
}

} // end anonymous namespace

/// Constructor - from a calibration matrix
simple_camera_intrinsics
::simple_camera_intrinsics( const matrix_3x3d& K,
                            const vector_t& d )
  : focal_length_( K( 0, 0 ) ),
  principal_point_( K( 0, 2 ), K( 1, 2 ) ),
  aspect_ratio_( K( 0, 0 ) / K( 1, 1 ) ),
  skew_( K( 0, 1 ) ),
  dist_coeffs_( d ),
  max_distort_radius_sq_(compute_max_distort_radius_sq())
{
}

/// Map normalized image coordinates into distorted coordinates
vector_2d
simple_camera_intrinsics
::distort( const vector_2d& norm_pt ) const
{
  double scale;
  vector_2d offset;

  distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
  return scale * norm_pt + offset;
}

/// Unnap distorted normalized coordinates into normalized coordinates
vector_2d
simple_camera_intrinsics
::undistort( const vector_2d& dist_pt ) const
{
  double scale;
  vector_2d offset, residual;
  vector_2d norm_pt = dist_pt;

  // iteratively solve for the undistorted point
  for ( unsigned int i = 0; i < 5; ++i )
  {
    distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
    // This is a Gauss-Newton update
    // an alternative is a fixed point iteration as used by OpenCV:
    //   norm_pt = (dist_pt - offset) / scale;
    // Gauss-Newton seems to have faster convergence
    matrix_2x2d J = distortion_jacobian( norm_pt, dist_coeffs_ );
    residual = norm_pt * scale + offset - dist_pt;
    // check the maximum absolution residual to test convergence
    if ( residual.cwiseAbs().maxCoeff() < 1e-12 )
    {
      break;
    }
    norm_pt -= J.ldlt().solve( residual );
  }
  return norm_pt;
}

/// Check if a normalized image coordinate can map into image coordinates
bool
simple_camera_intrinsics
::is_map_valid(const vector_2d& norm_pt) const
{
  return norm_pt.squaredNorm() < this->max_distort_radius_sq_;
}

/// Compute the maximum distortion radius from dist_coeffs_
double
simple_camera_intrinsics
::compute_max_distort_radius_sq() const
{
  double a = 0.0;
  double b = 0.0;
  double c = 0.0;
  if (dist_coeffs_.rows() > 0)
  {
    a = dist_coeffs_[0];
    if (dist_coeffs_.rows() > 1)
    {
      b = dist_coeffs_[1];
      if (dist_coeffs_.rows() > 4)
      {
        c = dist_coeffs_[4];
        if (dist_coeffs_.rows() > 5)
        {
          // the rational polynomial case is not yet handled
          // TODO: implement something or throw a warning
        }
      }
    }
  }
  return max_distort_radius_sq(a, b, c);
}

/// Compute the maximum radius for radial distortion given coefficients
double
simple_camera_intrinsics
::max_distort_radius_sq(double a, double b, double c)
{
  constexpr double inf = std::numeric_limits<double>::infinity();
  using kwiver::vital::pi;
  // this function finds the smallest positive root of
  //   (1 + a r^2 + b r^4 + c r^6) r
  // the derivative with respect to r is
  //   1 + 3 a r^2 + 5 b r^4 + 7 c r^6
  // substituting x = r^2 we can solve this cubic polynomial
  //   1 + 3 a x + 5 b x^2 + 7 c r^3
  // taking the square root of the smallest positive x gives r

  // fold the constants from the derivative into a, b, and c
  a *= 3;
  b *= 5;
  c *= 7;

  // if all non-negative there is no root
  if (a >= 0.0 && b >= 0.0 && c >= 0.0)
  {
    return inf;
  }
  // general solution for non-zero cubic term
  if (c != 0.0)
  {
    // an array for the three possible solutions
    double solns[3] = { inf, inf, inf };
    // precompute commonly used terms (boc is "b over c")
    double boc = b / c;
    double boc2 = boc * boc;
    double t1 = (9 * a * boc - 2 * b * boc2 - 27) / c;
    double t2 = 3 * a / c - boc2;
    double discrim = t1 * t1 + 4 * t2 * t2 * t2;
    if (discrim > 0.0)
    {
      discrim = std::cbrt((std::sqrt(discrim) + t1) / 2.0);
      solns[0] = (discrim - (t2 / discrim) - boc) / 3;
    }
    else
    {
      double theta = (std::atan2(std::sqrt(-discrim), t1)) / 3;
      constexpr double twothirdpi = 2.0 * pi / 3.0;
      // by construction, if discrim < 0 then t2 < 0, so the sqrt is safe
      double t3 = 2 * std::sqrt(-t2);
      solns[0] = (t3 * std::cos(theta) - boc) / 3;
      solns[1] = (t3 * std::cos(theta + twothirdpi) - boc) / 3;
      solns[2] = (t3 * std::cos(theta - twothirdpi) - boc) / 3;
    }
    // find the minimum positive solution
    double min_soln = inf;
    for (auto const& s : solns)
    {
      if (s > 0.0 && s < min_soln)
      {
        min_soln = s;
      }
    }
    return min_soln;
  }
  // simplified solution for the quadratic case
  else if (b != 0.0)
  {
    double discrim = a * a - 4 * b;
    // if less than zero both solutions are not real
    if (discrim >= 0.0)
    {
      // we only need the smaller of the two possible solutions
      // solutions must be either both positive or both negative
      discrim = std::sqrt(discrim) - a;
      // if less than zero both solutions are negative
      if (discrim > 0.0)
      {
        return 2.0 / discrim;
      }
    }
  }
  // simple linear case for b = c = 0
  else if (a < 0.0)
  {
    return 1.0 / -a;
  }
  return inf;
}

/// output stream operator for a base class camera_intrinsics
std::ostream&
operator<<( std::ostream& s, const camera_intrinsics& k )
{
  using std::setprecision;
  std::vector<double> d = k.dist_coeffs();
  // if no distortion coefficients, create a zero entry as a place holder
  if ( d.empty() )
  {
    d.push_back(0.0);
  }
  s << setprecision( 12 ) << k.as_matrix() << "\n\n";
  for(unsigned i=0; i<d.size(); ++i)
  {
    s << setprecision( 12 ) << d[i] << " ";
  }
  s << "\n";

  return s;
}

/// input stream operator for a camera intrinsics
std::istream&
operator>>( std::istream& s, simple_camera_intrinsics& k )
{
  matrix_3x3d K;
  Eigen::VectorXd d;

  s >> K >> d;
  // a single 0 in d is used as a place holder,
  // if a single 0 was loaded then clear d
  if ( ( d.rows() == 1 ) && ( d[0] == 0.0 ) )
  {
    d.resize( 0 );
  }
  k = simple_camera_intrinsics( K, d );
  return s;
}

} } // end namespace


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_PIO_2LO_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_PIO_2LO_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio_2lo generic tag

     Represents the Pio_2lo constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Pio_2lo, double
                                , 0, 0xb33bbd2eUL
                                , 0x3c91a62633145c07ULL
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio_2lo, Site> dispatching_Pio_2lo(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio_2lo, Site>();
   }
   template<class... Args>
   struct impl_Pio_2lo;
  }
  /*!
    This constant is such that, for pairs of types (T, Tup)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_2lo<T>())+Tup(Pio_2<T>())-Pio_2<Tup>()) is  lesser than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    \f$\pi/2 + x\f$ with x small,  by replacing them by
    Pio_2 + (Pio_2lo + x)

    @par Semantic:

    For type T0:

    @code
    T r = Pio_2lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 6.123233995736766e-17
    else if T is float
      r = -4.3711388e-08
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio_2lo, Pio_2lo);
}

#endif


#include <cryptopp/eccrypto.h>
#include <cryptopp/ecpoint.h>
#include <cryptopp/asn.h>
#include <cryptopp/crc.h>
#include <cryptopp/oids.h>
#include <cryptopp/osrng.h>
#include <cryptopp/drbg.h>
#include <cryptopp/modes.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <set>
#include <thread>
#include <string>
#include <sstream>
#include <boost/tokenizer.hpp>

const CryptoPP::ECPPoint G(CryptoPP::Integer("362dc3caf8a0e8afd06f454a6da0cdce6e539bc3f15e79a15af8aa842d7e3ec2h"),
                           CryptoPP::Integer("b9f8addb295b0fd4d7c49a686eac7b34a9a11ed2d6d243ad065282dc13bce575h"));

const CryptoPP::ECPPoint H(CryptoPP::Integer("a3cf0a4b6e1d9146c73e9a82e4bfdc37ee1587bc2bf3b0c19cb159ae362e38beh"),
                           CryptoPP::Integer("db4369fabd3d770dd4c19d81ac69a1749963d69c687d7c4e12d186548b94cb2ah"));

std::mutex mut;

unsigned NUM_THREADS = 6;

uint32_t num_values = 256;
std::vector<std::vector<CryptoPP::ECPPoint>> testMatrix(num_values);

int main() {
    CryptoPP::AutoSeededRandomPool PRNG;
    CryptoPP::DL_GroupParameters_EC<CryptoPP::ECP> ec_group;
    ec_group.Initialize(CryptoPP::ASN1::secp256k1());

    auto start = std::chrono::high_resolution_clock::now();
    std::vector<CryptoPP::ECPPoint> test;
    test.reserve(10000);
    for(uint32_t i = 0; i < 8864; i++) {
        CryptoPP::Integer exponent(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::ECPPoint testPoint = ec_group.GetCurve().ScalarMultiply(G, exponent);
        test.push_back(std::move(testPoint));
    }
    auto finish = std::chrono::high_resolution_clock::now();

    std::chrono::duration<double> duration = finish - start;

    std::cout << "Duration: " << duration.count() << std::endl;
    /*
    for(;;) {
        CryptoPP::Integer d(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer r(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer z(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());

        CryptoPP::Integer w = r * z + d;
        w = w.Modulo(ec_group.GetSubgroupOrder());

        CryptoPP::Integer rz = r * z;
        rz = rz.Modulo(ec_group.GetSubgroupOrder());

        CryptoPP::ECPPoint wG = ec_group.GetCurve().ScalarMultiply(G, w);

        CryptoPP::ECPPoint rzG = ec_group.GetCurve().ScalarMultiply(G, rz);
        CryptoPP::ECPPoint dG = ec_group.GetCurve().ScalarMultiply(G, d);

        CryptoPP::ECPPoint rzG_dG = ec_group.GetCurve().Add(rzG, dG);

        std::cout << "wG" << std::endl;
        std::cout << std::hex << wG.x << std::endl;
        std::cout << std::hex << wG.y << std::endl;

        std::cout << "rzG + dG" << std::endl;
        std::cout << std::hex << rzG_dG.x << std::endl;
        std::cout << std::hex << rzG_dG.y << std::endl;

        if ((wG.x != rzG_dG.x) || (wG.y != rzG_dG.y))
            break;

        CryptoPP::Integer r_(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer x(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());

        CryptoPP::Integer rr_ = r + r_;

        CryptoPP::ECPPoint rG = ec_group.GetCurve().ScalarMultiply(G, r);
        CryptoPP::ECPPoint r_G = ec_group.GetCurve().ScalarMultiply(G, r_);

        CryptoPP::ECPPoint xH = ec_group.GetCurve().ScalarMultiply(H, x);

        CryptoPP::ECPPoint rr_G = ec_group.GetCurve().ScalarMultiply(G, rr_);

        CryptoPP::ECPPoint C = ec_group.GetCurve().Add(rG, xH);

        CryptoPP::ECPPoint C_ = ec_group.GetCurve().Add(rr_G, xH);

        CryptoPP::ECPPoint Cr_G = ec_group.GetCurve().Add(C, r_G);

        CryptoPP::ECPPoint CC_ = ec_group.GetCurve().Add(C_, ec_group.GetCurve().Inverse(C));

        std::cout << "C:" << std::endl;
        std::cout << std::hex << C.x << std::endl;
        std::cout << std::hex << C.y << std::endl;

        std::cout << "C_:" << std::endl;
        std::cout << std::hex << C_.x << std::endl;
        std::cout << std::hex << C_.y << std::endl;

        std::cout << "Cr_G:" << std::endl;
        std::cout << std::hex << Cr_G.x << std::endl;
        std::cout << std::hex << Cr_G.y << std::endl;

        std::cout << "r_G:" << std::endl;
        std::cout << std::hex << r_G.x << std::endl;
        std::cout << std::hex << r_G.y << std::endl;

        std::cout << "CC_:" << std::endl;
        std::cout << std::hex << CC_.x << std::endl;
        std::cout << std::hex << CC_.y << std::endl;

        if ((CC_.x != r_G.x) || (CC_.y != r_G.y))
            break;
    }
    */
    return 0;
}


/*
 * Copyright (c) 2016, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Michael X. Grey <mxgrey@gatech.edu>
 *
 * Georgia Tech Graphics Lab and AMBER Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Aaron Ames
 * <karenliu@cc.gatech.edu> <ames@gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef HUBO_UTILS_HPP
#define HUBO_UTILS_HPP

#include <Eigen/Geometry>

namespace hubo {

const double DefaultIsometryTolerance = 1e-6;
const double DefaultVectorTolerance = 1e-6;

//==============================================================================
static inline bool checkDist(Eigen::Vector3d& p, double a, double b)
{
  double d = p.norm();
  double dmax = a+b;
  double dmin = fabs(a-b);

  if (d > dmax)
  {
    p *= dmax/d;
    return false;
  }
  else if (d < dmin)
  {
    p *= dmin/d;
    return false;
  }
  else
  {
    return true;
  }
}

//==============================================================================
static inline void clamp_sincos(double& sincos, bool& valid)
{
  if (sincos < -1)
  {
    valid = false;
    sincos = -1;
  }
  else if (sincos > 1)
  {
    valid = false;
    sincos = 1;
  }
}

//==============================================================================
static inline Eigen::Vector3d flipEuler3Axis(const Eigen::Vector3d& u)
{
  Eigen::Vector3d v;
  v[0] = u[0] - M_PI;
  v[1] = M_PI - u[1];
  v[2] = u[2] - M_PI;
  return v;
}

//==============================================================================
inline double diff(const Eigen::Isometry3d& tf0,
                   const Eigen::Isometry3d& tf1)
{
  double cost = (tf0.translation() - tf1.translation()).norm();
  Eigen::AngleAxisd aa(tf0.linear().transpose()*tf1.linear());
  cost += aa.angle();

  return cost;
}

//==============================================================================
inline bool equal(const Eigen::Isometry3d& tf0,
                  const Eigen::Isometry3d& tf1,
                  const double tolerance = DefaultIsometryTolerance)
{
  return (diff(tf0, tf1) < tolerance);
}

//==============================================================================
inline bool equal(const Eigen::VectorXd& v0,
                  const Eigen::VectorXd& v1,
                  const double tolerance = DefaultVectorTolerance)
{
  return ((v1-v0).norm() < tolerance);
}

//==============================================================================
inline Eigen::AngleAxisd findRotation(
    const Eigen::Vector3d& x1, const Eigen::Vector3d& x0)
{
  Eigen::Vector3d N = x1.cross(x0);
  double s = N.norm();
  double theta = asin(s);
  // Deal with numerical imprecision issues
  if(s < -1.1 || 1.1 < s)
  {
    std::cout << "[findRotation] Strange value for s: " << s
              << " | Results in theta: " << theta << std::endl;
    return Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
  }
  else if(1.0 <= s)
  {
    theta = M_PI/2.0;
  }
  else if(s <= -1.0)
  {
    theta = -M_PI/2.0;
  }

  N.normalize();

  const double wrap = x1.dot(x0);
  if(wrap < 0)
    theta = M_PI - theta;

  if(std::abs(theta-M_PI) < 1e-4
     || std::abs(theta+M_PI) < 1e-4)
    return Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());

  return Eigen::AngleAxisd(theta, N);
}

} // namespace hubo

#endif // HUBO_UTILS_HPP