/* Copyright (C) 2013 Ion Torrent Systems, Inc. All Rights Reserved */

#include <iostream>
#include <algorithm>
#include "DfcCompr.h"
//#include "Utils.h"
#include <malloc.h>
#include <cmath>
// DFC doesn't use anything from the Eigen library
//#define EIGEN_USE_MKL_ALL 1
//#include <Eigen/Dense>
//#include <Eigen/LU>

using namespace std;
//using namespace Eigen;

/***
 The DFT engine needs to be kept in sync with any changes to the number of frames.
*/
int DfcCompr::SetNumFrames(int frames)
{
    if ((frames > 0) && (frames != n_frames))
    {
        n_frames = frames;
        DFT.SetPts(frames);
    }

    return n_frames;
}

/***
 Range check the new window length value and generate the new window coefficients.
 Window lengths should be relatively short.
*/
int DfcCompr::SetWindowLength(int length)
{
    if ((length >= 0) && (length < (n_frames/4)) && (length != winParamLen))
    {
        winParamLen = length;
        GenerateWindow();
    }

    return winParamLen;
}

/***
 Range check the new window alpha value and generat the new window coefficients.
*/
float DfcCompr::SetWindowAlpha(float alpha)
{
    if ((alpha >= 0.0f) && (alpha <= 3.0f) && (alpha != winParamAlpha))
    {
        winParamAlpha = alpha;
        GenerateWindow();
    }

    return winParamAlpha;
}

/***
 Generate a new set of Gaussian window coefficients based on the current set of
 window length and alpha configuration parameters.
*/
void DfcCompr::GenerateWindow()
{
    // setup vectors to store compression and decompression windows
    windowCompress.resize(winParamLen*2);
    windowExpand.resize(winParamLen*2);

    float halfN = static_cast<float>(winParamLen);
    float n     = 0.5f-halfN;  // starting at -(N-1)/2
    int   idx, maxIdx;
    float an_sq, val, oneOnVal;

    maxIdx = (winParamLen * 2) - 1;
    for (idx = 0; idx < winParamLen; ++idx)
    {
        an_sq    = winParamAlpha * (n / halfN);
        val      = exp((-0.5f) * (an_sq * an_sq));
        oneOnVal = 1.0f / val;
        n       += 1.0f;  // setup n for next loop iteration
        windowCompress[idx]        = val;
        windowCompress[maxIdx-idx] = val;  // exploit window symmetry
        windowExpand[idx]          = oneOnVal;
        windowExpand[maxIdx-idx]   = oneOnVal;
    }

}

/***
  Decompose the image data into a set of basis vectors and each well's projection
  onto them.
  @param n_wells - number of wells in the image patch
  @param n_frame - number of frames in this image patch.
  @param image - Same order as RawImage structure. Individual wells of data in frame, row, col major order so
                 value(row_i,col_j,frame_k) = image[row_i * ncol + col_j + (nrow * ncol * frame_k)]
  @param n_sample_wells - number of wells in the sample of the patch
  @param image_sample - sample of image above for vectors and such
  @param compressed - output of a lossy compressed patch
*/
void DfcCompr::LossyCompress(float *image)
{
    //////////////////////////////////////////
    // Temporary scratch_pad memory allocation
    if (scratch_pad == NULL) scratch_pad = new float[n_wells*n_basis*2];
    //////////////////////////////////////////

    int wellIdx, frameIdx;
    float tmpWell[n_frames];
    register float tmpSum;
    register int offset, winIdx;
    float * __restrict dftPtrI = &scratch_pad[0];
    float * __restrict dftPtrQ = &scratch_pad[n_wells*n_basis];
    float deltaMagI[n_basis];
    float deltaMagQ[n_basis];

    // Initialize keyframe
    for (frameIdx = 0; frameIdx < n_basis; ++frameIdx)
    {
        keyFrameI[frameIdx] = 0.0f;
        keyFrameQ[frameIdx] = 0.0f;
    }

    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
    {
        // Step 1a - extract the next well from the image cube
        tmpSum = 0.0f;
        winIdx = 0;
        for (frameIdx = 0; frameIdx < n_frames; ++frameIdx)
        {
            tmpWell[frameIdx] = image[(frameIdx*n_wells)+wellIdx];
            tmpSum           += image[(frameIdx*n_wells)+wellIdx];
        }
        // Step 1b - remove DC offset and apply windowing to each well
        tmpSum /= static_cast<float>(n_frames);  // DC offset
        // Apply front part of window
        for (frameIdx = 0; frameIdx < winParamLen; ++frameIdx)
        {
            tmpWell[frameIdx] -= tmpSum;
            tmpWell[frameIdx] *= windowCompress[winIdx++];
        }
        // Middle part (window is unity)
        for (; frameIdx < (n_frames-winParamLen); ++frameIdx)
            tmpWell[frameIdx] -= tmpSum;
        // Apply back part of window
        for (; frameIdx < n_frames; ++frameIdx)
        {
            tmpWell[frameIdx] -= tmpSum;
            tmpWell[frameIdx] *= windowCompress[winIdx++];
        }
        // Step 2 - Compute partial DFT
        // store for deltas and accumulate for keyframe
        winIdx = n_basis * wellIdx;
        DFT.PartialDFT(1, static_cast<unsigned int>(n_basis), &tmpWell[0], &dftPtrI[winIdx], &dftPtrQ[winIdx]);
        for (frameIdx = 0; frameIdx < n_basis; ++frameIdx, ++offset)
        {
            keyFrameI[frameIdx] += dftPtrI[offset];
            keyFrameQ[frameIdx] += dftPtrQ[offset];
        }
    }

    // Step 3 - Convert accumulated spectrum into mean keyframe and initialize
    // maximum delta magnitude vector
    for (frameIdx = 0; frameIdx < n_basis; ++frameIdx)
    {
        keyFrameI[frameIdx] /= static_cast<float>(n_wells);
        keyFrameQ[frameIdx] /= static_cast<float>(n_wells);
        deltaMagI[frameIdx]  = 0.0f;
        deltaMagQ[frameIdx]  = 0.0f;
    }

    // Step 4 - Calculate the correlation statistics and emphasis vector needed to
    // populate the bits per frequency element vector
    Emphasis();

    // Step 5 - Convert DFT data from absolute to raw delta, keeping maximum
    // component magnitude observed for each frequency component along the way
    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
        // May be able to use SSE/AVX vector acceleration here to process multiple frequency elements in parallel
        for (frameIdx = 0; frameIdx < n_basis; ++frameIdx, ++offset)
        {
            dftPtrI[offset] -= keyFrameI[frameIdx];
            dftPtrQ[offset] -= keyFrameQ[frameIdx];
            if (deltaMagI[frameIdx] < abs(dftPtrI[offset])) deltaMagI[frameIdx] = abs(dftPtrI[offset]);  // update maximum observed real magnitude vector
            if (deltaMagQ[frameIdx] < abs(dftPtrQ[offset])) deltaMagQ[frameIdx] = abs(dftPtrQ[offset]);  // update maximum observed imaginary magnitude vector
        }

    // Populate the scale vectors
    for (frameIdx = 0; frameIdx < n_basis; ++frameIdx)
    {
        tmpSum = static_cast<float>((1<<static_cast<int>(bitsPerFreq[frameIdx]-1))-1);
        scaleVectorI[frameIdx] = deltaMagI[frameIdx] / tmpSum;
        scaleVectorQ[frameIdx] = deltaMagQ[frameIdx] / tmpSum;
    }

    // Step 6 - Quantize the spectrum delta values and pack into output vectors
    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
        // May be able to use SSE/AVX vector acceleration here to process multiple frequency elements in parallel
        for (frameIdx = 0; frameIdx < n_basis; ++frameIdx, ++offset)
        {
            deltaI[offset] = static_cast<short>(round(dftPtrI[offset]/scaleVectorI[frameIdx]));
            deltaQ[offset] = static_cast<short>(round(dftPtrQ[offset]/scaleVectorQ[frameIdx]));
        }

    ////////////////////////////////////////////
    // Temporary scratch_pad memory deallocation
    delete[] scratch_pad; scratch_pad = NULL;
    ////////////////////////////////////////////
}

/***
*/
void DfcCompr::Emphasis()
{
    float kernelI[n_basis];
    float kernelQ[n_basis];
    float *dftPtrI = &scratch_pad[0];
    float *dftPtrQ = &scratch_pad[n_wells*n_basis];
    float *corrI = new float[n_wells*n_basis];  // probably a good idea to make this static or semi-static
    float *corrQ = new float[n_wells*n_basis];  // probably a good idea to make this static or semi-static
    register int offset, freqIdx;
    int   wellIdx;
    float meanMag[n_basis];
    float meanAng[n_basis];
    float varMag[n_basis];
    float varAng[n_basis];
    register float delta;
    float emphasisVector[n_basis];
    register int bitVal;

    // Generate correlation kernel from keyframe and initialize statistics vectors
    for (freqIdx = 0; freqIdx < n_basis; ++freqIdx)
    {
        kernelI[freqIdx] = keyFrameI[freqIdx];
        kernelQ[freqIdx] = -keyFrameQ[freqIdx];
        meanMag[freqIdx] = 0.0f;
        meanAng[freqIdx] = 0.0f;
        varMag[freqIdx]  = 0.0f;
        varAng[freqIdx]  = 0.0f;
    }

    // Correlate each well spectrum with the complex conjugate of the mean well spectrum
    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
        // May be able to vectorize the correlation process
        for (freqIdx = 0; freqIdx < n_basis; ++freqIdx, ++offset)
        {
            // DFT data        ==> (a+ib)
            // kernel          ==> (x+ib)
            // correlated data ==> (a+ib)(x+iy) = ax+iay+ibx-by = (ax-by)+i(ay+bx)
            corrI[offset] = dftPtrI[offset]*kernelI[freqIdx]-dftPtrQ[offset]*kernelQ[freqIdx];  // (ax-by)
            corrQ[offset] = dftPtrI[offset]*kernelQ[freqIdx]+dftPtrQ[offset]*kernelI[freqIdx];  // (ay+bx)
        }

    // Find mean of magnitude and angle of each frequency element
    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
        // May be able to vectorize the frequency elements
        for (freqIdx = 0; freqIdx < n_basis; ++freqIdx, ++offset)
        {
            meanMag[freqIdx] += sqrt((corrI[offset]*corrI[offset])+(corrQ[offset]*corrQ[offset]));
            meanAng[freqIdx] += atan2(corrQ[offset], corrI[offset]);
        }
    for (freqIdx = 0; freqIdx < n_basis; ++freqIdx)
    {
        meanMag[freqIdx] /= static_cast<float>(n_wells);
        meanAng[freqIdx] /= static_cast<float>(n_wells);
    }

    // Find standard deviation (now that we have the mean)
    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
        // May be able to vectorize the frequency elements
        for (freqIdx = 0; freqIdx < n_basis; ++freqIdx, ++offset)
        {
            delta = sqrt((corrI[offset]*corrI[offset])+(corrQ[offset]*corrQ[offset])) - meanMag[freqIdx];
            varMag[freqIdx] += delta * delta;  // accumulate magnitude variance
            delta = atan2(corrQ[offset], corrI[offset]) - meanAng[freqIdx];
            varAng[freqIdx] += delta * delta;  // accumulate angle variance
        }

    // Finished with correlation data (don't need to release once we make this block of memory static/semi-static)
    delete[] corrI;
    delete[] corrQ;

    // Convert accumulated variance to raw emphasis (original Matlab calculation
    // uses standard deviation rather than variance, but that uses an additional
    // sqrt step that has been optimized out here)
    for (freqIdx = 0, delta = 0.0f; freqIdx < n_basis; ++freqIdx)
    {
        emphasisVector[freqIdx] = sqrt((varMag[freqIdx]*varAng[freqIdx])/static_cast<float>(n_wells));
        if (emphasisVector[freqIdx] > delta)
            delta = emphasisVector[freqIdx];  // keep the maximum value for normalization
    }

    // Normalize the emphasis vector and convert it to bits per frequency element
    for (freqIdx = 0; freqIdx < n_basis; ++freqIdx)
    {
        // calculate the number of magnitude bits to allocate (log will always return zero or a negative value)
        bitVal = static_cast<int>(static_cast<float>(n_maxBits)+ceil(log2(emphasisVector[freqIdx]/delta)));
        if (bitVal < n_minBits) bitVal = n_minBits;  // ensure at least the minimum number of magnitude bits are allocated
        ++bitVal;  // add the sign bit
        bitsPerFreq[freqIdx] = static_cast<unsigned char>(bitVal);
    }
}

/***
 Reconstruct the image data from the compressed frequency domain vectors.
 @param n_wells - number of wells in the image patch
 @param n_frame - number of frames in this image patch.
 @param image   - Same order as RawImage structure. Individual wells of data in frame, row, col major order so
                  value(row_i,col_j,frame_k) = image[row_i * ncol + col_j + (nrow * ncol * frame_k)]
*/
void DfcCompr::LossyUncompress(float *image)
{
    float wellBufI[n_basis];
    float wellBufQ[n_basis];
    float tmpBuf[n_frames];
    int   wellIdx, frameIdx;
    register int offset, winIdx;

    for (wellIdx = 0, offset = 0; wellIdx < n_wells; ++wellIdx)
    {
        // Reconstruct partial spectrum from keyframe, delta and scale information
        for (frameIdx = 0; frameIdx < n_basis; ++frameIdx, ++offset)
        {
            wellBufI[frameIdx] = keyFrameI[frameIdx] + scaleVectorI[frameIdx]*static_cast<float>(deltaI[offset]);
            wellBufQ[frameIdx] = keyFrameQ[frameIdx] + scaleVectorQ[frameIdx]*static_cast<float>(deltaQ[offset]);
        }
        // Reconstruct time domain signal from partial spectrum using IDFT
        DFT.PartialIDFT(1, n_basis, &tmpBuf[0], &wellBufI[0], &wellBufQ[0]);
        // Store reconstructed values into the image cube
        for (frameIdx = 0, winIdx = 0; frameIdx < winParamLen; ++frameIdx)
            image[(frameIdx*n_wells)+wellIdx] = tmpBuf[frameIdx] * windowExpand[winIdx++];
        for (; frameIdx < (n_frames-winParamLen); ++frameIdx)
            image[(frameIdx*n_wells)+wellIdx] = tmpBuf[frameIdx];
        for (; frameIdx < n_frames; ++frameIdx)
            image[(frameIdx*n_wells)+wellIdx] = tmpBuf[frameIdx] * windowExpand[winIdx++];
    }
}

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

/***
 Map the memory blocks and call LossyCompress.
*/
void DfcComprWrapper::LossyCompress(float *image)
{
    SetupPointers();
    dfc.LossyCompress(image);
};

/***
 Map the memory blocks and call LossyUncompress.
*/
void DfcComprWrapper::LossyUncompress(float *image)
{
    SetupPointers();
    dfc.LossyUncompress(image);
};

/***
 Do some evil pointer arithmetic to map the DFC header and payload data vectors
 into the basis_vectors and coefficients memory blocks.
*/
void DfcComprWrapper::SetupPointers()
{
    void *hdrPtr = reinterpret_cast<void *>(basis_vectors);
    void *dtrPtr = reinterpret_cast<void *>(coefficients);
    int   offset = sizeof(float) * dfc.n_basis;

    // Map the header vectors into the basis_vectors memory block
    dfc.keyFrameI    = reinterpret_cast<float *>(hdrPtr);
    dfc.keyFrameQ    = reinterpret_cast<float *>(hdrPtr) + offset;
    dfc.scaleVectorI = reinterpret_cast<float *>(hdrPtr) + 2*offset;
    dfc.scaleVectorQ = reinterpret_cast<float *>(hdrPtr) + 3*offset;
    dfc.bitsPerFreq  = reinterpret_cast<unsigned char *>(hdrPtr) + 4*offset;

    // Map the delta data into the coefficients memory block
    offset     = sizeof(short) * dfc.n_basis * dfc.n_wells;
    dfc.deltaI = reinterpret_cast<short *>(dtrPtr);
    dfc.deltaQ = reinterpret_cast<short *>(dtrPtr) + offset;
}


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

// ---
// Author: Zachary Kann

// A set of functions for calculation of electric field / frequency maps
// as outlined in the papers of Skinner et al. See, for example, S.M. Gruenbaum,
// et al. J. Chem. Theory Comput. 2013 9 (7), 3109. These maps are necessary
// for calculation of vibrational spectra using the methodology  outline in the
// aforementioned papers.

// Abbreviations:
//   IR = infrared
//   SFG = sum-frequency generation

#include <armadillo>
#include "z_frequency_map.hpp"
#include "z_histogram.hpp"
#include "z_molecule_group.hpp"
#include "z_cx_tcf.hpp"

#ifndef _Z_SFG_MAP_HPP_
#define _Z_SFG_MAP_HPP_

class SfgMap:
  public FrequencyMap {
 public:
  inline SfgMap(const MoleculeGroup& group, const Chromophore chromophore,
                const double timestep, const int correlation_length = 200,
                const double slab_center_z = 0.0)
      : FrequencyMap(group, chromophore, timestep, correlation_length),
        slab_center_z_(slab_center_z) {
    mu_01_ = arma::zeros<arma::mat>(num_chromophores(), steps_guess_);
    alpha_01_ = arma::zeros<arma::mat>(num_chromophores(), steps_guess_);
  }

 private:
  arma::mat mu_01_;
  arma::mat alpha_01_;
  double slab_center_z_;
  static const double r_switch_ = 0.4;
  static const double r_switch_squared_ = 0.16;
  static const double r_switch_cubed_ = 0.064;

  inline void ResizeArrays() {
    omega_01_.resize(num_chromophores(), steps_guess_);
    mu_01_.resize(num_chromophores(), steps_guess_);
    alpha_01_.resize(num_chromophores(), steps_guess_);
  }

  inline double MuOrAlphaProduct(const int chromophore, const int corr_start,
                                 const int i_corr) {
    return mu_01_(chromophore,corr_start)*mu_01_(chromophore,corr_start+i_corr);
  }

  // Calculates switching function needed for SFG calculations in slab geometry.
  // Allows smooth switching between contribution to upper and lower surface as
  // one moves through the slab.
  double SwitchingFunction(const arma::rowvec& box);

  // Calculates omega and mu/alpha using the frequency maps.
  void UseMapping(const int chromophore,
                  const arma::rowvec& box);
};
#endif


#include "teca_ar_detect.h"

#include "teca_cartesian_mesh.h"
#include "teca_variant_array.h"
#include "teca_table.h"
#include "teca_calendar.h"
#include "teca_cartesian_mesh_util.h"

#include <iostream>
#include <sstream>
#include <limits>
#include <algorithm>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::ostream;
using std::vector;
using std::string;
using std::ostringstream;
using std::cerr;
using std::endl;

#define TECA_DEBUG 0
#if TECA_DEBUG > 0 && defined(TECA_HAS_VTK)
#include "teca_vtk_cartesian_mesh_writer.h"
#include "teca_programmable_source.h"
int write_mesh(
    const const_p_teca_cartesian_mesh &mesh,
    const const_p_teca_variant_array &vapor,
    const const_p_teca_variant_array &thres,
    const const_p_teca_variant_array &ccomp,
    const const_p_teca_variant_array &lsmask,
    const std::string &base_name);
#endif

// a description of the atmospheric river
struct atmospheric_river
{
    atmospheric_river() :
        pe(false), length(0.0),
        min_width(0.0), max_width(0.0),
        end_top_lat(0.0), end_top_lon(0.0),
        end_bot_lat(0.0), end_bot_lon(0.0)
    {}

    bool pe;
    double length;
    double min_width;
    double max_width;
    double end_top_lat;
    double end_top_lon;
    double end_bot_lat;
    double end_bot_lon;
};

ostream &operator<<(ostream &os, const atmospheric_river &ar)
{
    os << " type=" << (ar.pe ? "PE" : "AR")
        << " length=" << ar.length
        << " width=" << ar.min_width << ", " << ar.max_width
        << " bounds=" << ar.end_bot_lon << ", " << ar.end_bot_lat << ", "
        << ar.end_top_lon << ", " << ar.end_top_lat;
    return os;
}

unsigned sauf(const unsigned nrow, const unsigned ncol, unsigned int *image);

bool ar_detect(
    const_p_teca_variant_array lat,
    const_p_teca_variant_array lon,
    const_p_teca_variant_array land_sea_mask,
    const_p_teca_unsigned_int_array con_comp,
    unsigned long n_comp,
    double river_start_lat,
    double river_start_lon,
    double river_end_lat_low,
    double river_end_lon_low,
    double river_end_lat_high,
    double river_end_lon_high,
    double percent_in_mesh,
    double river_width,
    double river_length,
    double land_threshold_low,
    double land_threshold_high,
    atmospheric_river &ar);

// set locations in the output where the input array
// has values within the low high range.
template <typename T>
void threshold(
    const T *input, unsigned int *output,
    size_t n_vals, T low, T high)
{
    for (size_t i = 0; i < n_vals; ++i)
        output[i] = ((input[i] >= low) && (input[i] <= high)) ? 1 : 0;
}

// --------------------------------------------------------------------------
teca_ar_detect::teca_ar_detect() :
    water_vapor_variable("prw"),
    land_sea_mask_variable(""),
    low_water_vapor_threshold(20),
    high_water_vapor_threshold(75),
    search_lat_low(19.0),
    search_lon_low(180.0),
    search_lat_high(56.0),
    search_lon_high(250.0),
    river_start_lat_low(18.0),
    river_start_lon_low(180.0),
    river_end_lat_low(29.0),
    river_end_lon_low(233.0),
    river_end_lat_high(56.0),
    river_end_lon_high(238.0),
    percent_in_mesh(5.0),
    river_width(1250.0),
    river_length(2000.0),
    land_threshold_low(1.0),
    land_threshold_high(std::numeric_limits<double>::max())
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_ar_detect::~teca_ar_detect()
{}

#if defined(TECA_HAS_BOOST)

// --------------------------------------------------------------------------
void teca_ar_detect::get_properties_description(
    const string &prefix, options_description &opts)
{
    options_description ard_opts("Options for " + prefix + "(teca_ar_detect)");

    ard_opts.add_options()
        TECA_POPTS_GET(string, prefix, water_vapor_variable, "name of variable containing water vapor values")
        TECA_POPTS_GET(double, prefix, low_water_vapor_threshold, "low water vapor threshold")
        TECA_POPTS_GET(double, prefix, high_water_vapor_threshold, "high water vapor threshold")
        TECA_POPTS_GET(double, prefix, search_lat_low, "search space low latitude")
        TECA_POPTS_GET(double, prefix, search_lon_low, "search space low longitude")
        TECA_POPTS_GET(double, prefix, search_lat_high, "search space high latitude")
        TECA_POPTS_GET(double, prefix, search_lon_high, "search space high longitude")
        TECA_POPTS_GET(double, prefix, river_start_lat_low, "latitude used to classify as AR or PE")
        TECA_POPTS_GET(double, prefix, river_start_lon_low, "longitude used to classify as AR or PE")
        TECA_POPTS_GET(double, prefix, river_end_lat_low, "CA coastal region low latitude")
        TECA_POPTS_GET(double, prefix, river_end_lon_low, "CA coastal region low longitude")
        TECA_POPTS_GET(double, prefix, river_end_lat_high, "CA coastal region high latitude")
        TECA_POPTS_GET(double, prefix, river_end_lon_high, "CA coastal region high longitude")
        TECA_POPTS_GET(double, prefix, percent_in_mesh, "size of river in relation to search space area")
        TECA_POPTS_GET(double, prefix, river_width, "river width")
        TECA_POPTS_GET(double, prefix, river_length, "river length")
        TECA_POPTS_GET(string, prefix, land_sea_mask_variable, "name of variable containing land-sea mask values")
        TECA_POPTS_GET(double, prefix, land_threshold_low, "low land value")
        TECA_POPTS_GET(double, prefix, land_threshold_high, "high land value")
        ;

    opts.add(ard_opts);
}

// --------------------------------------------------------------------------
void teca_ar_detect::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, string, prefix, water_vapor_variable)
    TECA_POPTS_SET(opts, double, prefix, low_water_vapor_threshold)
    TECA_POPTS_SET(opts, double, prefix, high_water_vapor_threshold)
    TECA_POPTS_SET(opts, double, prefix, search_lat_low)
    TECA_POPTS_SET(opts, double, prefix, search_lon_low)
    TECA_POPTS_SET(opts, double, prefix, search_lat_high)
    TECA_POPTS_SET(opts, double, prefix, search_lon_high)
    TECA_POPTS_SET(opts, double, prefix, river_start_lat_low)
    TECA_POPTS_SET(opts, double, prefix, river_start_lon_low)
    TECA_POPTS_SET(opts, double, prefix, river_end_lat_low)
    TECA_POPTS_SET(opts, double, prefix, river_end_lon_low)
    TECA_POPTS_SET(opts, double, prefix, river_end_lat_high)
    TECA_POPTS_SET(opts, double, prefix, river_end_lon_high)
    TECA_POPTS_SET(opts, double, prefix, percent_in_mesh)
    TECA_POPTS_SET(opts, double, prefix, river_width)
    TECA_POPTS_SET(opts, double, prefix, river_length)
    TECA_POPTS_SET(opts, string, prefix, land_sea_mask_variable)
    TECA_POPTS_SET(opts, double, prefix, land_threshold_low)
    TECA_POPTS_SET(opts, double, prefix, land_threshold_high)
}

#endif

// --------------------------------------------------------------------------
teca_metadata teca_ar_detect::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id()
        << "teca_ar_detect::get_output_metadata" << endl;
#endif
    (void)port;

    teca_metadata output_md(input_md[0]);
    return output_md;
}

// --------------------------------------------------------------------------
int teca_ar_detect::get_active_extent(
    p_teca_variant_array lat,
    p_teca_variant_array lon,
    std::vector<unsigned long> &extent) const
{
    extent.resize(6, 0l);

    TEMPLATE_DISPATCH_FP(
        teca_variant_array_impl,
        lat.get(),

        unsigned long high_j = lat->size() - 1;
        NT *p_lat = std::dynamic_pointer_cast<TT>(lat)->get();

        unsigned long high_i = lon->size() - 1;
        NT *p_lon = std::dynamic_pointer_cast<TT>(lon)->get();

        if (index_of(p_lon, 0, high_i, static_cast<NT>(this->search_lon_low), false, extent[0])
            || index_of(p_lon, 0, high_i, static_cast<NT>(this->search_lon_high), true, extent[1])
            || index_of(p_lat, 0, high_j, static_cast<NT>(this->search_lat_low), false, extent[2])
            || index_of(p_lat, 0, high_j, static_cast<NT>(this->search_lat_high), true, extent[3]))
        {
            TECA_ERROR(
                << "search space ["
                << this->search_lon_low << ", " << this->search_lon_high << ", "
                << this->search_lat_low << ", " << this->search_lat_high
                << "] is not contained in the current dataset bounds ["
                << p_lon[0] << ", " << p_lon[high_i] << ", "
                << p_lat[0] << ", " << p_lat[high_j] << "]")
            return -1;
        }
        return 0;
        )

    TECA_ERROR("invalid coordinate array type")
    return -1;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_ar_detect::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id()
        << "teca_ar_detect::get_upstream_request" << endl;
#endif
    (void)port;

    vector<teca_metadata> up_reqs;

    teca_metadata md = input_md[0];

    // locate the extents of the user supplied region of
    // interest
    teca_metadata coords;
    if (md.get("coordinates", coords))
    {
        TECA_ERROR("metadata is missing \"coordinates\"")
        return up_reqs;
    }

    p_teca_variant_array lat;
    p_teca_variant_array lon;
    if (!(lat = coords.get("y")) || !(lon = coords.get("x")))
    {
        TECA_ERROR("metadata missing lat lon coordinates")
        return up_reqs;
    }

    vector<unsigned long> extent(6, 0l);
    if (this->get_active_extent(lat, lon, extent))
    {
        TECA_ERROR("failed to determine the active extent")
        return up_reqs;
    }

#if TECA_DEBUG > 1
    cerr << teca_parallel_id() << "active_bound = "
        << this->search_lon_low<< ", " << this->search_lon_high
        << ", " << this->search_lat_low << ", " << this->search_lat_high
        << endl;
    cerr << teca_parallel_id() << "active_extent = "
        << extent[0] << ", " << extent[1] << ", " << extent[2] << ", "
        << extent[3] << ", " << extent[4] << ", " << extent[5] << endl;
#endif

    // build the request
    vector<string> arrays;
    request.get("arrays", arrays);
    arrays.push_back(this->water_vapor_variable);
    if (!this->land_sea_mask_variable.empty())
        arrays.push_back(this->land_sea_mask_variable);

    teca_metadata up_req(request);
    up_req.insert("arrays", arrays);
    up_req.insert("extent", extent);

    up_reqs.push_back(up_req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_ar_detect::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#if TECA_DEBUG > 1
    cerr << teca_parallel_id() << "teca_ar_detect::execute";
    this->to_stream(cerr);
    cerr << endl;
#endif
    (void)port;
    (void)request;

    // get the input dataset
    const_p_teca_cartesian_mesh mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);
    if (!mesh)
    {
        TECA_ERROR("invalid input. teca_cartesian_mesh is required")
        return nullptr;
    }

    // get coordinate arrays
    const_p_teca_variant_array lat = mesh->get_y_coordinates();
    const_p_teca_variant_array lon = mesh->get_x_coordinates();

    if (!lon || !lat)
    {
        TECA_ERROR("invalid mesh. missing lat lon coordinates")
        return nullptr;
    }

    // get land sea mask
    const_p_teca_variant_array land_sea_mask;
    if (this->land_sea_mask_variable.empty() ||
        !(land_sea_mask = mesh->get_point_arrays()->get(this->land_sea_mask_variable)))
    {
        // input doesn't have it, generate a stand in such
        // that land fall criteria will evaluate true
        size_t n = lat->size()*lon->size();
        p_teca_double_array lsm = teca_double_array::New(n, this->land_threshold_low);
        land_sea_mask = lsm;
    }

    // get time step
    unsigned long time_step;
    mesh->get_time_step(time_step);

    // get units
    std::string units;
    mesh->get_time_units(units);
    if ((units.find("days since") == string::npos) || (units.size() < 21))
    {
        TECA_ERROR("invalid time units" << endl << units)
        return nullptr;
    }

    // base/origin date
    long base_y = atoi(units.c_str() + 11);
    long base_m = atoi(units.c_str() + 16);
    long base_d = atoi(units.c_str() + 19);

    // get calendar
    std::string calendar;
    mesh->get_calendar(calendar);

    // compute the current date
    long curr_y = 0;
    long curr_m = 0;
    long curr_d = 0;

    if ((calendar == "standard") || (calendar == "gregorian"))
    {
        // assume units format is "days since YYYY-MM-DD"
        if (!valid_gregorian_date(base_y, base_m, base_d))
        {
            TECA_ERROR("CF time:units convert to an invalid date" << endl
                << units << " -> " << base_y << "-" << base_m << "-" << base_d)
            return nullptr;
        }
        long base_g = gregorian_number(base_y, base_m, base_d);

        // get offset of the current timestep
        double curr_g = 0.0;
        mesh->get_time(curr_g);

        // make date calc
        date_from_gregorian_number(
            static_cast<long>(curr_g) + base_g, curr_y, curr_m, curr_d);
    }

#if TECA_DEBUG > 1
    cerr << teca_parallel_id()
        << " " << time_step << " "
        << curr_y << "/" << curr_m << "/" << curr_d << endl;
#endif

    // get water vapor data
    vector<unsigned long> extent;
    mesh->get_extent(extent);

    unsigned long num_rows = extent[3] - extent[2] + 1;
    unsigned long num_cols = extent[1] - extent[0] + 1;
    unsigned long num_rc = num_rows*num_cols;

    const_p_teca_variant_array water_vapor
        = mesh->get_point_arrays()->get(this->water_vapor_variable);

    if (!water_vapor)
    {
        TECA_ERROR(
            << "Dataset missing water vapor variable \""
            << this->water_vapor_variable << "\"")
        return nullptr;
    }

    p_teca_table event = teca_table::New();

    event->declare_columns(
        "year", int(), "month", int(), "day", int(),
        "time_step", long(), "length", double(),
        "min width", double(), "max width", double(),
        "end_top_lat", double(), "end_top_lon", double(),
        "end_bot_lat", double(), "end_bot_lon", double(),
        "type", std::string());

    TEMPLATE_DISPATCH(
        const teca_variant_array_impl,
        water_vapor.get(),

        const NT *p_wv = dynamic_cast<TT*>(water_vapor.get())->get();

        // threshold
        p_teca_unsigned_int_array con_comp
            = teca_unsigned_int_array::New(num_rc, 0);

        unsigned int *p_con_comp = con_comp->get();

        threshold(p_wv, p_con_comp, num_rc,
            static_cast<NT>(this->low_water_vapor_threshold),
            static_cast<NT>(this->high_water_vapor_threshold));

#if TECA_DEBUG > 0 && defined(TECA_HAS_VTK)
        p_teca_variant_array thresh = con_comp->new_copy();
#endif

        // label
        int num_comp = sauf(num_rows, num_cols, p_con_comp);

#if TECA_DEBUG > 0 && defined(TECA_HAS_VTK)
        write_mesh(mesh, water_vapor, thresh, con_comp, land_sea_mask, "ar_mesh");
#endif

        // detect ar
        atmospheric_river ar;
        if (num_comp
            && ar_detect(lat, lon, land_sea_mask, con_comp, num_comp,
            this->river_start_lat_low, this->river_start_lon_low,
            this->river_end_lat_low, this->river_end_lon_low,
            this->river_end_lat_high, this->river_end_lon_high,
            this->percent_in_mesh, this->river_width,
            this->river_length, this->land_threshold_low,
            this->land_threshold_high, ar))
        {
#if TECA_DEBUG > 0
            cerr << teca_parallel_id() << " event detected " << time_step
                << " " << curr_y << "/" << curr_m << "/" << curr_d << endl;
#endif
            event << curr_y << curr_m << curr_d << time_step
                << ar.length << ar.min_width << ar.max_width
                << ar.end_top_lat << ar.end_top_lon
                << ar.end_bot_lat << ar.end_bot_lon
                << std::string(ar.pe ? "PE" : "AR");
        }
        )

    return event;
}

// --------------------------------------------------------------------------
void teca_ar_detect::to_stream(ostream &os) const
{
    os << " water_vapor_variable=" << this->water_vapor_variable
        << " land_sea_mask_variable=" << this->land_sea_mask_variable
        << " low_water_vapor_threshold=" << this->low_water_vapor_threshold
        << " high_water_vapor_threshold=" << this->high_water_vapor_threshold
        << " river_start_lon_low=" << this->river_start_lon_low
        << " river_start_lat_low=" << this->river_start_lat_low
        << " river_end_lon_low=" << this->river_end_lon_low
        << " river_end_lat_low=" << this->river_end_lat_low
        << " river_end_lon_high=" << this->river_end_lon_high
        << " river_end_lat_high=" << this->river_end_lat_high
        << " percent_in_mesh=" << this->percent_in_mesh
        << " river_width=" << this->river_width
        << " river_length=" << this->river_length
        << " land_threshodl_low=" << this->land_threshold_low
        << " land_threshodl_high=" << this->land_threshold_high;
}

// Code borrowed from John Wu's sauf.cpp
// Find the minimal value starting @arg ind.
inline unsigned afind(const std::vector<unsigned>& equiv,
              const unsigned ind)
{
    unsigned ret = ind;
    while (equiv[ret] < ret)
    {
        ret = equiv[ret];
    }
    return ret;
}

// Set the values starting with @arg ind.
inline void aset(std::vector<unsigned>& equiv,
         const unsigned ind, const unsigned val)
{
    unsigned i = ind;
    while (equiv[i] < i)
    {
        unsigned j = equiv[i];
        equiv[i] = val;
        i = j;
    }
    equiv[i] = val;
}

/*
* Purpose:        Scan with Array-based Union-Find
* Return vals:    Number of connected components
* Description:    SAUF -- Scan with Array-based Union-Find.
* This is an implementation that follows the decision try to minimize
* number of neighbors visited and uses the array-based union-find
* algorithms to minimize work on the union-find data structure.  It works
* with each pixel/cell of the 2D binary image individually.
* The 2D binary image is passed to sauf as a unsigned*.  On input, the
* zero value is treated as the background, and non-zero is treated as
* object.  On successful completion of this function, the non-zero values
* in array image is replaced by its label.
* The return value is the number of components found.
*/
unsigned sauf(const unsigned nrow, const unsigned ncol, unsigned *image)
{
    const unsigned ncells = ncol * nrow;
    const unsigned ncp1 = ncol + 1;
    const unsigned ncm1 = ncol - 1;
    std::vector<unsigned int> equiv;    // equivalence array
    unsigned nextLabel = 1;

    equiv.reserve(ncol);
    equiv.push_back(0);

    // the first cell of the first line
    if (*image != 0)
    {
    *image = nextLabel;
    equiv.push_back(nextLabel);
    ++ nextLabel;
    }
    // first row of cells
    for (unsigned i = 1; i < ncol; ++ i)
    {
    if (image[i] != 0)
    {
        if (image[i-1] != 0)
        {
        image[i] = image[i-1];
        }
        else
        {
        equiv.push_back(nextLabel);
        image[i] = nextLabel;
        ++ nextLabel;
        }
    }
    }

    // scan the rest of lines, check neighbor b first
    for (unsigned j = ncol; j < ncells; j += ncol)
    {
    unsigned nc, nd, k, l;

    // the first point of the line has two neighbors, and the two
    // neighbors must have at most one label (recorded as nc)
    if (image[j] != 0)
    {
        if (image[j-ncm1] != 0)
        nc = image[j-ncm1];
        else if (image[j-ncol] != 0)
        nc = image[j-ncol];
        else
        nc = nextLabel;
        if (nc != nextLabel) { // use existing label
        nc = equiv[nc];
        image[j] = nc;
        }
        else { // need a new label
        equiv.push_back(nc);
        image[j] = nc;
        ++ nextLabel;
        }
    }

    // the rest of the line
    for (unsigned i = j+1; i < j+ncol; ++ i)
    {
        if (image[i] != 0) {
        if (image[i-ncol] != 0) {
            nc = image[i-ncol];
            l = afind(equiv, nc);
            aset(equiv, nc, l);
            image[i] = l;
        }
        else if (i-ncm1<j && image[i-ncm1] != 0) {
            nc = image[i-ncm1];

            if (image[i-1] != 0)
            nd = image[i-1];
            else if (image[i-ncp1] != 0)
            nd = image[i-ncp1];
            else
            nd = nextLabel;
            if (nd < nextLabel) {
            k = afind(equiv, nc);
            l = afind(equiv, nd);
            if (l <= k) {
                aset(equiv, nc, l);
                aset(equiv, nd, l);
            }
            else {
                l = k;
                aset(equiv, nc, k);
                aset(equiv, nd, k);
            }
            image[i] = l;
            }
            else {
            l = afind(equiv, nc);
            aset(equiv, nc, l);
            image[i] = l;
            }
        }
        else if (image[i-1] != 0) {
            nc = image[i-1];
            l = afind(equiv, nc);
            aset(equiv, nc, l);
            image[i] = l;
        }
        else if (image[i-ncp1] != 0) {
            nc = image[i-ncp1];
            l = afind(equiv, nc);
            aset(equiv, nc, l);
            image[i] = l;
        }
        else { // need a new label
            equiv.push_back(nextLabel);
            image[i] = nextLabel;
            ++ nextLabel;
        }
        }
    }
    } // for (unsigned j ...

    // phase II: re-number the labels to be consecutive
    nextLabel = 0;
    const unsigned nequiv = equiv.size();
    for (unsigned i = 0; i < nequiv;  ++ i) {
    if (equiv[i] < i) { // chase one more time
#if defined(_DEBUG) || defined(DEBUG)
        std::cout << i << " final " << equiv[i] << " ==> "
              << equiv[equiv[i]] << std::endl;
#endif
        equiv[i] = equiv[equiv[i]];
    }
    else { // change to the next smallest unused label
#if defined(_DEBUG) || defined(DEBUG)
        std::cout << i << " final " << equiv[i] << " ==> "
              << nextLabel << std::endl;
#endif
        equiv[i] = nextLabel;
        ++ nextLabel;
    }
    }

    if (nextLabel < nequiv) {// relabel all cells to their minimal labels
    for (unsigned i = 0; i < ncells; ++ i)
        image[i] = equiv[image[i]];
    }

#if defined(_DEBUG) || defined(DEBUG)
    std::cout << "sauf(" << nrow << ", " << ncol << ") assigned "
          << nextLabel-1 << " label" << (nextLabel>2?"s":"")
          << ", used " << nequiv << " provisional labels"
          << std::endl;
#endif
    return nextLabel-1;
}

// do any of the detected points meet the river start
// criteria. retrun true if so.
template<typename T>
bool river_start_criteria_lat(
    const vector<int> &con_comp_r,
    const T *p_lat,
    T river_start_lat)
{
    unsigned long n = con_comp_r.size();
    for (unsigned long q = 0; q < n; ++q)
    {
        if (p_lat[con_comp_r[q]] >= river_start_lat)
            return true;
    }
    return false;
}

// do any of the detected points meet the river start
// criteria. retrun true if so.
template<typename T>
bool river_start_criteria_lon(
    const vector<int> &con_comp_c,
    const T *p_lon,
    T river_start_lon)
{
    unsigned long n = con_comp_c.size();
    for (unsigned long q = 0; q < n; ++q)
    {
        if (p_lon[con_comp_c[q]] >= river_start_lon)
            return true;
    }
    return false;
}

// helper return true if the start criteria is
// met, and classifies the ar as PE if it starts
// in the bottom boundary.
template<typename T>
bool river_start_criteria(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon,
    T start_lat,
    T start_lon,
    atmospheric_river &ar)
{
    return
         ((ar.pe = river_start_criteria_lat(con_comp_r, p_lat, start_lat))
         || river_start_criteria_lon(con_comp_c, p_lon, start_lon));
}

// do any of the detected points meet the river end
// criteria? (ie. does it hit the west coasts?) if so
// store a bounding box covering the river and return
// true.
template<typename T>
bool river_end_criteria(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon,
    T river_end_lat_low,
    T river_end_lon_low,
    T river_end_lat_high,
    T river_end_lon_high,
    atmospheric_river &ar)
{
    bool end_criteria = false;

    vector<int> end_col_idx;

    unsigned int count = con_comp_r.size();
    for (unsigned int i = 0; i < count; ++i)
    {
        // approximate land mask boundaries for the western coast of the US,
        T lon_val = p_lon[con_comp_c[i]];
        if ((lon_val >= river_end_lon_low) && (lon_val <= river_end_lon_high))
            end_col_idx.push_back(i);
    }

    // look for rows that fall between lat boundaries
    T top_lat = T();
    T top_lon = T();
    T bot_lat = T();
    T bot_lon = T();

    bool top_touch = false;
    unsigned int end_col_count = end_col_idx.size();
    for (unsigned int i = 0; i < end_col_count; ++i)
    {
        // approximate land mask boundaries for the western coast of the US,
        T lat_val = p_lat[con_comp_r[end_col_idx[i]]];
        if ((lat_val >= river_end_lat_low) && (lat_val <= river_end_lat_high))
        {
            T lon_val = p_lon[con_comp_c[end_col_idx[i]]];
            end_criteria = true;
            if (!top_touch)
            {
                top_touch = true;
                top_lat = lat_val;
                top_lon = lon_val;
            }
            bot_lat = lat_val;
            bot_lon = lon_val;
        }
    }

    ar.end_top_lat = top_lat;
    ar.end_top_lon = top_lon;
    ar.end_bot_lat = bot_lat;
    ar.end_bot_lon = bot_lon;

    return end_criteria;
}

/*
* Calculate geodesic distance between two lat, long pairs
* CODE borrowed from: from http://www.geodatasource.com/developers/c
* from http://osiris.tuwien.ac.at/~wgarn/gis-gps/latlong.html
* from http://www.codeproject.com/KB/cpp/Distancecplusplus.aspx
*/
template<typename T>
T geodesic_distance(T lat1, T lon1, T lat2, T lon2)
{
    T deg_to_rad = T(M_PI/180.0);

    T dlat1 = lat1*deg_to_rad;
    T dlon1 = lon1*deg_to_rad;
    T dlat2 = lat2*deg_to_rad;
    T dlon2 = lon2*deg_to_rad;

    T dLon = dlon1 - dlon2;
    T dLat = dlat1 - dlat2;

    T sin_dLat_half_sq = sin(dLat/T(2.0));
    sin_dLat_half_sq *= sin_dLat_half_sq;

    T sin_dLon_half_sq = sin(dLon/T(2.0));
    sin_dLon_half_sq *= sin_dLon_half_sq;

    T aHarv = sin_dLat_half_sq
        + cos(dlat1)*cos(dlat2)*sin_dLon_half_sq;

    T cHarv = T(2.0)*atan2(sqrt(aHarv), sqrt(T(1.0) - aHarv));

    T R = T(6371.0);
    T distance = R*cHarv;

    return distance;
}

/*
* This function calculates the average geodesic width
* As each pixel represents certain area, the total area
* is the product of the number of pixels and the area of
* one pixel. The average width is: the total area divided
* by the medial axis length
* We are calculating the average width, since we are not
* determining where exactly to cut off the tropical region
* to calculate the real width of an atmospheric river
*/
template <typename T>
T avg_width(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    T ar_len,
    const T *p_lat,
    const T *p_lon)
{
/*
    // TODO -- need bounds checking when doing things like
    // p_lat[con_comp_r[0] + 1]. also because it's potentially
    // a stretched cartesian mesh need to compute area of
    // individual cells

    // length of cell in lat direction
    T lat_val[2] = {p_lat[con_comp_r[0]], p_lat[con_comp_r[0] + 1]};
    T lon_val[2] = {p_lon[con_comp_c[0]], p_lon[con_comp_c[0]]};
    T dlat = geodesic_distance(lat_val[0], lon_val[0], lat_val[1], lon_val[1]);

    // length of cell in lon direction
    lat_val[1] = lat_val[0];
    lon_val[1] = p_lon[con_comp_c[0] + 1];
    T dlon = geodesic_distance(lat_val[0], lon_val[0], lat_val[1], lon_val[1]);
*/
    (void)con_comp_c;
    // compute area of the first cell in the input mesh
    // length of cell in lat direction
    T lat_val[2] = {p_lat[0], p_lat[1]};
    T lon_val[2] = {p_lon[0], p_lon[0]};
    T dlat = geodesic_distance(lat_val[0], lon_val[0], lat_val[1], lon_val[1]);

    // length of cell in lon direction
    lat_val[1] = lat_val[0];
    lon_val[1] = p_lon[1];
    T dlon = geodesic_distance(lat_val[0], lon_val[0], lat_val[1], lon_val[1]);

    // area
    T pixel_area = dlat*dlon;
    T total_area = pixel_area*con_comp_r.size();

    // avg width
    T avg_width = total_area/ar_len;
    return avg_width;
}

/*
* Find the middle point between two pairs of lat and lon values
* http://stackoverflow.com/questions/4164830/geographic-midpoint-between-two-coordinates
*/
template<typename T>
void geodesic_midpoint(T lat1, T lon1, T lat2, T lon2, T &mid_lat, T &mid_lon)
{
    T deg_to_rad = T(M_PI/180.0);
    T dLon = (lon2 - lon1) * deg_to_rad;
    T dLat1 = lat1 * deg_to_rad;
    T dLat2 = lat2 * deg_to_rad;
    T dLon1 = lon1 * deg_to_rad;

    T Bx = cos(dLat2) * cos(dLon);
    T By = cos(dLat2) * sin(dLon);

    mid_lat = atan2(sin(dLat1)+sin(dLat2),
        sqrt((cos(dLat1)+Bx)*(cos(dLat1)+Bx)+By*By));

    mid_lon = dLon1 + atan2(By, (cos(dLat1)+Bx));

    T rad_to_deg = T(180.0/M_PI);
    mid_lat *= rad_to_deg;
    mid_lon *= rad_to_deg;
}

/*
* Find the length along the medial axis of a connected component
* Medial length is the sum of the distances between the medial
* points in the connected component
*/
template<typename T>
T medial_length(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon)
{
    vector<int> jb_r1;
    vector<int> jb_c1;
    vector<int> jb_c2;

    long row_track = -1;

    unsigned long count = con_comp_r.size();
    for (unsigned long i = 0; i < count; ++i)
    {
        if (row_track != con_comp_r[i])
        {
            jb_r1.push_back(con_comp_r[i]);
            jb_c1.push_back(con_comp_c[i]);

            jb_c2.push_back(con_comp_c[i]);

            row_track = con_comp_r[i];
        }
        else
        {
            jb_c2.back() = con_comp_c[i];
        }
    }

    T total_dist = T();

    long b_count = jb_r1.size() - 1;
    for (long i = 0; i < b_count; ++i)
    {
        T lat_val[2];
        T lon_val[2];

        lat_val[0] = p_lat[jb_r1[i]];
        lat_val[1] = p_lat[jb_r1[i]];

        lon_val[0] = p_lon[jb_c1[i]];
        lon_val[1] = p_lon[jb_c2[i]];

        T mid_lat1;
        T mid_lon1;

        geodesic_midpoint(
            lat_val[0], lon_val[0], lat_val[1], lon_val[1],
            mid_lat1, mid_lon1);

        lat_val[0] = p_lat[jb_r1[i+1]];
        lat_val[1] = p_lat[jb_r1[i+1]];

        lon_val[0] = p_lon[jb_c1[i+1]];
        lon_val[1] = p_lon[jb_c2[i+1]];

        T mid_lat2;
        T mid_lon2;

        geodesic_midpoint(
            lat_val[0], lon_val[0], lat_val[1], lon_val[1],
            mid_lat2, mid_lon2);

        total_dist
            += geodesic_distance(mid_lat1, mid_lon1, mid_lat2, mid_lon2);
    }

    return total_dist;
}

/*
// Suren's function
// helper return true if the geometric conditions
// on an ar are satisfied. also stores the length
// and width of the river.
template <typename T>
bool river_geometric_criteria(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon,
    double river_length,
    double river_width,
    atmospheric_river &ar)
{
    ar.length = medial_length(con_comp_r, con_comp_c, p_lat, p_lon);

    ar.width = avg_width(con_comp_r, con_comp_c,
        static_cast<T>(ar.length), p_lat, p_lon);

    return (ar.length >= river_length) && (ar.width <= river_width);
}
*/

// Junmin's function for height of a triangle
template<typename T>
T triangle_height(T base, T s1, T s2)
{
    // area from Heron's fomula
    T p = (base + s1 + s2)/T(2);
    T area = p*(p - base)*(p - s1)*(p - s2);
    // detect impossible triangle
    if (area < T())
        return std::min(s1, s2);
    // height from A = 1/2 b h
    return T(2)*sqrt(area)/base;
}

// TDataProcessor::check_geodesic_width_top_down
// Junmin's function for detecting river based on
// it's geometric properties
template <typename T>
bool river_geometric_criteria(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon,
    double river_length,
    double river_width,
    atmospheric_river &ar)
{
    vector<int> distinct_rows;
    vector<int> leftmost_col;
    vector<int> rightmost_col;

    int row_track = -1;
    size_t count = con_comp_r.size();
    for (size_t i = 0; i < count; ++i)
    {
        if (row_track != con_comp_r[i])
        {
            row_track = con_comp_r[i];

            distinct_rows.push_back(con_comp_r[i]);
            leftmost_col.push_back(con_comp_c[i]);
            rightmost_col.push_back(con_comp_c[i]);
        }
        else
        {
            rightmost_col.back() = con_comp_c[i];
        }
    }

    // river metrics
    T length_from_top = T();
    T min_width = std::numeric_limits<T>::max();
    T max_width = std::numeric_limits<T>::lowest();

    for (long i = distinct_rows.size() - 2; i >= 0; --i)
    {
        // for each row w respect to row above it. triangulate
        // a quadrilateral composed of left and right most points
        // in this and the above rows. ccw ordering from lower
        // left corner is A,B,D,C.

        // low left-right distance
        T AB = geodesic_distance(
            p_lat[distinct_rows[i]], p_lon[leftmost_col[i]],
            p_lat[distinct_rows[i]], p_lon[rightmost_col[i]]);

        // left side bottom-top distance
        T AC = geodesic_distance(
            p_lat[distinct_rows[i]], p_lon[leftmost_col[i]],
            p_lat[distinct_rows[i+1]], p_lon[leftmost_col[i]]);

        // distance from top left to bottom right, across
        T BC = geodesic_distance(
            p_lat[distinct_rows[i]], p_lon[rightmost_col[i]],
            p_lat[distinct_rows[i+1]], p_lon[leftmost_col[i+1]]);

        // high left-right distance
        T CD = geodesic_distance(
            p_lat[distinct_rows[i+1]], p_lon[leftmost_col[i+1]],
            p_lat[distinct_rows[i+1]], p_lon[rightmost_col[i+1]]);

        // right side bottom-top distance
        T BD = geodesic_distance(
            p_lat[distinct_rows[i]], p_lon[rightmost_col[i]],
            p_lat[distinct_rows[i+1]], p_lon[rightmost_col[i+1]]);

        T height_from_b = triangle_height(AC, AB, BC);
        T height_from_c = triangle_height(BD, BC, CD);

        T curr_min = std::min(height_from_b, height_from_c);

        // test width criteria
        if (curr_min > river_width)
        {
            // TODO -- first time through the loop length == 0. is it intentional
            // to discard the detection or should length calc take place before this test?
            // note: first time through loop none of the event details have been recoreded
            if (length_from_top <= river_length)
            {
                 // too short to be a river
                return false;
            }
            else
            {
                 // part of a connected region is AR
                ar.min_width = static_cast<double>(min_width);
                ar.max_width = static_cast<double>(max_width);
                ar.length = static_cast<double>(length_from_top);
                return true;
            }
        }

        // update width
        min_width = std::min(min_width, curr_min);
        max_width = std::max(max_width, curr_min);

        // update length
        T mid_bot_lat;
        T mid_bot_lon;
        geodesic_midpoint(
            p_lat[distinct_rows[i]], p_lon[leftmost_col[i]],
            p_lat[distinct_rows[i]], p_lon[rightmost_col[i]],
            mid_bot_lat, mid_bot_lon);

        T mid_top_lat;
        T mid_top_lon;
        geodesic_midpoint(
            p_lat[distinct_rows[i+1]], p_lon[leftmost_col[i+1]],
            p_lat[distinct_rows[i+1]], p_lon[rightmost_col[i+1]],
            mid_top_lat, mid_top_lon);

        length_from_top += geodesic_distance(
            mid_bot_lat, mid_bot_lon, mid_top_lat, mid_top_lon);
    }

    // check the length criteria.
    // TODO: if we are here the widtrh critera was not met
    // so the following detection is based solely on the length?
    if (length_from_top > river_length)
    {
        // AR
        ar.min_width = static_cast<double>(min_width);
        ar.max_width = static_cast<double>(max_width);
        ar.length = static_cast<double>(length_from_top);
        return true;
    }

    return false;
}

// Junmin's function checkRightBoundary
// note: if land sea mask is not available land array
// must all be true.
template<typename T>
bool river_end_criteria(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const vector<bool> &land,
    const T *p_lat,
    const T *p_lon,
    T river_end_lat_low,
    T river_end_lon_low,
    T river_end_lat_high,
    T river_end_lon_high,
    atmospheric_river &ar)
{
    // locate component points within shoreline
    // box
    bool first_crossing = false;
    bool event_detected = false;

    T top_lat = T();
    T top_lon = T();
    T bot_lat = T();
    T bot_lon = T();

    std::vector<int> right_bound_col_idx;
    size_t count = con_comp_c.size();
    for (size_t i = 0; i < count; ++i)
    {
        T lat = p_lat[con_comp_r[i]];
        T lon = p_lon[con_comp_c[i]];

        if ((lat >= river_end_lat_low) && (lat <= river_end_lat_high)
            && (lon >= river_end_lon_low) && (lon <= river_end_lon_high))
        {
            if (!event_detected)
                event_detected = land[i];

            if (!first_crossing)
            {
                first_crossing = true;
                top_lat = lat;
                top_lon = lon;
            }
            bot_lat = lat;
            bot_lon = lon;
        }
    }

    ar.end_top_lat = top_lat;
    ar.end_top_lon = top_lon;
    ar.end_bot_lat = bot_lat;
    ar.end_bot_lon = bot_lon;

    return event_detected;
}

// Junmin's function
template<typename T>
void classify_event(
    const vector<int> &con_comp_r,
    const vector<int> &con_comp_c,
    const T *p_lat,
    const T *p_lon,
    T start_lat,
    T start_lon,
    atmospheric_river &ar)
{
    // classification determined by first detected point in event
    // is closer to left or to bottom
    T lat = p_lat[con_comp_r[0]];
    T lon = p_lon[con_comp_c[0]];

    ar.pe = false;
    if ((lat - start_lat) < (lon - start_lon))
        ar.pe = true; // PE
}

/*
* The main function that checks whether an AR event exists in
* given sub-plane of data. This currently applies only to the Western
* coast of the USA. return true if an ar is found.
*/
bool ar_detect(
    const_p_teca_variant_array lat,
    const_p_teca_variant_array lon,
    const_p_teca_variant_array land_sea_mask,
    const_p_teca_unsigned_int_array con_comp,
    unsigned long n_comp,
    double river_start_lat,
    double river_start_lon,
    double river_end_lat_low,
    double river_end_lon_low,
    double river_end_lat_high,
    double river_end_lon_high,
    double percent_in_mesh,
    double river_width,
    double river_length,
    double land_threshold_low,
    double land_threshold_high,
    atmospheric_river &ar)
{
    NESTED_TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        lat.get(),
        1,

        NESTED_TEMPLATE_DISPATCH(
            const teca_variant_array_impl,
            land_sea_mask.get(),
            2,

            const NT1 *p_lat = dynamic_cast<TT1*>(lat.get())->get();
            const NT1 *p_lon = dynamic_cast<TT1*>(lon.get())->get();

            const NT2 *p_land_sea_mask
                = dynamic_cast<TT2*>(land_sea_mask.get())->get();

            NT1 start_lat = static_cast<NT1>(river_start_lat);
            NT1 start_lon = static_cast<NT1>(river_start_lon);
            NT1 end_lat_low = static_cast<NT1>(river_end_lat_low);
            NT1 end_lon_low = static_cast<NT1>(river_end_lon_low);
            NT1 end_lat_high = static_cast<NT1>(river_end_lat_high);
            NT1 end_lon_high = static_cast<NT1>(river_end_lon_high);

            unsigned long num_rows = lat->size();
            unsigned long num_cols = lon->size();

            // # in PE is % of points in regious mesh
            unsigned long num_rc = num_rows*num_cols;
            unsigned long thr_count = num_rc*percent_in_mesh/100.0;

            unsigned int *p_labels = con_comp->get();
            for (unsigned int i = 1; i <= n_comp; ++i)
            {
                // for all discrete connected component labels
                // verify if there exists an AR
                vector<int> con_comp_r;
                vector<int> con_comp_c;
                vector<bool> land;

                for (unsigned long r = 0, q = 0; r < num_rows; ++r)
                {
                    for (unsigned long c = 0; c < num_cols; ++c, ++q)
                    {
                        if (p_labels[q] == i)
                        {
                            // gather points of this connected component
                            con_comp_r.push_back(r);
                            con_comp_c.push_back(c);

                            // identify them as land or not
                            land.push_back(
                                (p_land_sea_mask[q] >= land_threshold_low)
                                && (p_land_sea_mask[q] < land_threshold_high));
                        }
                    }
                }

                // check for ar criteria
                unsigned long count = con_comp_r.size();
                if ((count > thr_count)
                    && river_end_criteria(
                        con_comp_r, con_comp_c, land,
                        p_lat, p_lon,
                        end_lat_low, end_lon_low,
                        end_lat_high, end_lon_high,
                        ar)
                    && river_geometric_criteria(
                        con_comp_r, con_comp_c, p_lat, p_lon,
                        river_length, river_width, ar))
                {
                    // determine if PE or AR
                    classify_event(
                        con_comp_r, con_comp_c, p_lat, p_lon,
                        start_lat, start_lon, ar);
                    return true;
                }
            }
            )
        )
    return false;
}

#if TECA_DEBUG > 0 && defined(TECA_HAS_VTK)
// helper to dump a dataset for debugging
int write_mesh(
    const const_p_teca_cartesian_mesh &mesh,
    const const_p_teca_variant_array &vapor,
    const const_p_teca_variant_array &thresh,
    const const_p_teca_variant_array &ccomp,
    const const_p_teca_variant_array &lsmask,
    const std::string &base_name)
{
    p_teca_cartesian_mesh m = teca_cartesian_mesh::New();
    m->copy_metadata(mesh);

    p_teca_array_collection pac = m->get_point_arrays();
    pac->append("vapor", std::const_pointer_cast<teca_variant_array>(vapor));
    pac->append("thresh", std::const_pointer_cast<teca_variant_array>(thresh));
    pac->append("ccomp", std::const_pointer_cast<teca_variant_array>(ccomp));
    pac->append("lsmask", std::const_pointer_cast<teca_variant_array>(lsmask));

    p_teca_programmable_source s = teca_programmable_source::New();
    s->set_execute_function(
        [m] (const teca_metadata &) -> const_p_teca_dataset
        { return m; }
        );

    p_teca_vtk_cartesian_mesh_writer w
        = teca_vtk_cartesian_mesh_writer::New();

    w->set_base_file_name(base_name);
    w->set_input_connection(s->get_output_port());
    w->update();

    return 0;
}
#endif

/* some of the datasets have date array with calcs already made!
    // get date
    // TODO -- handle date in a unform manner
    long YYYY = 0;
    long MM = 0;
    long DD = 0;
    const_p_teca_variant_array date
        = mesh->get_information_arrays()->get("date");

    if (date)
    {
        TEMPLATE_DISPATCH(
            const teca_variant_array_impl,
            date.get(),

            NT d = dynamic_cast<TT*>(date.get())->get()[0];
            YYYY = static_cast<long>(d)/10000l;
            MM = (static_cast<long>(d) - YYYY*10000l)/100l;
            DD = static_cast<long>(d) - YYYY*10000l - MM*100l;
            )
    }
*/


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, AASS Research Center, Orebro University.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
#ifndef CEVENT_COUNTER_DATA_H
#define CEVENT_COUNTER_DATA_H
#include<cmath>
#include <iostream>
#include <cstdio>
#include <Eigen/Dense>
#include <inttypes.h>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
///Defines for presets
#define EVENTMAP_OCCU  255
#define EVENTMAP_UNKNOWN  127
#define EVENTMAP_FREE  0
#define EVENTMAP_NUMBER_OF_EVENTS_STORED 128

///Recency filtering stuff
#define EVENTMAP_USE_RECENCY_FILTERING
#define EVENTMAP_OBSERVATION_LIMIT 30000.0
#define Wforget (5000.0/5001.0)

/**
* Structure in the map
**/
struct TEventData
{
  uint8_t 	occval; 			///<Occupancy value
  float 		a_exit_event; ///Number of exit events ( OCC 2 EMP)
  float  		b_exit_event; ///Number of times the cell is perceived as occupied

  float 		a_entry_event;
  float  		b_entry_event; ///Number of times the cell is perceived as empty
  uint64_t  events;        /// A storage for the last 64 events (as bits)

  TEventData():
    occval(127),
    a_exit_event(1.0),
    b_exit_event(1.0),
    a_entry_event(1.0),
    b_entry_event(1.0),
    events(0)
  {
  };

  ~TEventData()
  {
  }

  const TEventData &operator =(const TEventData &copy)
  {
    occval = copy.occval;
    a_exit_event = copy.a_exit_event;
    b_exit_event = copy.b_exit_event;
    a_entry_event = copy.a_entry_event;
    b_entry_event = copy.b_entry_event;
    events = copy.events;

    return *this;
  }

  TEventData(const TEventData& copy):
    occval(copy.occval),
    a_exit_event(copy.a_exit_event),
    b_exit_event(copy.b_exit_event),
    a_entry_event(copy.a_entry_event),
    b_entry_event(copy.b_entry_event),
    events( copy.events)
  {
  }

  void Reset()
  {
    occval = 127;
    a_exit_event = 1;
    b_exit_event = 1;
    a_entry_event = 1;
    b_entry_event = 1;
  }

  bool getBit(int ind)
  {
    if((events & (1<<ind))>0) return true;
    return false;
  }

  float computeShortTermOccupancy()
  {
    static float OCC_LOG_PROP = log(0.6/0.4);
    static float EMP_LOG_PROP = log(0.3/0.7);
    uint bs=0;

    if( (b_entry_event+b_exit_event-2) > 63) bs=64;
    else bs = b_entry_event+b_exit_event-2;
    uint cnt=0;
    float log_ogg_prob  = 0 ;
    for(uint i = 0; i<bs; i++)
    {
      if(getBit(i))
      {
        log_ogg_prob += OCC_LOG_PROP;
        cnt++;
      }
      else
      {
        log_ogg_prob += EMP_LOG_PROP;
      }
    }
    return (1.0- 1.0/(1.0+exp(log_ogg_prob)));
  }

  int getObservations()
  {
    return (b_entry_event+b_exit_event);
  }

  /**
    * Updates a new measurement
    */
  void updateSimple(uint8_t meas)
  {
    events = events<<1;
    if(meas > EVENTMAP_UNKNOWN)  ///OCC
    {
      events |= 1;
    }
    if(occval > EVENTMAP_UNKNOWN)  ///cell is considered Occupied
    {
      updateExitEvent(meas, 1.0);
    }
    else if(occval < EVENTMAP_UNKNOWN)
    {
      updateEntryEvent(meas,1.0);
    }
    else
    {
      occval = meas; ///Just update
    }
  }

  /**
    * Updates a new measurement
    */
  void updateSimple(uint8_t meas, float w)
  {
    events = events<<1;
    if(meas > EVENTMAP_UNKNOWN)  ///OCC
    {
      events |= 1;
    }
    if(occval > EVENTMAP_UNKNOWN)  ///cell is considered Occupied
    {
      updateExitEvent(meas, w);
    }
    else if(occval < EVENTMAP_UNKNOWN)
    {
      updateEntryEvent(meas,w);
    }
    else
    {
      occval = meas; ///Just update
    }
#ifdef EVENTMAP_USE_RECENCY_FILTERING
    performRecencyFiltering();
#endif
  }

  void performRecencyFiltering()
  {
    if(occval > EVENTMAP_UNKNOWN)  ///We are in Exit state
    {
      if(b_exit_event>EVENTMAP_OBSERVATION_LIMIT)
      {
        float w = EVENTMAP_OBSERVATION_LIMIT / b_exit_event;
        b_exit_event *= w;
        a_exit_event *= w;
      }
      a_entry_event = 1.0 + (a_entry_event-1.0) * Wforget;
      b_entry_event = 1.0 + (b_entry_event-1.0) * Wforget;
    }
    else
    {
      if(b_entry_event>EVENTMAP_OBSERVATION_LIMIT)
      {
        float w = EVENTMAP_OBSERVATION_LIMIT / b_entry_event;
        b_entry_event *= w;
        a_entry_event *= w;
      }
      a_exit_event = 1.0 + (a_exit_event-1.0) * Wforget;
      b_exit_event = 1.0 + (b_exit_event-1.0) * Wforget;
    }
  }

  /**
    * Update Exit event (cell is OCC)
    */
  void updateExitEvent(uint8_t meas, float w)
  {
    if(meas>EVENTMAP_UNKNOWN)  ///OCC
    {
      b_exit_event+=1.0;
    }
    else
    {
      if(w>1.0) fprintf(stderr,"EXIT W larger that one = %f\n ",w);
      a_exit_event+=w;
      b_exit_event+=1.0;
      occval = EVENTMAP_FREE;
    }
  }
  /**
    * Update Entry event (Cell is Free)
    */
  void updateEntryEvent(uint8_t meas, float w)
  {
    if(meas>EVENTMAP_UNKNOWN)  ///OCC
    {
      b_entry_event+=1.0;
      if(w>1.0) fprintf(stderr,"W larger that one = %f\n ",w);
      a_entry_event+=w;
      occval = EVENTMAP_OCCU;
    }
    else
    {
      b_entry_event+=1.0;
    }
  }

  ///Expected number of observations/event
  int getEntryN()
  {
    return (int)((float)b_entry_event/(float)a_entry_event +0.5);
  }
  ///Expected number of observations/event
  int getExitN()
  {
    return (int)((float)b_exit_event/(float)a_exit_event +0.5);
  }

  double entryL()
  {
    if(b_entry_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)a_entry_event/(double)b_entry_event );
  }
  double exitL()
  {
    if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)a_exit_event/(double)b_exit_event);
  }
  double L()
  {
    if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
    return ((double)(a_exit_event+a_entry_event)/(double)(b_exit_event+b_entry_event));
  }

  double fac(int n)
  {
    double t=1;
    for (int i=n; i>1; i--)
      t*=i;
    return t;
  }

  double Bin(int n,double p,int r)
  {
    return fac(n)/(fac(n-r)*fac(r))*pow(p,r)*pow(1-p,n-r);
  }

  float binaryBayesUpdate(float Pold, float P)
  {
    return (( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P))); 	///< Updated probability
  }

  /**
    * To prevent numerical difficulties
    */
  void normalizeProb(float &p)
  {
    if(p>0.999) p = 0.999;
    if(p<0.001) p = 0.001;

  }
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /// Compute different probabilities for the cell
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /**
    * Compute the probability of this cell being Static occupied.
    */
  float getOccStaticLikelihood()
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.5f;
    }
    Eigen::Matrix2f P;
    Eigen::Vector2f u1(0.5, 0.5);
    Eigen::Vector2f P0;
    float Lex = exitL();
    float Len = entryL();
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);

    P0 = u1.transpose() * P;
    return (P0(1));
  }

  /**
    * Returns the probability of static empty
    */
  float getFreeStaticLikelihood()
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.5f;
    }
    Eigen::Matrix2f P;
    Eigen::Vector2f P0;
    Eigen::Vector2f u1(0.5, 0.5);
    float Lex = exitL();
    float Len = entryL();
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);

    P0 = u1.transpose() * P;
    return (P0(0));
  }

  /**
    * Returns the probability of the cell having dynamics after 2^N
    * Observations. The larger the N, and higher the value, the more "Semi-static"
    * is the cell. Note that the filter returns
    * 0.5 >= P <=1.0 and P = 0.0 only if there is not enough evidence about the
    * Behaviour
    */
  float computeSemiStaticLikelihood(int N)
  {
    if( (b_entry_event + b_exit_event)<20.0 )
    {
      return 0.0f;
    }

    Eigen::Matrix2f P;
    Eigen::Vector2f u2(0, 1.0);
    Eigen::Vector2f u3(1.0, 0);
    float Lex = exitL();
    float Len = entryL();
    Eigen::Vector2f P2,P3;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    for(int i=0; i<N; i++) P = P*P;
    P2 = u2.transpose() * P;
    P3 = u3.transpose() * P;

    float Po = P2(1);
    float Pu = binaryBayesUpdate(Po, P3(0));
    normalizeProb(Pu);

    return Pu;
  }

  float getOccupancyNow()
  {
    if( (b_entry_event + b_exit_event)<50.0)
    {
      return 0.5f;
    }
    float Po = 0;

    Po = computeShortTermOccupancy();
    float Lex = exitL();
    float Len = entryL();
    Eigen::Matrix2f P;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    Eigen::Vector2f u1(1.0-Po, Po);
    Eigen::Vector2f u = u1.transpose() *P;
    return u(1);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77
  ///Predict
  /////////////////////////////////////////////////////////////////////////////////////////////77
  /////////////////////////////////////////////////////////////////////////////////////////////77

  float predictOccupancy(int N)
  {
    if( (b_entry_event + b_exit_event)<20.0)
    {
      return 0.5f;
    }
    ///Testing
    float ps = getOccStaticLikelihood();
    if(ps>0.6) return 0.9;
    ///end testing

    float Po = computeShortTermOccupancy();
    float Lex = exitL();
    float Len = entryL();
    Eigen::Matrix2f P;
    P(0,0) = (1.0-Len);
    P(0,1) =  Len;
    P(1,0)	 = Lex;
    P(1,1) = (1-Lex);
    Eigen::Vector2f u1(1.0-Po, Po);
    for(int i=0; i<N; i++) P = P*P;
    Eigen::Vector2f u = u1.transpose() *P;

    return u(1);
  }

};
namespace boost {
namespace serialization {

template<class Archive>
void serialize(Archive & ar, TEventData & g, const unsigned int version)
{
  ar & 	g.occval; 			///<Occupancy value
  ar &  g.a_exit_event; ///Number of exit events ( OCC 2 EMP)
  ar &  g.b_exit_event; ///Number of times the cell is perceived as occupied
  ar & 	g.a_entry_event;
  ar & 	g.b_entry_event; ///Number of times the cell is perceived as empty
  ar &  g.events;
}

} // namespace serialization
} // namespace boost
#endif


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

#include <stdio.h>
#include <math.h>
#include <boost/array.hpp>
#include <ros/ros.h>
#include <ros/time.h>
#include <vicon_bridge/Markers.h>
#include <vicon_bridge/Marker.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <wvu_swarm_std_msgs/vicon_bot.h>
#include <wvu_swarm_std_msgs/vicon_bot_array.h>
#include <wvu_swarm_std_msgs/robot_command.h>
#include <wvu_swarm_std_msgs/robot_command_array.h>
#include <wvu_swarm_std_msgs/vicon_point.h>
#include <wvu_swarm_std_msgs/vicon_points.h>

void msgCallback(const wvu_swarm_std_msgs::vicon_bot_array &msg) {}

// Method to find distance between two points
double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second);

// Method to populate a vector with a Gerono Lemniscate
//   Params: a vector to fill with the points, a value 'a' defining the curve,
//   and a value for an increment in degrees to iterate through 360 degrees
void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr);

int main(int argc, char **argv)
{
    // Initialize a node
    ros::init(argc, argv, "tracker");

    // Generates nodehandle, publisher, subscribers
    ros::NodeHandle n;
    ros::NodeHandle n_priv("~"); // private handle
    ros::Publisher pub;
    ros::Subscriber sub;

    //  Where to get the array of bots, what topic to advertise, where to get transform of target
    std::string viconArrayTopic, advertiseTopic, transformPrefix;

    //  Value of 'a' in formula in cm, value to iterate degrees, distance to "find" points at
    int lemniscateConstant, lemniscateInterval, cutoffRadius;

    // Import parameters from launchfile
    n_priv.param<std::string>("vicon_array_topic", viconArrayTopic, "/viconArray");
    n_priv.param<std::string>("advertise_topic", advertiseTopic, "target");
    n_priv.param<std::string>("transform_prefix", transformPrefix, "cardbot_");
    n_priv.param<int>("lemniscate_constant", lemniscateConstant, 50);
    n_priv.param<int>("lemniscate_interval", lemniscateInterval, 10);
    n_priv.param<int>("cutoff_radius", cutoffRadius, 10);

    // Sets loop rate at 100Hz
    ros::Rate rate(10);

    // Subscribe to tracker's vicon topic, advertise result vector
    sub = n.subscribe(viconArrayTopic, 10, &msgCallback);
    pub = n.advertise<wvu_swarm_std_msgs::vicon_points>(advertiseTopic, 1000);

    // Set up transform buffer
    tf2_ros::Buffer tfBuffer;

    // Set up transform listener
    tf2_ros::TransformListener tfListener(tfBuffer);

    // Generate a Gerono Lemniscate (figure 8) as discrete points
    std::vector<geometry_msgs::Point> path;
    genGeronoLemn(path, lemniscateConstant, lemniscateInterval);

    // Pick one point out of the path
    geometry_msgs::Point currentPoint = path.at(0);
    int currentIndex = 0;
    ROS_INFO("Point initialized at %f, %f\n", currentPoint.x, currentPoint.y);

    // Run while ros functions
    while(ros::ok())
    {
        // Get the information of the vicon array
        wvu_swarm_std_msgs::vicon_bot_array viconArray =
                *(ros::topic::waitForMessage<wvu_swarm_std_msgs::vicon_bot_array>(viconArrayTopic));

        double minimumPointDistance = 10000.0; // Trivially large number, in cm

        // Iterate through all bots
        for(wvu_swarm_std_msgs::vicon_bot iteratorBot : viconArray.poseVect)
        {
            // Find this bot's point
            geometry_msgs::Point botPt;
            botPt.x = iteratorBot.botPose.transform.translation.x;
            botPt.y = iteratorBot.botPose.transform.translation.y;
            botPt.z = 0;

            // Find distance from this bot to the point
            double thisDist = getDist(botPt, currentPoint);

            // Change minimum if necessary
            if(thisDist < minimumPointDistance) minimumPointDistance = thisDist;
        }

        // Move the point if any bot has come too close
        if(minimumPointDistance < cutoffRadius)
        {
            // Rotate through vector if needed
            if(currentIndex + 1 >= path.size())
                currentIndex = 0;
            else
                currentIndex++;

            currentPoint = path.at(currentIndex);

            ROS_INFO("Point moved to %f, %f\n", currentPoint.x, currentPoint.y);
        }

        // Publish target point to be grabbed by the Processor
        wvu_swarm_std_msgs::vicon_points output;
        wvu_swarm_std_msgs::vicon_point outputPt;
        outputPt.x = currentPoint.x;
        outputPt.y = currentPoint.y;
        output.point.push_back(outputPt);
        wvu_swarm_std_msgs::vicon_point dummyPt;
        dummyPt.x = 30.0;
        dummyPt.y = 30.0;
        output.point.push_back(dummyPt);
        pub.publish(output);

        ros::spinOnce();
        rate.sleep();
    }
}

double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second)
{
    double xDiff = first.x - second.x;
    double yDiff = first.y - second.y;
    double zDiff = first.z - second.z;

    return sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff);
}

void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr)
{
    // Iterate across 360 degrees
    for(int t = 0; t < 360; t += incr)
    {
        // Create a point
        geometry_msgs::Point temp;

        double rads = t * 3.14156265 / 180;

        // Do math to find its location
        //   This lemniscate is rotated 90 deg
        temp.x = a * cos(rads) * sin(rads);
        temp.y = a * sin(rads);
        temp.z = 0; //flatworld

        // Add to vector
        target.push_back(temp);

        ROS_INFO("Added point at %d degrees, or %f rads, at %f, %f, 0\n", t, rads, temp.x, temp.y);
    }
}


#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>
#include "ClassEngine.hh"
#include "CosmoTheory.hpp"
#include "CMB.hpp"
#include "class.h"

using namespace std;
using namespace imcmc;
using namespace imcmc::parser;

Data_CMB_Dist::Data_CMB_Dist(){
    // do nothing
    use_Hu_fitting = true;
}

Data_CMB_Dist::~Data_CMB_Dist(){
    // do nothing
}

void Data_CMB_Dist::Init( string& CMB_dist_prior_dataset ){

    data_info.GetInfo(CMB_dist_prior_dataset);

    string dist_prior_file = "";
    string inv_covmat_file = "";
	string dist_prior_std_file = ""; // used only for Planck distance prior
	format = -1;	// 0--WMAP; 1--Planck

	if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"format") ){
		format = Read::Read_Int_from_File(CMB_dist_prior_dataset,"format");
		if( format != 0 && format != 1 ){
			MPI_ClassMC_ERROR("format can only take 0 or 1 !");
		}
	}
	else{
		MPI_ClassMC_ERROR("the key \'format\' can not be found in file: "+CMB_dist_prior_dataset);
	}

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"use_Hu_fitting") )
        use_Hu_fitting = Read::Read_Bool_from_File(CMB_dist_prior_dataset,"use_Hu_fitting");
    else{
        MPI_ClassMC_WARNING("use_Hu_fitting is not found, so use default value \'true\'.");
        // exit(0);
    }

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"dist_prior_file") )
        dist_prior_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"dist_prior_file");
    else
        MPI_ClassMC_ERROR("dist_prior_file not set in: "+CMB_dist_prior_dataset);

    if( Read::Has_Key_in_File(CMB_dist_prior_dataset,"inv_covmat_file") )
        inv_covmat_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"inv_covmat_file");
    else
        MPI_ClassMC_ERROR("inv_covmat_file not set in: "+CMB_dist_prior_dataset);

	if( format == 0 ){
    	distance_prior  = arma::zeros(1,3);
    	covmat_inv      = arma::zeros(3,3);

    	distance_prior.load(dist_prior_file,arma::auto_detect); // loading CMB distance prior {lA,R,z_rec}
    	covmat_inv.load(inv_covmat_file,arma::auto_detect);     // loading covariance matrix
	}
	else if( format == 1 ){
//		cout << "loading Planck distance prior data ...\n";
		arma::rowvec tmp = arma::zeros(1,3);
		string std_file = Read::Read_String_from_File(CMB_dist_prior_dataset,"dist_prior_std_file");
		tmp.load(std_file,arma::auto_detect);
		std_R 	= tmp(0);
		std_lA	= tmp(1);
		std_Obh2= tmp(2);

		distance_prior_plk = arma::zeros(1,3);
		covmat_inv_plk     = arma::zeros(3,3);

    //  read Planck distance prior data
		distance_prior_plk.load(dist_prior_file,arma::auto_detect);

    //  read the normalized covariance matrix
		covmat_inv_plk.load(inv_covmat_file,arma::auto_detect);

    //  convert to the original covariance matrix
        covmat_inv_plk(0,0) *= std_R*std_R;
        covmat_inv_plk(0,1) *= std_R*std_lA;
        covmat_inv_plk(0,2) *= std_R*std_Obh2;

		covmat_inv_plk(1,0) *= std_lA*std_R;
		covmat_inv_plk(1,1) *= std_lA*std_lA;
		covmat_inv_plk(1,2) *= std_lA*std_Obh2;

		covmat_inv_plk(2,0) *= std_Obh2*std_R;
		covmat_inv_plk(2,1) *= std_Obh2*std_lA;
		covmat_inv_plk(2,2) *= std_Obh2*std_Obh2;

        covmat_inv_plk = covmat_inv_plk.i();
	}

}

double Prior_CMB_Dist(  imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state ){

    chisq = lndet = 0.0;
    CosmoTheory *cosmo = static_cast<CosmoTheory*>(model);
    CMB_Dist    *cmbdist = static_cast<CMB_Dist*>(data);

    double lA, R, z_rec; // this format matches WMAP7/9 distance prior data

    double H0   = cosmo->engine->get_H0();
    double Om0  = cosmo->engine->get_Omega_m();
    double ra_rec, rs_rec;

    if( cmbdist->use_Hu_fitting ){
        z_rec  = cosmo->engine->z_rec_Hu();
        ra_rec = cosmo->engine->get_Da(z_rec)*(1.+z_rec)/cosmo->engine->get_a_today();
        rs_rec = cosmo->engine->get_rs(z_rec);
    }
    else{
        z_rec  = cosmo->engine->z_rec();
        ra_rec = cosmo->engine->ra_rec();
        rs_rec = cosmo->engine->rs_rec();
    }

    lA = _PI_*ra_rec/rs_rec;
    R  = sqrt(Om0)*H0*ra_rec/(_c_*1e-3);

    arma::rowvec residual = arma::zeros(1,3);

    switch (cmbdist->format){
        case 0:
        // for WMAP distance prior
            residual(0) = lA;
            residual(1) = R;
            residual(2)	= z_rec;
            residual = residual - cmbdist->distance_prior;
            chisq = arma::as_scalar( residual * cmbdist->covmat_inv * residual.t() );
            break;
        case 1:
        // for Planck distance prior
            residual(0) = R;
            residual(1) = lA;
            residual(2)	= cosmo->engine->get_Omega_b()*(H0/100)*(H0/100);
            residual = residual - cmbdist->distance_prior_plk;
            chisq = arma::as_scalar( residual * cmbdist->covmat_inv_plk * residual.t() );
            break;
        default:
            break;
    }

    return lndet - 0.5*chisq;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/LevenbergMarquardt.h>
#include <vw/Math/Matrix.h>
#include <vw/Camera/CameraModel.h>
#include <asp/IsisIO/IsisInterfaceLineScan.h>

#include <algorithm>
#include <vector>

#include <Camera.h>
#include <CameraDetectorMap.h>
#include <CameraDistortionMap.h>
#include <CameraFocalPlaneMap.h>
#include <iTime.h>

#include <boost/smart_ptr/scoped_ptr.hpp>

using namespace vw;
using namespace asp;
using namespace asp::isis;

// Construct
IsisInterfaceLineScan::IsisInterfaceLineScan(std::string const& filename): IsisInterface(filename), m_alphacube(*m_cube) {

  // Gutting Isis::Camera
  m_distortmap = m_camera->DistortionMap();
  m_focalmap   = m_camera->FocalPlaneMap();
  m_detectmap  = m_camera->DetectorMap();
}

// Custom Function to help avoid over invoking the deeply buried
// functions of Isis::Sensor
void IsisInterfaceLineScan::SetTime(Vector2 const& px, bool calc_pose) const {
  if (px != m_c_location) {
    m_c_location = px;
    m_detectmap->SetParent(m_alphacube.AlphaSample(px[0]),
                           m_alphacube.AlphaLine(px[1]));

    if (calc_pose) {
      // Calculating the spacecraft position and orientation (hence pose)
      m_camera->instrumentPosition(&m_center[0]);
      m_center *= 1000;

      std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
      std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
      MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
      MatrixProxy<double,3,3> R_body(&(rot_body[0]));
      m_pose = Quat(R_body*transpose(R_inst));
    }
  }
}

class EphemerisLMA : public vw::math::LeastSquaresModelBase<EphemerisLMA> {
  vw::Vector3 m_point;
  Isis::Camera* m_camera;
  Isis::CameraDistortionMap *m_distortmap;
  Isis::CameraFocalPlaneMap *m_focalmap;
public:
  typedef vw::Vector<double> result_type; // Back project result
  typedef vw::Vector<double> domain_type; // Ephemeris time
  typedef vw::Matrix<double> jacobian_type;

  inline EphemerisLMA(vw::Vector3 const& point,
                      Isis::Camera* camera,
                      Isis::CameraDistortionMap* distortmap,
                      Isis::CameraFocalPlaneMap* focalmap) : m_point(point), m_camera(camera), m_distortmap(distortmap), m_focalmap(focalmap) {}

  inline result_type operator()(domain_type const& x) const;
};

// LMA for projecting point to linescan camera
EphemerisLMA::result_type
EphemerisLMA::operator()(EphemerisLMA::domain_type const& x) const {

  // Setting Ephemeris Time
  m_camera->setTime(Isis::iTime(x[0]));

  // Calculating the look direction in camera frame
  Vector3 instru;
  m_camera->instrumentPosition(&instru[0]);
  instru *= 1000;  // Spice gives in km
  Vector3 lookB = normalize(m_point - instru);
  std::vector<double> lookB_copy(3);
  std::copy(lookB.begin(), lookB.end(), lookB_copy.begin());
  std::vector<double> lookJ = m_camera->bodyRotation()->J2000Vector(lookB_copy);
  std::vector<double> lookC = m_camera->instrumentRotation()->ReferenceVector(lookJ);
  Vector3 look;
  std::copy(lookC.begin(), lookC.end(), look.begin());

  // Projecting to mm focal plane
  look = m_camera->FocalLength() * (look / look[2]);
  m_distortmap->SetUndistortedFocalPlane(look[0], look[1]);
  m_focalmap->SetFocalPlane(m_distortmap->FocalPlaneX(),
                            m_distortmap->FocalPlaneY());
  result_type result(1);
  // Not exactly sure about lineoffset .. but ISIS does it
  result[0] = m_focalmap->DetectorLineOffset() - m_focalmap->DetectorLine();

  return result;
}

Vector2
IsisInterfaceLineScan::point_to_pixel(Vector3 const& point) const {

  // First seed LMA with an ephemeris time in the middle of the image
  double middle = lines() / 2;
  m_detectmap->SetParent(1, m_alphacube.AlphaLine(middle));
  double start_e = m_camera->time().Et();

  // Build LMA
  EphemerisLMA model(point, m_camera.get(), m_distortmap, m_focalmap);
  int status;
  Vector<double> objective(1), start(1);
  start[0] = start_e;
  Vector<double> solution_e = math::levenberg_marquardt(model,
                                                        start,
                                                        objective,
                                                        status);

  // Make sure we found ideal time
  VW_ASSERT(status > 0, vw::camera::PointToPixelErr() << " Unable to project point into ISIS linescan camera ");

  // Converting now to pixel
  m_camera->setTime(Isis::iTime(solution_e[0]));

  // Working out pointing
  m_camera->instrumentPosition(&m_center[0]);
  m_center *= 1000;
  Vector3 look = normalize(point-m_center);

  // Calculating rotation to camera frame
  std::vector<double> rot_inst = m_camera->instrumentRotation()->Matrix();
  std::vector<double> rot_body = m_camera->bodyRotation()->Matrix();
  MatrixProxy<double,3,3> R_inst(&(rot_inst[0]));
  MatrixProxy<double,3,3> R_body(&(rot_body[0]));
  m_pose = Quat(R_body*transpose(R_inst));

  look = inverse(m_pose).rotate(look);
  look = m_camera->FocalLength() * (look / look[2]);
  m_distortmap->SetUndistortedFocalPlane(look[0], look[1]);
  m_focalmap->SetFocalPlane(m_distortmap->FocalPlaneX(),
                            m_distortmap->FocalPlaneY());
  m_detectmap->SetDetector(m_focalmap->DetectorSample(),
                           m_focalmap->DetectorLine());
  Vector2 pixel(m_detectmap->ParentSample(),
                m_detectmap->ParentLine());
  pixel[0] = m_alphacube.BetaSample(pixel[0]);
  pixel[1] = m_alphacube.BetaLine(pixel[1]);
  SetTime(pixel, false);

  pixel -= Vector2(1,1);
  return pixel;
}

Vector3
IsisInterfaceLineScan::pixel_to_vector(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);

  // Projecting to get look direction
  Vector3 result;
  m_focalmap->SetDetector(m_detectmap->DetectorSample(),
                          m_detectmap->DetectorLine());
  m_distortmap->SetFocalPlane(m_focalmap->FocalPlaneX(),
                              m_focalmap->FocalPlaneY());
  result[0] = m_distortmap->UndistortedFocalPlaneX();
  result[1] = m_distortmap->UndistortedFocalPlaneY();
  result[2] = m_distortmap->UndistortedFocalPlaneZ();
  result = normalize(result);
  result = m_pose.rotate(result);
  return result;
}

Vector3
IsisInterfaceLineScan::camera_center(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);
  return m_center;
}

Quat
IsisInterfaceLineScan::camera_pose(Vector2 const& pix) const {
  Vector2 px = pix + Vector2(1,1);
  SetTime(px, true);
  return m_pose;
}