// TopicModelLocalStepCPPX.cpp
// Define this symbol to enable runtime tests for allocations
#define EIGEN_RUNTIME_NO_MALLOC

#include <math.h>
#include <time.h>
#include "Eigen/Dense"
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include "fastexp.h"
using namespace Eigen;
using namespace std;

// ======================================================== Declare funcs
// ======================================================== visible externally

extern "C" {
    void sparseLocalStepManyDocs_ActiveOnly(
        double* alphaEbeta_IN,
        double* Eloglik_IN,
        double* word_count_IN,
        int* word_id_IN,
        int* doc_range_IN,
        int nnzPerRow,
        int Nall,
        int K,
        int D,
        int V,
        int nCoordAscentIterLP,
        double convThrLP,
        int initProbsToEbeta,
        double* topicCount_OUT,
        double* spResp_data_OUT,
        int* spResp_colids_OUT,
        int* numIterVec_OUT,
        double* maxDiffVec_OUT,
        int numRestarts,
        int* rAcceptVec_IN,
        int* rTrialVec_IN,
        int REVISE_FIRST,
        int REVISE_EVERY,
        int verbose
    );
}

// ======================================================== Custom Type Defs
// ========================================================
// Simple names for array types
typedef Matrix<double, Dynamic, Dynamic, RowMajor> Mat2D_d;
typedef Matrix<double, 1, Dynamic, RowMajor> Mat1D_d;
typedef Array<double, Dynamic, Dynamic, RowMajor> Arr2D_d;
typedef Array<double, 1, Dynamic, RowMajor> Arr1D_d;
typedef Array<int, 1, Dynamic, RowMajor> Arr1D_i;

// Simple names for array types with externally allocated memory
typedef Map<Mat2D_d> ExtMat2D_d;
typedef Map<Mat1D_d> ExtMat1D_d;
typedef Map<Arr2D_d> ExtArr2D_d;
typedef Map<Arr1D_d> ExtArr1D_d;
typedef Map<Arr1D_i> ExtArr1D_i;

double calcElapsedTime(timespec start_time, timespec end_time) {
    double diffSec = (double) end_time.tv_sec - start_time.tv_sec;
    double diffNano = (double) end_time.tv_nsec - start_time.tv_nsec;
    return diffSec + diffNano / 1.0e9;
}

struct LessThanFor1DArray {
    const double* xptr;

    LessThanFor1DArray(const double * xptrIN) {
        xptr = xptrIN;
    }

    bool operator()(int i, int j) {
        return xptr[i] < xptr[j];
    }

};

struct GreaterThanFor1DArray {
    const double* xptr;

    GreaterThanFor1DArray(const double * xptrIN) {
        xptr = xptrIN;
    }

    bool operator()(int i, int j) {
        return xptr[i] > xptr[j];
    }
};

struct Argsortable1DArray {
    double* xptr;
    int* iptr;
    int size;

    // Constructor
    Argsortable1DArray(double* xptrIN, int sizeIN) {
        xptr = xptrIN;
        size = sizeIN;
        iptr = new int[size];
        resetIndices(size);
    }

    // Helper method: reset iptr array to 0, 1, ... K-1
    void resetIndices(int cursize) {
        assert(cursize <= size);
        for (int i = 0; i < cursize; i++) {
            iptr[i] = i;
        }
    }

    void pprint() {
        for (int i = 0; i < size; i++) {
            printf("%03d:% 05.2f ",
                this->iptr[i],
                this->xptr[this->iptr[i]]);
        }
        printf("\n");
    }

    void argsort() {
        this->argsort_AscendingOrder();
    }

    void argsort_AscendingOrder() {
        std::sort(
            this->iptr,
            this->iptr + this->size,
            LessThanFor1DArray(this->xptr)
            );
    }

    void argsort_DescendingOrder() {
        std::sort(
            this->iptr,
            this->iptr + this->size,
            GreaterThanFor1DArray(this->xptr)
            );
    }

    void findSmallestL(int L) {
        assert(L >= 0);
        assert(L < this->size);
        std::nth_element(
            this->iptr,
            this->iptr + L,
            this->iptr + this->size,
            LessThanFor1DArray(this->xptr)
            );
    }

    void findLargestL(int L, int Kactive) {
        assert(L >= 0);
        assert(L <= Kactive);
        std::nth_element(
            this->iptr,
            this->iptr + Kactive - L,
            this->iptr + Kactive,
            LessThanFor1DArray(this->xptr)
            );
    }
};

void precomputeTopLRespForEachVocabTerm(
        int nnzPerRow,
        int V,
        int K,
        ExtArr2D_d & Eloglik,
        ExtArr1D_d & alphaEbeta,
        Arr1D_d & ElogProb,
        Arr1D_d & logScores_n,
        Arr1D_d & tempScores_n,
        Arr1D_d & termResp_data,
        Arr1D_i & termResp_colids
        )
{
    ElogProb = alphaEbeta.log();
    for (int v = 0; v < V; v++) {
        int argmax_n = -1;
        double maxScore_n;
        double curScore;
        for (int k = 0; k < K; k++) {
            curScore = ElogProb(k) + Eloglik(v, k);
            if (k == 0 || curScore > maxScore_n) {
                maxScore_n = curScore;
                if (nnzPerRow == 1) {
                    argmax_n = k;
                }
            }
            logScores_n(k) = curScore;
        } // end loop over K
        if (nnzPerRow == 1) {
            termResp_data(v) = 1.0;
            termResp_colids(v) = argmax_n;
        } else {
            // Find the top L entries in logScores_n
            // Copy current row over into a temp buffer
            std::copy(
                logScores_n.data(),
                logScores_n.data() + K,
                tempScores_n.data());
            // Sort the data in the temp buffer (in place)
            std::nth_element(
                tempScores_n.data(),
                tempScores_n.data() + K - nnzPerRow,
                tempScores_n.data() + K);
            // Walk thru this row and find the top "nnzPerRow" positions
            double pivotScore = tempScores_n(K - nnzPerRow);
            double sumResp_n = 0.0;
            int termResp_start = v * nnzPerRow;
            int termResp_id = termResp_start;
            for (int k = 0; k < K; k++) {
                if (logScores_n(k) >= pivotScore) {
                    termResp_data(termResp_id) = \
                        fastexp(logScores_n(k) - maxScore_n);
                    termResp_colids(termResp_id) = k;
                    sumResp_n += termResp_data(termResp_id);
                    termResp_id += 1;
                }
            }
            assert(termResp_id - v * nnzPerRow == nnzPerRow);
            for (termResp_id = v * nnzPerRow;
                    termResp_id < (v+1) * nnzPerRow;
                    termResp_id++) {
                termResp_data(termResp_id) /= sumResp_n;
            }

        }
    }
}

int updateActiveSetForDoc(
        int d,
        ExtArr2D_d & topicCount,
        Arr1D_i & activeTopics_d,
        Arr1D_i & spareActiveTopics_d,
        double ACTIVE_THR,
        int prevKactive,
        int nnzPerRow
        )
{
    int newKactive = 0;
    int ia = 0; // spare inactive topics
    for (int a = 0; a < prevKactive; a++) {
        int k = activeTopics_d(a);
        if (topicCount(d, k) > ACTIVE_THR) {
            activeTopics_d(newKactive) = k;
            newKactive += 1;
        } else if (newKactive < nnzPerRow - ia) {
            spareActiveTopics_d(ia) = k;
            ia += 1;
        }
    }

    int Kactive = newKactive;
    while (Kactive < nnzPerRow) {
        int k = spareActiveTopics_d(Kactive - newKactive);
        activeTopics_d(Kactive) = k;
        Kactive++;
    }
    return Kactive;
}

/*
 * Initialize activeTopics_d from provided topicCount
 * All K possible topics are available.
 */
int updateActiveSetForDocFromScratch(
        int d,
        ExtArr2D_d & topicCount,
        Arr1D_i & activeTopics_d,
        Arr1D_i & spareActiveTopics_d,
        double ACTIVE_THR,
        int K,
        int nnzPerRow
        )
{
    int newKactive = 0;
    int ia = 0; // spare inactive topics
    for (int k = 0; k < K; k++) {
        if (topicCount(d, k) > ACTIVE_THR) {
            activeTopics_d(newKactive) = k;
            newKactive += 1;
        } else if (newKactive < nnzPerRow - ia) {
            spareActiveTopics_d(ia) = k;
            ia += 1;
        }
    }

    int Kactive = newKactive;
    while (Kactive < nnzPerRow) {
        int k = spareActiveTopics_d(Kactive - newKactive);
        activeTopics_d(Kactive) = k;
        Kactive++;
    }
    return Kactive;
}

double calcELBOForDoc(
    int d,
    ExtArr1D_d & alphaEbeta,
    ExtArr2D_d & topicCount,
    Arr1D_d & ElogProb_d,
    Arr1D_i & activeTopics_d,
    double totalLogSumResp,
    double sum_gammalnalphaEbeta,
    int Kactive
    )
{
    double ELBO = sum_gammalnalphaEbeta;
    for (int ka = 0; ka < Kactive; ka++) {
        int k = activeTopics_d(ka);
        ELBO += (
            boost::math::lgamma(topicCount(d, k) + alphaEbeta(k))
            - boost::math::lgamma(alphaEbeta(k))
            - topicCount(d, k) * ElogProb_d(k)
            );
    }
    ELBO += totalLogSumResp;
    return ELBO;
}

double updateAssignmentsForDoc_ReviseActiveSetDupOK(
    int d,
    int start_d,
    int N_d,
    int nnzPerRow,
    int Kactive,
    ExtArr1D_d alphaEbeta,
    ExtArr2D_d Eloglik,
    ExtArr1D_d word_count,
    ExtArr1D_i word_id,
    ExtArr2D_d & topicCount,
    ExtArr1D_d & spResp_data,
    ExtArr1D_i & spResp_colids,
    Arr1D_i & activeTopics_d,
    Arr1D_d & ElogProb_d,
    Arr1D_d & logScores_n,
    Argsortable1DArray & logScoresHandler,
    int initProbsToEbeta,
    int doTrackELBO
    )
{
    double totalLogSumResp = 0.0;

    // Update ElogProb_d for active topics
    if (initProbsToEbeta == 1) {
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            ElogProb_d(k) = log(alphaEbeta(k));
        }
    }  else {
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            ElogProb_d(k) = boost::math::digamma(
                topicCount(d, k) + alphaEbeta(k));
        }
    }
    // RESET topicCounts for doc d
    topicCount.row(d).fill(0);

    // Update Resp_d for active topics
    // UPDATE assignments, obeying sparsity constraint
    for (int n = start_d; n < start_d + N_d; n++) {
        int spRind_dn_start = n * nnzPerRow;
        double w_ct = word_count(n);
        int w_id = word_id(n);
        int argmax_n = 0;
        double maxScore_n;
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            logScores_n(ka) = ElogProb_d(k) + Eloglik(w_id, k);
            if (ka == 0 || logScores_n(ka) > maxScore_n) {
                maxScore_n = logScores_n(ka);
                if (nnzPerRow == 1) {
                    argmax_n = k;
                }
            }
        }
        if (nnzPerRow == 1) {
            spResp_data(spRind_dn_start) = 1.0;
            spResp_colids(spRind_dn_start) = argmax_n;
            topicCount(d, argmax_n) += w_ct;
            if (doTrackELBO) {
                totalLogSumResp += w_ct * maxScore_n;
            }
        } else {
            // Use handler's built-in findLargestL functionality
            logScoresHandler.resetIndices(Kactive);
            logScoresHandler.findLargestL(nnzPerRow, Kactive);

            // Read off the top L values into spR data structures
            int spRind_dn = spRind_dn_start;
            double sumResp_n = 0.0;
            for (int j = 0; j < nnzPerRow; j++) {
                int ka = logScoresHandler.iptr[Kactive - j - 1];
                spResp_data(spRind_dn) = fastexp(
                    logScoresHandler.xptr[ka] - maxScore_n);
                spResp_colids(spRind_dn) = activeTopics_d(ka);
                sumResp_n += spResp_data(spRind_dn);
                spRind_dn += 1;
            }
            for (spRind_dn = spRind_dn_start;
                    spRind_dn < spRind_dn_start + nnzPerRow; spRind_dn++) {
                spResp_data(spRind_dn) /= sumResp_n;
                topicCount(d, spResp_colids(spRind_dn)) += \
                    w_ct * spResp_data(spRind_dn);
            }
            if (doTrackELBO) {
                totalLogSumResp += w_ct * (maxScore_n + log(sumResp_n));
            }
        } // end if statement branch for nnz > 1
    } // end for loop over tokens in this doc
    return totalLogSumResp;
}

double updateAssignmentsForDoc_ReviseActiveSet(
    int d,
    int start_d,
    int N_d,
    int nnzPerRow,
    int Kactive,
    ExtArr1D_d alphaEbeta,
    ExtArr2D_d Eloglik,
    ExtArr1D_d word_count,
    ExtArr1D_i word_id,
    ExtArr2D_d & topicCount,
    ExtArr1D_d & spResp_data,
    ExtArr1D_i & spResp_colids,
    Arr1D_i & activeTopics_d,
    Arr1D_d & ElogProb_d,
    Arr1D_d & logScores_n,
    Arr1D_d & tempScores_n,
    int initProbsToEbeta,
    int doTrackELBO
    )
{
    double totalLogSumResp = 0.0;

    // Update ElogProb_d for active topics
    if (initProbsToEbeta == 1) {
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            ElogProb_d(k) = log(alphaEbeta(k));
        }
    }  else {
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            ElogProb_d(k) = boost::math::digamma(
                topicCount(d, k) + alphaEbeta(k));
        }
    }
    // RESET topicCounts for doc d
    topicCount.row(d).fill(0);

    // Update Resp_d for active topics
    // UPDATE assignments, obeying sparsity constraint
    for (int n = start_d; n < start_d + N_d; n++) {
        int spRind_dn_start = n * nnzPerRow;
        double w_ct = word_count(n);
        int w_id = word_id(n);
        int argmax_n = 0;
        double maxScore_n;
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            logScores_n(ka) = ElogProb_d(k) + Eloglik(w_id, k);
            if (ka == 0 || logScores_n(ka) > maxScore_n) {
                maxScore_n = logScores_n(ka);
                if (nnzPerRow == 1) {
                    argmax_n = k;
                }
            }
        }
        if (nnzPerRow == 1) {
            spResp_data(spRind_dn_start) = 1.0;
            spResp_colids(spRind_dn_start) = argmax_n;
            topicCount(d, argmax_n) += w_ct;
            if (doTrackELBO) {
                totalLogSumResp += w_ct * maxScore_n;
            }
        } else {
            // Find the top L entries in logScores_n
            // Copy current row over into a temp buffer
            std::copy(
                logScores_n.data(),
                logScores_n.data() + Kactive,
                tempScores_n.data());
            // Sort the data in the temp buffer (in place)
            std::nth_element(
                tempScores_n.data(),
                tempScores_n.data() + Kactive - nnzPerRow,
                tempScores_n.data() + Kactive);
            // Walk thru this row and find the top "nnzPerRow" positions
            double pivotScore = tempScores_n(Kactive - nnzPerRow);

            int spRind_dn = spRind_dn_start;
            double sumResp_n = 0.0;
            for (int ka = 0; ka < Kactive; ka++) {
                if (logScores_n(ka) >= pivotScore) {
                    spResp_data(spRind_dn) = \
                        fastexp(logScores_n(ka) - maxScore_n);
                    spResp_colids(spRind_dn) = activeTopics_d(ka);
                    sumResp_n += spResp_data(spRind_dn);
                    spRind_dn += 1;
                }
            }
            assert(spRind_dn - spRind_dn_start == nnzPerRow);
            for (spRind_dn = spRind_dn_start;
                    spRind_dn < spRind_dn_start + nnzPerRow; spRind_dn++) {
                spResp_data(spRind_dn) /= sumResp_n;
                topicCount(d, spResp_colids(spRind_dn)) += \
                    w_ct * spResp_data(spRind_dn);
            }
            if (doTrackELBO) {
                totalLogSumResp += w_ct * (maxScore_n + log(sumResp_n));
            }
        } // end if statement branch for nnz > 1
    } // end for loop over tokens in this doc
    return totalLogSumResp;
}

/* Update spResp_data values, keeping spResp_colids fixed.
 *
 */
void updateAssignmentsForDoc_FixPerTokenActiveSet(
    int d,
    int start_d,
    int N_d,
    int nnzPerRow,
    int Kactive,
    ExtArr1D_d alphaEbeta,
    ExtArr2D_d Eloglik,
    ExtArr1D_d word_count,
    ExtArr1D_i word_id,
    ExtArr2D_d & topicCount,
    ExtArr1D_d & spResp_data,
    ExtArr1D_i & spResp_colids,
    Arr1D_i & activeTopics_d,
    Arr1D_d & ElogProb_d,
    Arr1D_d & logScores_n
    )
{
    assert(nnzPerRow > 1);
    //timespec start_time, end_time;
    //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
    // Update ElogProb_d for active topics
    for (int ka = 0; ka < Kactive; ka++) {
        int k = activeTopics_d(ka);
        ElogProb_d(k) = boost::math::digamma(
            topicCount(d, k) + alphaEbeta(k));
    }
    //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
    //double timeSpent_Digamma = calcElapsedTime(start_time, end_time);

    // RESET topicCounts for doc d
    topicCount.row(d).fill(0);

    //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
    // Update Resp_d for active topics
    for (int n = start_d; n < start_d + N_d; n++) {
        int spRind_dn_start = n * nnzPerRow;
        double w_ct = word_count(n);
        int w_id = word_id(n);
        double maxScore_n;
        for (int ka = 0; ka < nnzPerRow; ka++) {
            int k = spResp_colids(spRind_dn_start + ka);
            logScores_n(ka) = ElogProb_d(k) + Eloglik(w_id, k);
            if (ka == 0 || logScores_n(ka) > maxScore_n) {
                maxScore_n = logScores_n(ka);
            }
        }

        double sumResp_n = 0.0;
        for (int ka = 0; ka < nnzPerRow; ka++) {
            spResp_data(spRind_dn_start + ka) = \
                fastexp(logScores_n(ka) - maxScore_n);
            sumResp_n += spResp_data(spRind_dn_start + ka);
        }

        for (int ka = 0; ka < nnzPerRow; ka++) {
            spResp_data(spRind_dn_start + ka) /= sumResp_n;
            topicCount(d, spResp_colids(spRind_dn_start + ka)) += \
                w_ct * spResp_data(spRind_dn_start + ka);
        }

    } // end for loop over tokens in this doc
    /*clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
    double timeSpent_Resp = calcElapsedTime(start_time, end_time);
    logScores_n(0) = timeSpent_Digamma;
    logScores_n(1) = timeSpent_Resp;
    */
}

double tryRestartsForDoc(
    int d,
    int start_d,
    int N_d,
    int nnzPerRow,
    int Kactive,
    ExtArr1D_d alphaEbeta,
    ExtArr2D_d Eloglik,
    ExtArr1D_d word_count,
    ExtArr1D_i word_id,
    ExtArr2D_d & topicCount,
    ExtArr1D_d & spResp_data,
    ExtArr1D_i & spResp_colids,
    Arr1D_i & activeTopics_d,
    Arr1D_d & prevTopicCount_d,
    Arr1D_d & ElogProb_d,
    Arr1D_d & logScores_n,
    Argsortable1DArray & logScoresHandler,
    //Arr1D_d & tempScores_n,
    ExtArr1D_i & rAcceptVec,
    ExtArr1D_i & rTrialVec,
    int numRestarts,
    double sum_gammalnalphaEbeta,
    int verbose
    )
{
    // Find active topics eligible for sparse restart
    int nAccept = 0;
    int nTrial = 0;

    double CANDIDATE_THR = 1e-10;
    double curELBO = 0.0;
    double totalLogSumResp = 0.0;
    prevTopicCount_d.fill(0);
    if (verbose > 1 && d < 1) {
        printf("\nSPARSE RESTARTS at doc %d!!\n", d);
    }
    if (verbose > 1 && d < 1) {
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            printf(" %03d:%06.2f", k, topicCount(d,k));
        }
        printf("\n");
    }
    for (int riter = 0; riter < numRestarts; riter++) {
        // Seach for smallest topic we have yet to try
        double minVal = N_d + 1.0; // Will never be a value in topicCount
        int numAboveThr = 0;
        int chosenTopicID = -1;
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            if (topicCount(d, k) > CANDIDATE_THR) {
                numAboveThr += 1;
                if (topicCount(d, k) < minVal) {
                    minVal = topicCount(d, k);
                    chosenTopicID = k;
                }
            }
        }

        // Need at least two topics with non-neglible size to try restarting.
        // Otherwise, we should just quit
        if (numAboveThr <= 1 || chosenTopicID < 0) {
            break;
        }

        if (riter == 0) {
            totalLogSumResp = updateAssignmentsForDoc_ReviseActiveSetDupOK(
                d, start_d, N_d, nnzPerRow, Kactive,
                alphaEbeta, Eloglik, word_count, word_id,
                topicCount, spResp_data, spResp_colids,
                activeTopics_d, ElogProb_d, logScores_n, logScoresHandler,
                0, 1);
            // ELBO for current configuration
            curELBO = calcELBOForDoc(d, alphaEbeta, topicCount,
                ElogProb_d, activeTopics_d,
                totalLogSumResp, sum_gammalnalphaEbeta, Kactive);
            // Remember the best-known topic-count vector!
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                prevTopicCount_d(k) = topicCount(d, k);
            }
        }

        if (verbose > 1 && d < 1) {
            printf("START: best known counts. ELBO=%.5e \n", curELBO);
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                printf("%02d:%06.2f ", k, topicCount(d, k));
            }
            printf("\n");
        }

        // Force chosen topic to zero
        topicCount(d, chosenTopicID) = 0.0;
        if (verbose > 1 && d < 1) {
            printf(
                "RESTART: Set index %d to zero (%d left)\n",
                chosenTopicID, numAboveThr - 1);
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                printf("%02d:%06.2f ", k, topicCount(d, k));
            }
            printf("\n");
        }
        // Run inference forward from forced new location
        int NSTEP = 2;
        for (int step = 0; step < NSTEP; step++) {
            totalLogSumResp = updateAssignmentsForDoc_ReviseActiveSetDupOK(
                d, start_d, N_d, nnzPerRow, Kactive,
                alphaEbeta, Eloglik, word_count, word_id,
                topicCount, spResp_data, spResp_colids,
                activeTopics_d, ElogProb_d, logScores_n, logScoresHandler,
                0, step == NSTEP - 1);
        }
        // If the change is small, abandon current proposal
        double propELBO;
        if (abs(prevTopicCount_d(chosenTopicID) - topicCount(d, chosenTopicID)) < 1e-5) {
            propELBO = curELBO;
        } else {
            propELBO = calcELBOForDoc(d, alphaEbeta, topicCount,
                ElogProb_d, activeTopics_d,
                totalLogSumResp, sum_gammalnalphaEbeta, Kactive);
        }
        if (verbose > 1 && d < 1) {
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                printf("%02d:%06.2f ", k, topicCount(d, k));
            }
            printf("\n");
            printf("propELBO % .6e\n", propELBO);
            printf(" curELBO % .6e\n", curELBO);
            if (propELBO > curELBO) {
                printf("beforeCount: %.6f\n", prevTopicCount_d(chosenTopicID));
                printf(" afterCount: %.6f\n", topicCount(d, chosenTopicID));
                printf("gainELBO % .6e ACCEPTED \n", propELBO - curELBO);
            } else {
                printf("gainELBO % .6e rejected \n", propELBO - curELBO);
            }
        }

        // Reset threshold for which topic to choose next
        CANDIDATE_THR = prevTopicCount_d(chosenTopicID);

        // If accepted, set current best doc-topic counts to latest proposal
        // Otherwise, reset the starting point for the next proposal.
        if (propELBO > curELBO) {
            curELBO = propELBO;
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                prevTopicCount_d(k) = topicCount(d, k);
            }
            nAccept += 1;
        } else {
            for (int ka = 0; ka < Kactive; ka++) {
                int k = activeTopics_d(ka);
                topicCount(d, k) = prevTopicCount_d(k);
            }
        }
        nTrial += 1;

        if (verbose > 1 && d < 1) {
            printf("topicCount(d,k) = %.5e\n", topicCount(d,chosenTopicID));
            printf("prevTopicCount(d,k) = %.5e\n", prevTopicCount_d(chosenTopicID));
            printf("NEW THR = %.5e\n", CANDIDATE_THR);
        }

    } // end loop over restarts

    if (numRestarts > 0 && nTrial > 0) {
        rAcceptVec(0) += nAccept;
        rTrialVec(0) += nTrial;
        for (int ka = 0; ka < Kactive; ka++) {
            int k = activeTopics_d(ka);
            topicCount(d, k) = prevTopicCount_d(k);
        }
        // Final update! Make sure spResp reflects best topicCounts found
        updateAssignmentsForDoc_ReviseActiveSetDupOK(
            d, start_d, N_d, nnzPerRow, Kactive,
            alphaEbeta, Eloglik, word_count, word_id,
            topicCount, spResp_data, spResp_colids,
            activeTopics_d, ElogProb_d, logScores_n, logScoresHandler,
            0, 0);
    } // end if to synchronize at best known assignments

    return curELBO;
}

void sparseLocalStepManyDocs_ActiveOnly(
        double* alphaEbeta_IN,
        double* Eloglik_IN,
        double* word_count_IN,
        int* word_id_IN,
        int* doc_range_IN,
        int nnzPerRow,
        int Nall,
        int K,
        int D,
        int V,
        int nCoordAscentIterLP,
        double convThrLP,
        int initProbsToEbeta,
        double* topicCount_OUT,
        double* spResp_data_OUT,
        int* spResp_colids_OUT,
        int* numIterVec_OUT,
        double* maxDiffVec_OUT,
        int numRestarts,
        int* rAcceptVec_IN,
        int* rTrialVec_IN,
        int REVISE_FIRST,
        int REVISE_EVERY,
        int verbose
        )
{
    /*timespec start_time, end_time;
    double timeSpent_FindActive = 0.0;
    double timeSpent_Assign = 0.0;
    double timeSpent_AssignFixed = 0.0;
    double timeSpent_Restart = 0.0;
    double timeSpent_AssignFixed_Digamma = 0.0;
    double timeSpent_AssignFixed_Resp = 0.0;
    */
    nCoordAscentIterLP = max(nCoordAscentIterLP, 0);
    if (initProbsToEbeta == 1) {
        nCoordAscentIterLP += 1;
    }
    nCoordAscentIterLP = max(nCoordAscentIterLP, 1);

    if (nnzPerRow == 1) {
        REVISE_EVERY = 1;
    } else {
        REVISE_EVERY = max(REVISE_EVERY, 1);
    }
    REVISE_FIRST = max(REVISE_FIRST, 0);

    int CHECK_EVERY = 5; // Check convergence every X iterations.

    // Allocate temporary storage
    Arr1D_d ElogProb_d (K);
    Arr1D_d prevTopicCount_d (K);
    Arr1D_d logScores_n (K);
    Arr1D_d tempScores_n (K);
    Arr1D_i activeTopics_d (K);
    Arr1D_i spareActiveTopics_d (nnzPerRow);

    Arr1D_d termResp_data;
    Arr1D_i termResp_colids;

    Argsortable1DArray logScoresHandler = Argsortable1DArray(
        logScores_n.data(), K);

    if (initProbsToEbeta == 2) {
        termResp_data = Arr1D_d::Zero(V * nnzPerRow);
        termResp_colids = Arr1D_i::Zero(V * nnzPerRow);
    }

    // Disable all further memory allocation.
    // We do this to verify that we have no memory leaks!
    internal::set_is_malloc_allowed(false);

    // Unpack input arrays
    ExtArr1D_d alphaEbeta (alphaEbeta_IN, K);
    ExtArr2D_d Eloglik (Eloglik_IN, V, K);

    ExtArr1D_d word_count (word_count_IN, Nall);
    ExtArr1D_i word_id (word_id_IN, Nall);
    ExtArr1D_i doc_range (doc_range_IN, D+1);

    // Unpack output arrays
    ExtArr1D_d spResp_data (spResp_data_OUT, Nall * nnzPerRow);
    ExtArr1D_i spResp_colids (spResp_colids_OUT, Nall * nnzPerRow);
    ExtArr2D_d topicCount (topicCount_OUT, D, K);

    ExtArr1D_i numIterVec (numIterVec_OUT, D);
    ExtArr1D_d maxDiffVec (maxDiffVec_OUT, D);

    ExtArr1D_i rAcceptVec (rAcceptVec_IN, 1);
    ExtArr1D_i rTrialVec (rTrialVec_IN, 1);

    // Compute quantity used in sparse restart ELBO computation
    double sum_gammalnalphaEbeta = 0.0;
    if (numRestarts > 0) {
        for (int k = 0; k < K; k++) {
            sum_gammalnalphaEbeta += boost::math::lgamma(alphaEbeta(k));
        }
    }

    if (initProbsToEbeta == 2) {
        precomputeTopLRespForEachVocabTerm(
            nnzPerRow, V, K,
            Eloglik,
            alphaEbeta,
            ElogProb_d,
            logScores_n,
            tempScores_n,
            termResp_data,
            termResp_colids);
        if (verbose > 0) {
            printf("Precalculated Topics-by-term\n");
            for (int v = 0; v < 5; v++) {
                printf("term %06d ", v);
                for (int vid = v * nnzPerRow; vid < (v+1) * nnzPerRow; vid++) {
                    printf("%03d:%.2f ",
                        termResp_colids(vid), termResp_data(vid));
                }
                printf("\n");
            }
            for (int v = V-5; v < V; v++) {
                printf("term %06d ", v);
                for (int vid = v * nnzPerRow; vid < (v+1) * nnzPerRow; vid++) {
                    printf("%03d:%.2f ",
                        termResp_colids(vid), termResp_data(vid));
                }
                printf("\n");
            }
        }
    }

    // Visit each document and update its spResp (and corresponding topicCount)
    for (int d = 0; d < D; d++) {
        if (verbose == 2 && d < 1) {
            printf("\nSTANDARD INFERENCE AT DOC %d\n", d);
        }

        int start_d = doc_range(d);
        int N_d = doc_range(d+1) - doc_range(d);

        prevTopicCount_d.fill(0);
        double maxDiff = N_d;
        int iter = 0;
        double ACTIVE_THR = 1e-9;
        int doReviseActiveSet = 1;
        int Kactive = K;
        // Initialize activeTopics_d and topicCount_d
        if (initProbsToEbeta == 2) {
            // using the precomputed per-token resp values!
            for (int n = start_d; n < start_d + N_d; n++) {
                int wid = word_id(n);
                for (int a = wid * nnzPerRow; a < (wid+1) * nnzPerRow; a++) {
                    int k = termResp_colids(a);
                    topicCount(d, k) += word_count(n) * termResp_data(a);
                }
            }
            Kactive = updateActiveSetForDocFromScratch(
                d,
                topicCount,
                activeTopics_d,
                spareActiveTopics_d,
                ACTIVE_THR,
                K, nnzPerRow);
        } else if (initProbsToEbeta == 0) {
             Kactive = updateActiveSetForDocFromScratch(
                 d,
                 topicCount,
                 activeTopics_d,
                 spareActiveTopics_d,
                 ACTIVE_THR,
                 K, nnzPerRow);
        } else {
            // Initialize active set to ALL topics
            Kactive = K;
            for (int k = 0; k < K; k++) {
                activeTopics_d(k) = k;
            }
        }

        for (iter = 0; iter < nCoordAscentIterLP; iter++) {
            //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
            // DETERMINE CURRENT ACTIVE SET
            if (iter > 0 && nnzPerRow > 1 && Kactive <= nnzPerRow) {
                // Set of active docs is already as small as can be,
                // so nothing to gain from revising
                doReviseActiveSet = 0;
            } else if (iter < REVISE_FIRST || (iter - 1) % REVISE_EVERY == 0) {
                doReviseActiveSet = 1;
            } else {
                doReviseActiveSet = 0;
            }
            if (iter > 0 && doReviseActiveSet) {
                Kactive = updateActiveSetForDoc(
                    d,
                    topicCount,
                    activeTopics_d,
                    spareActiveTopics_d,
                    ACTIVE_THR,
                    Kactive, nnzPerRow);
            }
            assert(Kactive >= nnzPerRow);
            assert(Kactive <= K);
            //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
            //timeSpent_FindActive += calcElapsedTime(start_time, end_time);

            // COMPUTE ASSIGNMENTS FOR CURRENT ACTIVE SET
            if (doReviseActiveSet) {
                //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
                updateAssignmentsForDoc_ReviseActiveSetDupOK(
                    d, start_d, N_d, nnzPerRow, Kactive,
                    alphaEbeta,
                    Eloglik,
                    word_count,
                    word_id,
                    topicCount,
                    spResp_data,
                    spResp_colids,
                    activeTopics_d,
                    ElogProb_d,
                    logScores_n,
                    logScoresHandler,
                    (initProbsToEbeta == 1) && (iter == 0),
                    0);
                //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
                //timeSpent_Assign += calcElapsedTime(start_time, end_time);
            } else {
                //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
                updateAssignmentsForDoc_FixPerTokenActiveSet(
                    d, start_d, N_d, nnzPerRow, Kactive,
                    alphaEbeta,
                    Eloglik,
                    word_count,
                    word_id,
                    topicCount,
                    spResp_data,
                    spResp_colids,
                    activeTopics_d,
                    ElogProb_d,
                    logScores_n
                    );
                /*clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
                timeSpent_AssignFixed += calcElapsedTime(start_time, end_time);
                timeSpent_AssignFixed_Digamma += logScores_n(0);
                timeSpent_AssignFixed_Resp += logScores_n(1);
                */
            }

            // END ITERATION. Decide whether to quit early
            // Find maximum difference from previous doc-topic count
            if (iter % CHECK_EVERY == 0 && iter > 0) {
                double absDiff_k = 0.0;
                maxDiff = 0.0;
                for (int ka = 0; ka < Kactive; ka++) {
                    int k = activeTopics_d(ka);
                    absDiff_k = abs(prevTopicCount_d(k) - topicCount(d, k));
                    if (absDiff_k > maxDiff) {
                        maxDiff = absDiff_k;
                    }
                }
            }

            if (verbose == 2 && d < 1) {
                printf("d %3d iter %3d doRevise %d Kactive %4d maxDiff %11.6f\n",
                    d, iter, doReviseActiveSet, Kactive, maxDiff);
                printf("  ");
                for (int ka = 0; ka < Kactive; ka++) {
                    int k = activeTopics_d(ka);
                    printf("%02d:%06.2f ", k, topicCount(d,k));
                }
                printf("\n");
            }
            if (maxDiff <= convThrLP) {
                if (verbose > 2) {
                    printf("EARLY EXIT! maxDiff < %11.6f\n", convThrLP);
                }
                break;
            }

            if (iter % CHECK_EVERY == CHECK_EVERY - 1) {
                // Make sure prevTopicCount vector is updated
                for (int ka = 0; ka < Kactive; ka++) {
                    int k = activeTopics_d(ka);
                    prevTopicCount_d(k) = topicCount(d, k);
                }
            }

        } // end loop over iterations at doc d
        maxDiffVec(d) = maxDiff;
        numIterVec(d) = iter; // iter will already be +1'd by last iter of loop

        if (numRestarts > 0) {
            //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
            tryRestartsForDoc(
                d, start_d, N_d, nnzPerRow, Kactive,
                alphaEbeta,
                Eloglik,
                word_count,
                word_id,
                topicCount,
                spResp_data,
                spResp_colids,
                activeTopics_d,
                prevTopicCount_d,
                ElogProb_d,
                logScores_n,
                logScoresHandler,
                rAcceptVec,
                rTrialVec,
                numRestarts,
                sum_gammalnalphaEbeta,
                verbose
                );
           //clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &end_time);
           //timeSpent_Restart += calcElapsedTime(start_time, end_time);

        }
    } // end loop over documents
    internal::set_is_malloc_allowed(true);

    if (verbose > 0) {
        printf(
            "Sparse Restarts: %d/%d accepted\n", rAcceptVec(0), rTrialVec(0));
    }

    /*
    double timeSpent_total = \
        timeSpent_FindActive + timeSpent_Assign + \
        timeSpent_AssignFixed + timeSpent_Restart;

    printf("FindActive   %8.3f sec  %.3f %%\n", timeSpent_FindActive,
        timeSpent_FindActive / timeSpent_total);
    printf("Assign:      %8.3f sec  %.3f %%\n", timeSpent_Assign,
        timeSpent_Assign / timeSpent_total);
    printf("AssignFixed: %8.3f sec  %.3f %%\n", timeSpent_AssignFixed,
        timeSpent_AssignFixed / timeSpent_total);
    printf("  -Digamma:  %8.3f sec\n", timeSpent_AssignFixed_Digamma);
    printf("  -Resp:     %8.3f sec\n", timeSpent_AssignFixed_Resp);
    printf("Restarts:    %8.3f sec  %.3f %%\n", timeSpent_Restart,
        timeSpent_Restart / timeSpent_total);
    printf("TOTAL:       %8.3f sec\n", timeSpent_total);
    */
}


//==============================================================================
//
//   (c) Copyright, 2010 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//       Do not copy or distribute without authorization.
//
//       File: $RCSfile: lambertGrid.cc,v $
//       Version: $Revision: 1.6 $  Dated: $Date: 2012-01-06 19:57:45 $
//
//==============================================================================

/**
 * @file cloudGrid.cc
 *
 * Convert lat-longs cloud grid coordinates
 *
 * @date 3/25/10
 */

// Include files
#include <stdio.h>
#include <string>
#include <vector>
#include <projects.h>
#include <boost/format.hpp>
#include "LambertConfigReader.hh"
#include "Proj4Wrap.hh"

using boost::format;
using std::string;
using std::vector;

// Functions

void usage(char *programName)
{
  fprintf(stderr, "Usage: %s configFile\n", programName);
}

int main(int argc, char **argv)
{
  printf("argc %d\n", argc);

  if (argc != 2)
    {
      usage(argv[0]);
      exit(2);
    }

  LambertConfigReader cfg(argv[1]);
  if (cfg.error != string(""))
    {
      printf("Error: configuration file error %s\n", cfg.error.c_str());
      return 1;
    }

  // Uses latitude where lambert conformal projection is true and
  // median aligned with cartesian y-axis
  string paramString = str(format("+proj=lcc +R=6371200 +lon_0=%1% +lat_0=%2% +lat_1=%3% +lat_2=%4%") % cfg.lov % cfg.latin1 % cfg.latin1 % cfg.latin2);

  printf("paramString %s\n", paramString.c_str());
  p4w::Proj4Wrap lambertProj(paramString, p4w::Proj4Wrap::LON_LAT_TYPE, cfg.lo1, cfg.la1, cfg.dx, cfg.dy);
  double xc;
  double yc;
  double lon;
  double lat;

  lambertProj.ll2xy(cfg.lo1, cfg.la1, &xc, &yc);
  printf("lower corner for %g %g: xc, yc: %g %g\n", cfg.lo1, cfg.la1, xc, yc);
  lambertProj.ll2xy(-57.383, 55.481, &xc, &yc);
  printf("upper corner for -57.383, 55.481 : xc, yc: %g %g\n", xc, yc);
  lambertProj.xy2ll(0, 0, &lon, &lat);
  printf("0,0 lon, lat: %g %g\n", lon, lat);
  lambertProj.xy2ll(1, 0, &lon, &lat);
  printf("1,0 lon, lat: %g %g\n", lon, lat);
  lambertProj.xy2ll(2, 0, &lon, &lat);
  printf("2,0 lon, lat: %g %g\n", lon, lat);
  lambertProj.xy2ll(cfg.nx-1, cfg.ny-1, &lon, &lat);
  printf("%d, %d, lon, lat: %g %g\n", cfg.nx-1, cfg.ny-1, lon, lat);
  /*
  */

  vector<float> xvec;
  vector<float> yvec;

  for (int i=0; i<cfg.ny; i++)
    {
      for (int j=0; j<cfg.nx; j++)
	{
	  lat = cfg.la1 + i * cfg.dy;
	  lon = cfg.lo1 + j * cfg.dx;

	  lambertProj.ll2xy(lon, lat, &xc, &yc);
	  xvec.push_back(xc);
	  yvec.push_back(yc);
	}
    }

  // Print cdl header

  printf("netcdf coord {\n");
  printf("dimensions:\n");
  printf("y = %d ;\n", cfg.ny);
  printf("x = %d ;\n", cfg.nx);
  printf("variables:\n");
  printf("float x(y, x) ;\n");
  printf("x:long_name = \"x coordinate\";\n") ;
  printf("float y(y, x) ;\n");
  printf("y:long_name = \"y coordinate\";\n") ;
  printf("data:\n\n");

  printf("x = \n");
  for (uint i=0; i<xvec.size()-1; i++)
    {
      printf("%.7f,\n", xvec[i]);
    }

  printf("%.7f;\n", xvec[xvec.size()-1]);

  printf("y = \n");

  for (uint i=0; i<yvec.size()-1; i++)
    {
      printf("%.7f,\n", yvec[i]);
    }
  printf("%.7f;\n", yvec[yvec.size()-1]);

  printf("}\n");
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern example algorithm raster
// #include <numeric>
#include <boost/test/unit_test.hpp>
#include "fern/example/algorithm/raster.h"

BOOST_AUTO_TEST_CASE(constructor)
{
    double const cell_size = 5.0;
    size_t const nr_rows = 600;
    size_t const nr_cols = 400;
    example::Raster<int32_t> raster1(cell_size, nr_rows, nr_cols);
    // std::iota(raster1.values().begin(), raster1.values().end(), 0);
}


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, boost::property<vertex_id_t, std::size_t>, boost::property<edge_id_t, std::size_t> > Graph;
typedef boost::property<vertex_id_t, std::size_t> VertexId;
typedef boost::property<edge_id_t, std::size_t> EdgeID;


#include <stdexcept>
#include <algorithm>
#include <iostream>
#include <chrono>
#include <boost/math/special_functions/factorials.hpp>
#include "solver.h"
#include "debug.h"

namespace ms {

	/**
	 * Copies grid, all other members default initialize
	 **/
	solver::solver(const grid& start, grid::copy_type gct) :
		g(start, gct), regions(height(), width()) {	}

	/**
	 * Initializes the internal grid with the given parameters
	 **/
	solver::solver(unsigned int height, unsigned int width, unsigned int bombs) :
		g(height,width,bombs), regions(height, width) { }

	/**
	 * Copies all contents of solver, copies grid with the given copy type
	 **/
	solver::solver(const solver& copy, grid::copy_type gct) :
		g(copy.g, gct), regions(copy.regions) {}

	/**
	 * Find the areas around each number where there could be bombs.
	 * Such places must fit the following criteria:
	 *   - on the grid
	 *   - not not flagged or opened (so are hidden or questionmarked)
	 *   - adjacent to the given square
	 *
	 * Determine in each case how many bombs there are.
	 * This will be exact, so `region.min == region.max` for all regions generated.
	 *
	 * Store the results in `this.regions`
	 *
	 * Complexity \f$O(N)\f$ where \f$N\f$ is the number of cells in the grid
	 **/
	int solver::find_base_regions() {
		region remaining;

		if(regions_were_reset) {
			//all regions must be re-added
			for (unsigned r = 0; r < height(); ++r) {
				for(unsigned c = 0; c < width(); ++c) {
					modified_cells.insert(rc_coord(r,c));
				}
			}
		}

		for (rc_coord cell : modified_cells) {
			unsigned r = cell.row, c = cell.col;
			grid::cell gotten = get(r,c);
			if(gotten <= 8 && gotten > 0) { //grid handles zeroes automatically
				remaining.remove_safe(rc_coord(r,c));
				region reg;
				int num_flags = 0;

				for (int rr = -1; rr <= 1; ++rr)
					for (int cc = -1; cc <= 1; ++cc)
						if (!(rr == 0 && cc == 0) && g.iscontained(r + rr, c + cc)) {
							switch (get(r + rr, c + cc)) {
							case grid::ms_hidden:
							case grid::ms_question:
								reg.add_cell(rc_coord(r + rr, c + cc));
								break;
							case grid::ms_flag:
								++num_flags;
								break;
							default:
								break;
							}

						}
				if(!reg.empty()) {
					if(gotten < num_flags)
						throw bad_region_error("number of flags surrounding the cell exceeds the number of the cell");
					reg.set_count(gotten - num_flags);
					regions.add(reg);
				}
			}
		}
		modified_cells.clear();

		//this check is added to prevent massive slowdowns from adding remaining when it intersects around 100 cells -> 2000 cells
		//the choice of 10 is somewhat arbitrary, TODO get a better system for deciding when to add remaining
		if(g.count_unopened() < 10) {
			for (unsigned int r = 0; r < height(); ++r) {
				for (unsigned int c = 0; c < width(); ++c) {
					remaining.add_cell(rc_coord(r,c));
				}
			}
			remaining.set_count(bombs());
			for (unsigned int r = 0; r < height(); ++r) {
				for (unsigned int c = 0; c < width(); ++c) {
					grid::cell gotten = get(r,c);
					if(gotten == grid::ms_flag) {
						remaining.remove_bomb(rc_coord(r,c));
					} else if(gotten <= 8 && gotten >= 0) {
						remaining.remove_safe(rc_coord(r,c));
					}
				}
			}
			assert(remaining.size() < 10);
			regions.add(remaining);
		}

		return 0;
	}

	/**
	 * Find all regions that can be deduced from the existing regions.
	 *
	 * Calculate the intersection of each pair of regions
	 *   - if empty, discard it
	 *   - if nonempty, add the intersection and subraction to `this.aux_regions`
	 *
	 * Returns the number iterations, (zero if nothing to do)
	 *
	 * If `lazy == true` it stops as soon as a cells can be added to the queue
	 *
	 * Complexity of the inner loop \f$O(N^2)\f$ where N is the number of regions modified since last time the loop was reached
	 * Outer loop's complexity is nontrivial, runs until it has found aux regions. Often just one or zero iteration,
	 * but if there is nothing to be found, there are often around 10-20 (more when more regions).
	 **/
	int solver::find_aux_regions(bool lazy) {
		dbg::cout << "find_aux_regions [" <<  (lazy?"lazy":"nonlazy") << "]...";

		int iterations = 0;

		const region_set::subset_type& regions_added = regions.get_modified_regions();

		while (!regions_added.empty()) { //loops as long as something was added
			dbg::cout2 << "[" << regions_added.size() << "," << regions.size() << "]";

			if(lazy && fill_queue())
				break;

			std::vector<region> region_queue;

			for(auto ri = regions_added.begin(); ri != regions_added.end(); ++ri) {
				dbg::cout2 << ".";
				region_set::subset_type overlaps = regions.regions_intersecting(**ri);

				for(auto rj = overlaps.begin(); rj != overlaps.end(); ++rj) {
					if(*rj == *ri)
						continue;
					region_queue.push_back((*ri)->intersect(**rj));
					region_queue.push_back((*ri)->subtract(**rj));
					region_queue.push_back((*rj)->subtract(**ri));
				}
			}
			dbg::cout2 << "*";
			regions.reset_modified_regions();
			for(region& to_add : region_queue) {
				regions.add(to_add);
			}
			++iterations;
		}
		dbg::cout2 << "\n";
		dbg::cout << "done\n";
		return iterations;
	}

	int solver::find_regions() {
		find_base_regions();
		find_aux_regions(true);
		if(!(safe_queue.empty() && bomb_queue.empty()))
			return 0;

		return 1;
	}

	/**
	 * To be called when a safe cell is opened.
	 * Removes `cell` from all regions, so it can be forgotten about and free up memory
	 * and simplify future calculations
	 *
	 * returns nonzero if the safe cell is found, otherwise returns 0.
	 **/
	int solver::remove_safe(rc_coord cell) {
		int removed = regions.remove_safe(cell);

		auto it = safe_queue.find(cell);
		if(it != safe_queue.end())
			safe_queue.erase(it);
		assert(bomb_queue.find(cell) == bomb_queue.end());

		return removed;
	}

	/**
	 * to be called when a bomb cell is opened
	 * removes it from all regions, so it can be forgotten about and free up memory
	 * and simplify future calculations
	 *
	 * returns nonzero if the bomb cell is found, otherwise returns 0
	 **/
	int solver::remove_bomb(rc_coord cell) {
		int removed = regions.remove_bomb(cell);

		auto it = bomb_queue.find(cell);
		if(it != bomb_queue.end())
			bomb_queue.erase(it);
		assert(safe_queue.find(cell) == safe_queue.end());

		return removed;
	}

	rc_coord solver::get_safe_from_queue() const {
		return *safe_queue.begin();
	}

	rc_coord solver::get_bomb_from_queue() const {
		return *bomb_queue.begin();
	}

	/**
	 * Forces solver to flag a cell, and treat it as a bomb for all future calculations.
	 * This may result in errors and exceptions further on if it is incorrect.
	 *
	 * Returns zero if the cell is successfully flagged, nonzero otherwise.
	 **/
	int solver::manual_flag(rc_coord arg) {
		try {
			return apply_flag(arg);
		} catch (bad_region_error& bre) {
			reset_regions();
			return 1;
		}
	}

	/**
	 * Forces solver to open a cell.
	 *
	 * Returns the number of cells opened, or -1 on error
	 **/
	int solver::manual_open(rc_coord arg) {
		return apply_open(arg);
	}

	/**
	 * Unflags a cell, resetting the region_set.
	 *
	 * Slows down processing time considerably, but negligably from a user standpoint, so
	 * only used by a human user.
	 *
	 * Returns zero if the cell is successfully flagged, nonzero otherwise.
	 **/
	int solver::manual_unflag(rc_coord arg) {
		if(get(arg.row,arg.col) != grid::ms_flag)
			return 1;
		g.set_flag(arg.row,arg.col,grid::ms_hidden);
		reset_regions();
		return 0;
	}

	/**
	 * Flags a cell, and treat it as a bomb for all future calculations.
	 *
	 * Returns zero if the cell is successfully flagged
	 **/
	int solver::apply_flag(rc_coord arg) {
		g.set_flag(arg.row, arg.col, grid::ms_flag);

		if(get(arg.row, arg.col) == grid::ms_flag) {
			remove_bomb(arg);
			return 0;
		} else {
			return 1;
		}
	}

	/**
	 * Opens a cell and removes all instances of the cell from all regions
	 *
	 * Returns the number of cells opened, or -1 on error
	 **/
	int solver::apply_open(rc_coord arg) {
		std::unordered_set<rc_coord, rc_coord_hash> cells_opened = g.open(arg.row, arg.col);

		for (rc_coord cell : cells_opened) {
			remove_safe(cell);
			modified_cells.insert(cell);
		}

		return cells_opened.size();
	}

	/**
	 * Clears the region set and populates with base regions.
	 **/
	int solver::reset_regions() {
		regions.clear();
		regions_were_reset = true;
		return 0;
	}

	/**
	 * Finds all regions that guarantee that the contained cells are
	 * safe or bombs and adds them to the queues if they are not already.
	 **/
	int solver::fill_queue() {
		int num_added = 0;
		for(const region& check : regions) {
			if(check.size() == check.min()) { //implies size == max == min for all valid state regions
				for(rc_coord bomb : check) {
					num_added += add_to_bomb_queue(bomb);
				}
			} else if (check.max() == 0) {
				for(rc_coord safe : check) {
					num_added += add_to_safe_queue(safe);
				}
			}
		}
		return num_added;
	}

	/**
	 * Adds the input cell to the back of the bomb queue if it is not already present.
	 *
	 * Returns 1 if `to_add` is added, 0 if not.
	 **/
	int solver::add_to_bomb_queue(rc_coord to_add) {
		for(const rc_coord& check : bomb_queue) {
			if(to_add == check)
				return 0;
		}
		bomb_queue.insert(to_add);
		return 1;
	}

	/**
	 * Adds the input cell to the back of the safe queue if it is not already present.
	 *
	 * Returns 1 if `to_add` is added, 0 if not.
	 **/
	int solver::add_to_safe_queue(rc_coord to_add) {
		for(const rc_coord& check : safe_queue) {
			if(to_add == check)
				return 0;
		}
		safe_queue.insert(to_add);
		return 1;
	}

	region solver::approx_remain() const {
		region ret_base;
		for(unsigned r = 0; r < height(); ++r) {
			for(unsigned c = 0; c < width(); ++c) {
				ret_base.add_cell(rc_coord(r,c));
			}
		}
		ret_base.set_count(bombs());
		for(unsigned r = 0; r < height(); ++r) {
			for(unsigned c = 0; c < width(); ++c) {
				switch(get(r,c)) {
				case grid::ms_flag:
					ret_base.remove_bomb(rc_coord(r,c));
					break;
				case grid::ms_hidden:
				case grid::ms_question:
					break;
				default:
					ret_base.remove_safe(rc_coord(r,c));
				}
			}
		}
		std::vector<region_set::const_iterator> random_regions;
		for(region_set::const_iterator it = regions.cbegin(); it != regions.cend(); ++it) {
			random_regions.push_back(it);
		}
		//number of iterations arbitrary, the more iterations the closer to actual results you get
		region ret;
		for(int i = 0; i < 10; ++i) {
			region to_merge = ret_base;
			std::shuffle(random_regions.begin(), random_regions.end(), rng);
			for(region_set::iterator it : random_regions) {
				to_merge.subtract_to(*it);
			}
			if(ret.empty()) {
				ret = to_merge;
			} else {
				ret.merge_to(to_merge);
			}
		}
		return ret;
	}

	float solver::expected_payout(rc_coord cell) const {
		using boost::math::factorial;

		float payout[9] = { 0 };
		float permutations[9] = { 0 };
		region new_base;
		for(int r = -1; r <= 1; ++r) {
			for(int c = -1; c <= 1; ++c) {
				if((c != 0 || r != 0) && g.iscontained(cell.row + r, cell.col + c)) {
					grid::cell value = get(cell.row + r, cell.col + c);
					if(value == grid::ms_hidden || value == grid::ms_question)
						new_base.add_cell(rc_coord(cell.row + r, cell.col + c));
				}
			}
		}
		for(unsigned count = 0; count <= 8; ++count) {
			try {
				new_base.set_count(count);
				solver test(*this, grid::SURFACE_COPY);
				test.apply_open(cell);
				test.regions.add(new_base);
				test.find_aux_regions(false);
				payout[count] += test.safe_queue.size() + test.bomb_queue.size() / 1.5f; //opening safe cells is worth a bit more than flagging cells

				//restrict the set of possible locations of bombs around the openned cell
				//to get a better estimate of how likely a certain number is
				region final_base = new_base;
				final_base.set_count(count);
				for(rc_coord cell : new_base) {
					if(test.safe_queue.find(cell) != test.safe_queue.end()) {
						final_base.remove_safe(cell);
					} else if (test.bomb_queue.find(cell) != test.bomb_queue.end()) {
						final_base.remove_bomb(cell);
					}
				}
				permutations[count] = factorial<float>(final_base.size()) /
					(factorial<float>(final_base.size() - final_base.max()) * factorial<float>(final_base.max()));
			} catch (const bad_region_error& e) {
				payout[count] = 0;
				permutations[count] = 0;
			}
		}
		float num = 0;
		int den = 0;
		for(unsigned count = 0; count <= 8; ++count) {
			num += payout[count] * permutations[count];
			den += permutations[count];
		}
		if(den == 0) {
			return -1;
		} else {
			return num / den;
		}
	}

	/**
	 * Runs until the next step is not guaranteed to succeed.
	 *
	 * Returns the number of steps taken.
	 **/
	int solver::solve_certain() {
		int ret = 0;
		while(step_certain() != BAD_RC_COORD)
			++ret;
		return ret;
	}

	/**
	 * Opens/flags a cell if it is certain it will be correct.
	 *
	 * Returns the cell opened, or `BAD_RC_COORD` if none is opened
	 **/
	rc_coord solver::step_certain() {
		bool encountered_error = false;
		step_certain_start:
		try {
			if(g.gamestate() != grid::RUNNING) {
				return BAD_RC_COORD;
			}

			if(bomb_queue.empty() && safe_queue.empty()) {
				fill_queue();
				if(bomb_queue.empty() && safe_queue.empty()) {
					find_regions();
					fill_queue();
				}
			}

			if(!bomb_queue.empty()) {
				rc_coord ret = get_bomb_from_queue();
				apply_flag(ret);
				dbg::cout << "flagged: " << ret << "\n";
				return ret;
			} else if(!safe_queue.empty()) {
				rc_coord ret = get_safe_from_queue();
				if(get(ret.row,ret.col)!=grid::ms_hidden) {
					throw bad_region_error("Attempting to open a non-hidden cell");
				}
				int open_status = apply_open(ret);//removes ret
				if(!(open_status > 0)) {
					throw bad_region_error("Opened the wrong number of cells");
				}
				dbg::cout << "opened: " << ret << "\n";
				return ret;
			} else {
				return BAD_RC_COORD;
			}
		} catch (bad_region_error& bre) {
			if(encountered_error) {
				throw bad_region_error(std::string("unresolved error in step_certain: ") + bre.what());
			} else {
				g.clear_all_flags();
				reset_regions();
				encountered_error = true;
				goto step_certain_start;
			}
		}
	}

	std::mt19937 solver::rng(time(NULL));

	/**
	 * Runs until win or loss.
	 *
	 * Returns gamestate
	 **/
	int solver::solve() {
		int ret = 0;
		while(step() != BAD_RC_COORD)
			++ret;
		return ret;
	}

	/**
	 * Opens/flags a cell as long a game is running or ready to start running.
	 * Opens cells that are certain first before attempting to open a new cell.
	 *
	 * Returns the cell opened, or BAD_RC_COORD if none is opened
	 **/
	rc_coord solver::step() {
		dbg::cout2 << ">";
		if(g.gamestate() == grid::NEW) {
			std::uniform_int_distribution<> uid_row(0, height() - 1);
			std::uniform_int_distribution<> uid_col(0, width() - 1);
			rc_coord cell(uid_row(rng), uid_col(rng));
			apply_open(cell);
			return cell;
		} else if(g.gamestate() != grid::RUNNING) {
			return BAD_RC_COORD;
		}

		rc_coord ret = step_certain();
		if(ret != BAD_RC_COORD) {
			return ret;
		}

		region remain = approx_remain();

		std::vector<rc_coord> best_locs;
		float best_prob = 2; //higher than any real probability could be
		float default_prob = (remain.min() + remain.max()) / (2.f * remain.size()); //note that output could be infinite
		constexpr float threshhold = .001;

		for(unsigned row = 0; row < height(); ++row) {
			for(unsigned col = 0; col < width(); ++col) {
				if(get(row,col) == grid::ms_hidden || get(row,col) == grid::ms_question) {
					const region_set::subset_type& regions_at = regions.regions_intersecting(rc_coord(row, col));
					//select the smallest regions since they are most relevant
					std::vector<region_set::iterator> smallest_regions;
					for(region_set::iterator reg : regions_at) {
						if(smallest_regions.empty()) {
							smallest_regions.push_back(reg);
						} else if (reg->size() < smallest_regions.front()->size()) {
							smallest_regions.clear();
							smallest_regions.push_back(reg);
						} else if (reg->size() == smallest_regions.front()->size()) {
							smallest_regions.push_back(reg);
						}
					}
					float probability = 0;
					if(smallest_regions.empty()) {
						probability = default_prob;
					} else {
						for(region_set::iterator reg : smallest_regions) {
							probability +=  reg->min() + reg->max();
						}
						probability /= 2 * smallest_regions.size() * smallest_regions.front()->size();
					}

					if(fabs(probability - best_prob) < threshhold) { //close enough in probability
						best_locs.push_back(rc_coord(row,col));
					} else if(probability < best_prob) {
						best_locs.clear();
						best_locs.push_back(rc_coord(row,col));
						best_prob = probability;
					}
				}
			}
		}

		std::vector<rc_coord> payout_locs;
		float best_payout = 0;
		if(best_locs.size() > 1 && fabs(best_prob - default_prob) > threshhold) {
			for(rc_coord cell : best_locs) {
				float payout = expected_payout(cell);
				if(fabs(best_payout - payout) < threshhold) {
					payout_locs.push_back(cell);
				} else if (payout >= best_payout) {
					payout_locs.clear();
					payout_locs.push_back(cell);
					best_payout = payout;
				} else if (payout < 0) {
					apply_flag(cell);
					return cell;
				}
			}
		} else {
			payout_locs = best_locs;
		}

		if(!payout_locs.empty()) {
			std::uniform_int_distribution<> uid(0, payout_locs.size() - 1);
			ret = payout_locs[uid(rng)];
			apply_open(ret);
			return ret;
		}
		return BAD_RC_COORD;

	}

}


#define BOOST_TEST_MODULE FruitFallCalculatorTest

#include "FruitFallCalculator.hpp"
#include <vector>
#include <tuple>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(FruitFallCalculatorSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(ShouldCorrectlyCalculateNumberOfItems)
{
    // Arrange
    auto house_start_pos = 7;
    auto house_end_pos = 11;
    auto apple_tree_pos = 5;
    auto orange_tree_pos = 15;
    vector<int> apple_distances = { -2, 2, 1 };
    vector<int> orange_distances = { 5, -6 };

    FruitFallCalculator ffc(
      house_start_pos,
      house_end_pos,
      apple_tree_pos,
      orange_tree_pos);

    // Act
    auto tuple = ffc.getNumberOfFruitsFallenOnHouse(apple_distances, orange_distances);

    // Assert
    BOOST_CHECK_EQUAL(1, get<0>(tuple));
    BOOST_CHECK_EQUAL(1, get<1>(tuple));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2017 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// Endian-independent test

#include <boost/hash2/hash_append.hpp>
#include <boost/hash2/fnv1a.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/array.hpp>
#include <utility>
#if !defined(BOOST_NO_CXX11_HDR_ARRAY)
# include <array>
#endif
#if !defined(BOOST_NO_CXX11_HDR_TUPLE)
# include <tuple>
#endif

template<class H, class R> void test( R r )
{
    {
        unsigned char v[2] = { 1, 2 };

        H h;

        h.update( v, 2 );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        unsigned char v[2] = { 1, 2 };

        H h;

        hash_append( h, v[0] );
        hash_append( h, v[1] );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        unsigned char v[2] = { 1, 2 };

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        std::pair<unsigned char, unsigned char> v( 1, 2 );

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        boost::array<unsigned char, 2> v = { { 1, 2 } };

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

#if !defined(BOOST_NO_CXX11_HDR_ARRAY)

    {
        std::array<unsigned char, 2> v = { { 1, 2 } };

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

#endif

#if !defined(BOOST_NO_CXX11_HDR_TUPLE)

    {
        std::tuple<unsigned char, unsigned char> v( 1, 2 );

        H h;

        hash_append( h, v );

        BOOST_TEST_EQ( h.result(), r );
    }

#endif
}

int main()
{
    test<boost::hash2::fnv1a_32>( 3983810698ul );
    test<boost::hash2::fnv1a_64>( 589729691727335466ull );

    return boost::report_errors();
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//[ self_evaluation
#include <boost/yap/expression.hpp>

#include <boost/optional.hpp>
#include <boost/hana/fold.hpp>
#include <boost/hana/maximum.hpp>

#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>

// A super-basic matrix type, and a few associated operations.
struct matrix
{
    matrix() : values_(), rows_(0), cols_(0) {}

    matrix(int rows, int cols) : values_(rows * cols), rows_(rows), cols_(cols)
    {
        assert(0 < rows);
        assert(0 < cols);
    }

    int rows() const { return rows_; }
    int cols() const { return cols_; }

    double operator()(int r, int c) const
    { return values_[r * cols_ + c]; }
    double & operator()(int r, int c)
    { return values_[r * cols_ + c]; }

private:
    std::vector<double> values_;
    int rows_;
    int cols_;
};

matrix operator*(matrix const & lhs, double x)
{
    matrix retval = lhs;
    for (int i = 0; i < retval.rows(); ++i) {
        for (int j = 0; j < retval.cols(); ++j) {
            retval(i, j) *= x;
        }
    }
    return retval;
}
matrix operator*(double x, matrix const & lhs) { return lhs * x; }

matrix operator+(matrix const & lhs, matrix const & rhs)
{
    assert(lhs.rows() == rhs.rows());
    assert(lhs.cols() == rhs.cols());
    matrix retval = lhs;
    for (int i = 0; i < retval.rows(); ++i) {
        for (int j = 0; j < retval.cols(); ++j) {
            retval(i, j) += rhs(i, j);
        }
    }
    return retval;
}

// daxpy() means Double-precision AX Plus Y.  This crazy name comes from BLAS.
// It is more efficient than a naive implementation, because it does not
// create temporaries.  The covnention of using Y as an out-parameter comes
// from FORTRAN BLAS.
matrix & daxpy(double a, matrix const & x, matrix & y)
{
    assert(x.rows() == y.rows());
    assert(x.cols() == y.cols());
    for (int i = 0; i < y.rows(); ++i) {
        for (int j = 0; j < y.cols(); ++j) {
            y(i, j) += a * x(i, j);
        }
    }
    return y;
}

template <boost::yap::expr_kind Kind, typename Tuple>
struct self_evaluating_expr;

template <boost::yap::expr_kind Kind, typename Tuple>
auto evaluate_matrix_expr(self_evaluating_expr<Kind, Tuple> const & expr);

// This is the primary template for our expression template.  If you assign a
// self_evaluating_expr to a matrix, its conversion operator transforms and
// evaluates the expression with a call to evaluate_matrix_expr().
template <boost::yap::expr_kind Kind, typename Tuple>
struct self_evaluating_expr
{
    operator auto() const;

    static const boost::yap::expr_kind kind = Kind;

    Tuple elements;
};

// This is a specialization of our expression template for assignment
// expressions.  The destructor transforms and evaluates via a call to
// evaluate_matrix_expr(), and then assigns the result to the variable on the
// left side of the assignment.
//
// In a production implementation, you'd need to have specializations for
// plus_assign, minus_assign, etc.
template <typename Tuple>
struct self_evaluating_expr<boost::yap::expr_kind::assign, Tuple>
{
    ~self_evaluating_expr();

    static const boost::yap::expr_kind kind = boost::yap::expr_kind::assign;

    Tuple elements;
};

struct use_daxpy
{
    // A plus-expression, which may be of the form double * matrix + matrix,
    // or may be something else.  Since our daxpy() above requires a mutable
    // "y", we only need to match a mutable lvalue matrix reference here.
    template <typename Tuple>
    auto operator()(
        boost::yap::expr_tag<boost::yap::expr_kind::plus>,
        self_evaluating_expr<boost::yap::expr_kind::multiplies, Tuple> const & expr,
        matrix & m)
    {
        // Here, we transform the left-hand side into a pair if it's the
        // double * matrix operation we're looking for.  Otherwise, we just
        // get a copy of the left side expression.
        //
        // Note that this is a bit of a cheat, done for clarity.  If we pass a
        // larger expression that happens to contain a double * matrix
        // subexpression, that subexpression will be transformed into a tuple!
        // In production code, this transform should probably only be
        // performed on an expression with all terminal members.
        auto lhs = boost::yap::transform(
            expr,
            [](boost::yap::expr_tag<boost::yap::expr_kind::multiplies>,
               double d,
               matrix const & m) {
                return std::pair<double, matrix const &>(d, m);
            });

        // If we got back a copy of expr above, just re-construct the
        // expression this function mathes; in other words, do not effectively
        // transform anything.  Otherwise, replace the expression matched by
        // this function with a call to daxpy().
        if constexpr (boost::yap::is_expr<decltype(lhs)>::value) {
            return expr + m;
        } else {
            return boost::yap::make_terminal(daxpy)(lhs.first, lhs.second, m);
        }
    }
};

// This is the heart of what self_evaluating_expr does.  If we had other
// optimizations/transformations we wanted to do, we'd put them in this
// function, either before or after the use_daxpy transformation.
template <boost::yap::expr_kind Kind, typename Tuple>
auto evaluate_matrix_expr(self_evaluating_expr<Kind, Tuple> const & expr)
{
    auto daxpy_form = boost::yap::transform(expr, use_daxpy{});
    return boost::yap::evaluate(daxpy_form);
}

template<boost::yap::expr_kind Kind, typename Tuple>
self_evaluating_expr<Kind, Tuple>::operator auto() const
{
    return evaluate_matrix_expr(*this);
}

template<typename Tuple>
self_evaluating_expr<boost::yap::expr_kind::assign, Tuple>::
    ~self_evaluating_expr()
{
    using namespace boost::hana::literals;
    boost::yap::evaluate(elements[0_c]) = evaluate_matrix_expr(elements[1_c]);
}

// In order to define the = operator with the semantics we want, it's
// convenient to derive a terminal type from a terminal instantiation of
// self_evaluating_expr.  Note that we could have written a template
// specialization here instead -- either one would work.  That would of course
// have required more typing.
struct self_evaluating :
    self_evaluating_expr<
        boost::yap::expr_kind::terminal,
        boost::hana::tuple<matrix>
    >
{
    self_evaluating() {}

    explicit self_evaluating(matrix m)
    { elements = boost::hana::tuple<matrix>(std::move(m)); }

    BOOST_YAP_USER_ASSIGN_OPERATOR(self_evaluating_expr, ::self_evaluating_expr);
};

BOOST_YAP_USER_BINARY_OPERATOR(plus, self_evaluating_expr, self_evaluating_expr)
BOOST_YAP_USER_BINARY_OPERATOR(minus, self_evaluating_expr, self_evaluating_expr)
BOOST_YAP_USER_BINARY_OPERATOR(multiplies, self_evaluating_expr, self_evaluating_expr)

int main()
{
    matrix identity(2, 2);
    identity(0, 0) = 1.0;
    identity(1, 1) = 1.0;

    // These are YAP-ified terminal expressions.
    self_evaluating m1(identity);
    self_evaluating m2(identity);
    self_evaluating m3(identity);

    // This transforms the YAP expression to use daxpy(), so it creates no
    // temporaries.  The transform happens in the destructor of the
    // assignment-expression specialization of self_evaluating_expr.
    m1 = 3.0 * m2 + m3;

    // Same as above, except that it uses the matrix conversion operator on
    // the self_evaluating_expr primary template, because here we're assigning
    // a YAP expression to a non-YAP-ified matrix.
    matrix m_result_1 = 3.0 * m2 + m3;

    // Creates temporaries and does not use daxpy(), because the A * X + Y
    // pattern does not occur within the expression.
    matrix m_result_2 = 3.0 * m2;

    return 0;
}
//]


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward 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_SDK_META_SIZE_HPP_INCLUDED
#define NT2_SDK_META_SIZE_HPP_INCLUDED

#include <boost/mpl/bool.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/comparison.hpp>

namespace nt2 { namespace details
{
  template<class T1,class T2, class Tr, class Op>
  struct  size_check
        : boost::mpl::apply < Op
                            , boost::mpl::
                              sizeof_<typename boost::mpl::apply<Tr,T1>::type>
                            , boost::mpl::
                              sizeof_<typename boost::mpl::apply<Tr,T2>::type>
                            >
  {};
} }

namespace nt2 { namespace meta
{
    template<class T1,class T2,class Transform = boost::mpl::_>
    struct  has_same_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::equal_to< boost::mpl::_1
                                                      , boost::mpl::_2
                                                      >
                                >
    {};

    template<class T1,class T2,class Transform = boost::mpl::_>
    struct  has_different_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::not_equal_to< boost::mpl::_1
                                                          , boost::mpl::_2
                                                          >
                                >
    {};

    template<class T1,class T2,class Transform = boost::mpl::_>
    struct  has_smaller_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::less< boost::mpl::_1
                                                  , boost::mpl::_2
                                                  >
                                >
    {};

    template<class T1,class T2,class Transform = boost::mpl::_>
    struct has_larger_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::greater < boost::mpl::_1
                                                      , boost::mpl::_2
                                                      >
                                >
    {};

    template<class T1,class T2,class Transform = boost::mpl::_>
    struct has_smaller_or_equal_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::less_equal< boost::mpl::_1
                                                        , boost::mpl::_2
                                                        >
                                >
    {};

    template<class T1,class T2,class Transform = boost::mpl::_>
    struct has_larger_or_equal_size
          : details::size_check < T1
                                , T2
                                , Transform
                                , boost::mpl::greater_equal < boost::mpl::_1
                                                            , boost::mpl::_2
                                                            >
                                >
    {};

} }
#endif


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BA_INITIALIZE_HPP__
#define __OPTIMISATION2_BA_INITIALIZE_HPP__

#include "bas.hpp"
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/pair.hpp>

namespace lma
{
  namespace detail
  {
    template<class Bundle> struct ResizeInter
    {
      const Bundle& bundle;
      ResizeInter(const Bundle& bundle_):bundle(bundle_){}

      template<class Key1, class Key2, class Cont, template<class,class> class Pair> void operator()(Pair<Pair<Key1,Key2>,Cont>& obj) const
      {
        obj.second.resize(bundle.template indices<Key1,Key2>());
      }
    };

    template<class Bundle> struct ResizeInterInit
    {
      const Bundle& bundle;
      ResizeInterInit(const Bundle& bundle_):bundle(bundle_){}

      template<class Key1, class Key2, class Cont, template<class,class> class Pair> void operator()(Pair<Pair<Key1,Key2>,Cont>& obj) const
      {
        auto& indice = bundle.template indices<Key1,Key2>();
        auto& table = obj.second;

        table.indice.set_max(bundle.template at_opt<Key1>().size(),bundle.template at_opt<Key2>().size());

        for(auto i = indice.first() ; i < indice.size() ; ++i)
          for(auto j = indice.first(i) ; j < indice.size(i) ; ++j)
            table.indice.add(i,indice(i,j));
      }
    };

    template<class Bundle> struct ResizeDelta
    {
      const Bundle& bundle;
      ResizeDelta(const Bundle& bundle_):bundle(bundle_){}

      template<class Key, class Cont> void operator()(bf::pair<Key,Cont>& obj) const
      {
        obj.second.resize(bundle.template at_opt<Key>().size());
      }
    };

    struct SetZero
    {
      template<class Container> void operator()(Container& obj) const
      {
        obj.second.set_zero();
      }
    };
  }// eon

  template<class BDL, class BA> void initialize(const BDL& bundle, BA& ba)
  {
    using namespace detail;
    bf::for_each(ba.h,ResizeInter<BDL>(bundle));
    bf::for_each(ba.delta,ResizeDelta<BDL>(bundle));
    bf::for_each(ba.jte,ResizeDelta<BDL>(bundle));
  }

  template<class BA> void set_zero_(BA& ba)
  {
    using namespace detail;
    bf::for_each(ba.delta,SetZero());
  }

}// eon
#endif


#include "stdafx.h"
#include "camera.h"
#include "../core/logger.h"
#include <boost/math/constants/constants.hpp>
#include <tuple>

namespace overdrive {
	namespace scene {
		namespace {
			const float PI = boost::math::float_constants::pi;
		}

		Camera::Camera():
			mCameraMode(eMode::ORTHO),

			mPosition(0.0f, 0.0f, 0.0f),
			mOrientation(1.0f, 0.0f, 0.0f, 0.0f), // [NOTE] glm does WXYZ for quaternions...

			mWindow(nullptr),
			mViewportX(0),
			mViewportY(0),
			mViewportWidth(800),
			mViewportHeight(600),

			mFOVy(PI * 0.25f),
			mAspectRatio(16.0f / 9.0f), // for a while now this is the default for monitors (720p, 1080p, 4k etc are all 16:9)
			mNearClip(0.1f),
			mFarClip(100.0f)
		{
		}

		void Camera::setProjection(
			float fovY,
			float aspectRatio,
			float nearClip,
			float farClip
		) {
			mCameraMode = eMode::PERSPECTIVE;

			mFOVy = fovY;
			mAspectRatio = aspectRatio;
			mNearClip = nearClip;
			mFarClip = farClip;
		}

		void Camera::setOrtho() {
			mCameraMode = eMode::ORTHO;
		}

		void Camera::lookAt(const glm::vec3& pt) {
			glm::mat4 m = glm::lookAt(
				mPosition,
				pt,
				getUp() // re-use the current up orientation
			);

			// convert back into a new orientation (discarding position)
			mOrientation = glm::quat_cast(m);
		}

		void Camera::setPosition(const glm::vec3& pos) {
			mPosition = pos;
		}

		void Camera::setPosition(float x, float y, float z) {
			mPosition = glm::vec3(x, y, z);
		}

		const glm::vec3& Camera::getPosition() const {
			return mPosition;
		}

		void Camera::setOrientation(const glm::quat& q) {
			mOrientation = q;
		}

		const glm::quat& Camera::getOrientation() const {
			return mOrientation;
		}

		void Camera::setForward(const glm::vec3& fwd) {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix);

			transposed[2] = fwd;

			mOrientation = glm::quat_cast(
				glm::transpose(transposed)
			);
		}

		void Camera::setUp(const glm::vec3& up) {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix); // using a transposed matrix makes the expression somewhat neater

			transposed[1] = up;

			mOrientation = glm::quat_cast(
				glm::transpose(transposed)
			);
		}

		void Camera::setRight(const glm::vec3& right) {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix);

			transposed[0] = right;

			mOrientation = glm::quat_cast(
				glm::transpose(transposed)
			);
		}

		glm::vec3 Camera::getForward() const {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix);

			return transposed[2];
		}

		glm::vec3 Camera::getUp() const {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix);

			return transposed[1];
		}

		glm::vec3 Camera::getRight() const {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposed = glm::transpose(rotationMatrix);

			return transposed[0];
		}

		Camera::CameraAxes Camera::getDirections() const {
			auto rotationMatrix = glm::mat3_cast(mOrientation);
			auto transposedRotationMatrix = glm::transpose(rotationMatrix); // using a transposed matrix makes the expression somewhat neater

			return CameraAxes{
				transposedRotationMatrix[2],	// forward
				transposedRotationMatrix[1],	// up
				transposedRotationMatrix[0]	// right
			};
		}

		void Camera::setFOV(float radians) {
			mFOVy = radians;
		}

		void Camera::setAspect(float ratio) {
			mAspectRatio = ratio;
		}

		void Camera::setClip(float nearClip, float farClip) {
			mNearClip = nearClip;
			mFarClip = farClip;
		}

		void Camera::setViewport(int x, int y, int width, int height) {
			mViewportX = x;
			mViewportY = y;
			mViewportWidth = width;
			mViewportHeight = height;
		}

		void Camera::setViewportWindow(video::Window* window) {
			using std::tie;

			if (window) {
				mWindow = window;

				mViewportX = 0;
				mViewportY = 0;

				tie(mViewportWidth, mViewportHeight) = window->getFramebufferSize();

				mAspectRatio = static_cast<float>(mViewportWidth) / static_cast<float>(mViewportHeight);
			}
			else {
				mWindow = nullptr;

				mViewportX = 0;
				mViewportY = 0;
				mViewportWidth = 800;
				mViewportHeight = 600;
			}
		}

		Camera::eMode Camera::getMode() const {
			return mCameraMode;
		}

		const glm::mat4& Camera::getView() const {
			return mView;
		}

		const glm::mat4& Camera::getProjection() const {
			return mProjection;
		}

		void Camera::update() {
			// set viewport
			glViewport(
				mViewportX,
				mViewportY,
				mViewportWidth,
				mViewportHeight
			);

			// update view matrix
			mView = glm::mat4_cast(mOrientation) * glm::translate(-mPosition);

			// update projection matrix
			switch (mCameraMode) {
			case eMode::ORTHO:
				mProjection = glm::ortho(-1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f);
				break;

			case eMode::PERSPECTIVE:
				mProjection = glm::perspective(mFOVy, mAspectRatio, mNearClip, mFarClip);
				break;

			default:
				assert(false);
			}
		}

		void Camera::operator()(const video::Window::OnFramebufferResized& resized) {
			if (resized.mWindow == mWindow) {
				mViewportWidth = resized.mWidth;
				mViewportHeight = resized.mHeight;

				mAspectRatio = static_cast<float>(resized.mWidth) / static_cast<float>(resized.mHeight);
			}
		}

		std::ostream& operator << (std::ostream& os, const Camera::eMode& mode) {
			switch (mode) {
			case Camera::eMode::PERSPECTIVE:
				os << "Perspective";
				break;

			case Camera::eMode::ORTHO:
				os << "Ortho";
				break;

			default:
				os << "Unknown camera mode: " << static_cast<std::underlying_type_t<Camera::eMode>>(mode);
			}

			return os;
		}
	}
}


#ifndef __WIND_HH__
#define __WIND_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (wind model interface definition)
LIBRARY DEPENDENCY:
      ((../../src/env/wind.cpp))
*******************************************************************************/
#include <armadillo>
#include <aux.hh>

namespace cad
{
class Wind
{
    TRICK_INTERFACE(cad__Wind);

public:
    char name[256];

    Wind(double twind_In, double vertical_wind);

    virtual ~Wind() {}

    virtual void set_altitude(double altitude_in_meter);
    virtual void propagate_VAED(double int_step);
    virtual void apply_turbulance_if_have(double int_step, double dvba, arma::mat33 TBD, double alppx, double phipx);

    virtual double get_speed_of_wind() { return vwind; }
    virtual double get_direction_of_wind() { return psiwdx; }

    virtual arma::vec3 get_VAED() { return VAED; }
    virtual arma::vec3 get_VAEDS() { return VAEDS; }
    virtual arma::vec3 get_VAEDSD() { return VAEDSD; }

    virtual void enable_turbulance(double turb_length_In, double turb_sigma_In,
                                   double taux1_In, double taux1d_In,
                                   double taux2_In, double taux2d_In,
                                   double tau_In, double gauss_value_In)
    {
        has_turbulance = true;

        this->turb_length = turb_length_In;
        this->turb_sigma = turb_sigma_In;
        this->taux1 = taux1_In;
        this->taux1d = taux1d_In;
        this->taux2 = taux2_In;
        this->taux2d = taux2d_In;
        this->tau = tau_In;
        this->gauss_value = gauss_value_In;
    }
    virtual void disable_turbulance() { has_turbulance = false; }

protected:
    bool has_turbulance;

    double twind;    /* *o (s)          Wind smoothing time constant - sec */
    double altitude; /* *o (m)          Current Altitude */

    double vertical_wind_speed; /* *o (m/s)        Vertical air speed (pos.down) - m/s */

    double vwind;  /* *o (m/s)        Wind Speed */
    double psiwdx; /* *o (d)          Wind direction from north */

private:
    arma::vec VAED;  /* *o (m/s)        Smoothed wind velocity in geodetic coord */
    double _VAED[3]; /* *o (m/s)        Smoothed wind velocity in geodetic coord */

    arma::vec VAEDS;  /* *o (m/s)        Smoothed wind velocity in geodetic coord - m/s */
    double _VAEDS[3]; /* *o (m/s)        Smoothed wind velocity in geodetic coord - m/s */

    arma::vec VAEDSD;  /* *o (m/s)        Smoothed wind velocity derivative - m/s */
    double _VAEDSD[3]; /* *o (m/s)        Smoothed wind velocity derivative - m/s */

    double turb_length; /* *o (m)          Turbulence correlation length - m*/
    double turb_sigma;  /* *o (m/s)        Turbulence magnitude (1sigma) - m/s*/
    double taux1;       /* *o (--)         First turbulence state variable - ND*/
    double taux1d;      /* *o (--)         First turbulence state variable - ND*/
    double taux2;       /* *o (1/s)        First turbulence state variable derivative - 1/s*/
    double taux2d;      /* *o (1/s)        First turbulence state variable derivative - 1/s*/
    double tau;         /* *o (m/s)        Turblence velocity component in load factor plane - m/s*/
    double gauss_value; /* *o (--)         White Gaussian noise - ND*/
};
}  // namespace cad

#endif  // __WIND_HH__


#include <numpy_eigen/boost_python_headers.hpp>

#include <aslam/backend/BSplineMotionError.hpp>
#include <aslam/backend/BSplineMotionErrorFactory.hpp>
#include <aslam/splines/BSplineDesignVariable.hpp>
#include <aslam/splines/BSplinePoseDesignVariable.hpp>
#include <aslam/splines/EuclideanBSplineDesignVariable.hpp>
#include <boost/shared_ptr.hpp>

using namespace boost::python;
using namespace aslam::backend;

void exportBSplineMotionError() {
    def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::EuclideanBSplineDesignVariable>,
        "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
    def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplinePoseDesignVariable>,
        "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
    def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<1> >,
        "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
    def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<2> >,
        "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
    def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<3> >,
        "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");

    class_<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable>,
           boost::shared_ptr<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable> >, bases<ErrorTerm> >(
        "BSplineEuclideanMotionError", init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd>(
                                           "BSplineEuclideanMotionError(EuclideanBSplineDesignVariable, W)"))
        .def(init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd, unsigned int>(
            "BSplineGenericMotionError(EuclideanBSplineDesignVariable, W, errorTermOrder)"));
    using namespace aslam::splines;
    class_<BSplineMotionError<BSplineDesignVariable<1> >,
           boost::shared_ptr<BSplineMotionError<BSplineDesignVariable<1> > >, bases<ErrorTerm> >(
        "BSpline1MotionError",
        init<BSplineDesignVariable<1>*, Eigen::MatrixXd>("BSpline1MotionError(BSpline1DesignVariable, W)"))
        .def(init<aslam::splines::BSplineDesignVariable<1>*, Eigen::MatrixXd, unsigned int>(
            "BSpline1MotionError(BSpline1DesignVariable, W, errorTermOrder)"));
    class_<BSplineMotionError<BSplineDesignVariable<2> >,
           boost::shared_ptr<BSplineMotionError<BSplineDesignVariable<2> > >, bases<ErrorTerm> >(
        "BSpline2MotionError",
        init<BSplineDesignVariable<2>*, Eigen::MatrixXd>("BSpline2MotionError(BSpline2DesignVariable, W)"))
        .def(init<aslam::splines::BSplineDesignVariable<2>*, Eigen::MatrixXd, unsigned int>(
            "BSpline2MotionError(BSpline2DesignVariable, W, errorTermOrder)"));

    class_<BSplineMotionError<BSplineDesignVariable<3> >,
           boost::shared_ptr<BSplineMotionError<BSplineDesignVariable<3> > >, bases<ErrorTerm> >(
        "BSpline3MotionError",
        init<BSplineDesignVariable<3>*, Eigen::MatrixXd>("BSpline3MotionError(BSpline3DesignVariable, W)"))
        .def(init<aslam::splines::BSplineDesignVariable<3>*, Eigen::MatrixXd, unsigned int>(
            "BSpline3MotionError(BSpline3DesignVariable, W, errorTermOrder)"));

    class_<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable>,
           boost::shared_ptr<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable> >, bases<ErrorTerm> >(
        "BSplineMotionError", init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd>(
                                  "BSplineMotionError(BSplinePoseDesignVariable, W)"))
        .def(init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd, unsigned int>(
            "BSplineMotionError(BSplinePoseDesignVariable, W, errorTermOrder)"));
}


//==============================================================================
//         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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_VMX_FAST_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_VMX_FAST_RSQRT_HPP_INCLUDED
#if defined(BOOST_SIMD_HAS_VMX_SUPPORT)

#include <boost/simd/arithmetic/functions/fast_rsqrt.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_rsqrt_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , (scalar_< single_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(float a0) const
    {
      float r;
      __asm__ ("frsqrte %0, %1"
              : "=f" (r)
              : "f" (a0)
              );

      r *= ((3.0f - (r * r) * a0) * 0.5f);
      r *= ((3.0f - (r * r) * a0) * 0.5f);

      return r;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( fast_rsqrt_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , (scalar_< double_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(double a0) const
    {
      double r;
      __asm__ ("frsqrte %0, %1"
              : "=f" (r)
              : "f" (a0)
              );

      r *= ((3.0f - (r * r) * a0) * 0.5f);
      r *= ((3.0f - (r * r) * a0) * 0.5f);
      r *= ((3.0f - (r * r) * a0) * 0.5f);

      return r;
    }
  };
} } }

#endif

#endif