/*
 * LEGAL NOTICE
 * This computer software was prepared by Battelle Memorial Institute,
 * hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
 * with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
 * CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
 * LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
 * sentence must appear on any copies of this computer software.
 *
 * EXPORT CONTROL
 * User agrees that the Software will not be shipped, transferred or
 * exported into any country or used in any manner prohibited by the
 * United States Export Administration Act or any other applicable
 * export laws, restrictions or regulations (collectively the "Export Laws").
 * Export of the Software may require some form of license or other
 * authority from the U.S. Government, and failure to obtain such
 * export control license may result in criminal liability under
 * U.S. laws. In addition, if the Software is identified as export controlled
 * items under the Export Laws, User represents and warrants that User
 * is not a citizen, or otherwise located within, an embargoed nation
 * (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
 *     and that User is not otherwise prohibited
 * under the Export Laws from receiving the Software.
 *
 * Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
 * Distributed as open-source under the terms of the Educational Community
 * License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
 *
 * For further details, see: http://www.globalchange.umd.edu/models/gcam/
 *
 */

/*!
* \file get_glm_data.h
* \ingroup Objects
* \brief The GetGLMData class source file.
*
* \author Pralit Patel
* \author Sonny Kim
*/

#include <boost/lexical_cast.hpp>

#include "util/base/include/definitions.h"

#include "reporting/include/get_glm_data.h"
#include "containers/include/region.h"
#include "land_allocator/include/land_leaf.h"
#include "land_allocator/include/aland_allocator_item.h"
#include "technologies/include/ag_production_technology.h"
#include "ccarbon_model/include/icarbon_calc.h"
#include "containers/include/scenario.h"
#include "util/base/include/model_time.h"

using namespace std;
using namespace objects;

extern Scenario* scenario;

//! Default Constructor
GetGLMData::GetGLMData()
{
     //TODO: Hard coding renames here for the moment.  These should be read-in
     // through data or otherwise set exogenously to allow felxability in changes
     // to regional or land cover categorization.
     // mRegionMap[ GCAM Region ] = GLM Region
     mRegionMap[ "USA" ] = "USA";
     mRegionMap[ "Canada" ] = "Canada";
     mRegionMap[ "Western Europe" ] = "Western Europe";
     mRegionMap[ "Japan" ] = "Japan";
     mRegionMap[ "Australia_NZ" ] = "Australia_NZ";
     mRegionMap[ "Former Soviet Union" ] = "Former Soviet Union";
     mRegionMap[ "China" ] = "China";
     mRegionMap[ "Middle East" ] = "Middle East";
     mRegionMap[ "Africa" ] = "Africa";
     mRegionMap[ "Latin America" ] = "Latin America";
     mRegionMap[ "Southeast Asia" ] = "Southeast Asia";
     mRegionMap[ "Eastern Europe" ] = "Eastern Europe";
     mRegionMap[ "Korea" ] = "Korea";
     mRegionMap[ "India" ] = "India";

     mLandMap[ "biomass" ] = "Cropland";
     mLandMap[ "eucalyptus" ] = "Cropland";
     mLandMap[ "miscanthus" ] = "Cropland";
     mLandMap[ "willow" ] = "Cropland";
     mLandMap[ "Jatropha" ] = "Cropland";

     mLandMap[ "Corn" ] = "Cropland";
     mLandMap[ "FiberCrop" ] = "Cropland";
     mLandMap[ "OtherArableLand" ] = "Cropland";
     mLandMap[ "OtherGrain" ] = "Cropland";
     mLandMap[ "Rice" ] = "Cropland";
     mLandMap[ "SugarCrop" ] = "Cropland";
     mLandMap[ "Wheat" ] = "Cropland";

     mLandMap[ "OilCrop" ] = "Cropland";
     mLandMap[ "PalmFruit" ] = "Cropland";

     mLandMap[ "MiscCrop" ] = "Cropland";
     mLandMap[ "Root_Tuber" ] = "Cropland";

     mLandMap[ "FodderHerb" ] = "Cropland";
     mLandMap[ "FodderGrass" ] = "Cropland";

     mLandMap[ "Forest" ] = "Forest";
     mLandMap[ "UnmanagedForest" ] = "Forest";

     mLandMap[ "Grassland" ] = "Grassland";
     mLandMap[ "Shrubland" ] = "Grassland";
     mLandMap[ "Tundra" ] = "Grassland";

     mLandMap[ "Pasture" ] = "Pasture";
     mLandMap[ "UnmanagedPasture" ] = "Pasture";

     mLandMap[ "UrbanLand" ] = "Build-up";

     mLandMap[ "RockIceDesert" ] = "Other Land";

     // Crop name mapping for crop production
     mCropMap[ "Forest" ] = "Forest";

     // Land name mapping for carbon densities
     // TODO: create these mappings
}

/*!
 * \brief Get the land cover for a given GLM region and land category in the given year.
 * \details If data was requested for and unknown region/AEZ/category/year -1 will be returned.
 * \param aGLMRegionName A region name.
 * \param aAEZ The AEZ number.
 * \param aGLMLandCategory A land cover category.
 * \param aYear A GCAM model year in which to get data.  TODO: do we want to allow interpolating?
 * \return The requested land cover or -1 for error.
 */
double GetGLMData::getLandCover( const string& aGLMRegionName,
                                 const int aAEZ,
                                 const string& aGLMLandCategory,
                                 const int aYear ) const
{
    const Modeltime* modeltime = scenario->getModeltime();
    const int modelPeriod = modeltime->getyr_to_per( aYear );
    if( modelPeriod == 0 ) {
        return -1;
    }
    RegAEZLandMap::const_iterator regionIt = mLandCoverData.find( aGLMRegionName );
    if( regionIt != mLandCoverData.end() ) {
        map<int, map<string, PeriodVector<double> > >::const_iterator aezIt =
            (*regionIt).second.find( aAEZ );
        if( aezIt != (*regionIt).second.end() ) {
            map<string, PeriodVector<double> >::const_iterator categoryIt =
                (*aezIt).second.find( aGLMLandCategory );
            if( categoryIt != (*aezIt).second.end() ) {
                return (*categoryIt).second[ modelPeriod ];
            }
        }
    }

    // Did not find the region/category
    return -1;
}

/*!
 * \brief Get the crop production in carbon for a given GLM region and category in the given year.
 * \details If data was requested for and unknown region/AEZ/category/year -1 will be returned.
 * \param aGLMRegionName A region name.
 * \param aAEZ The AEZ number.
 * \param aGLMLandCategory A crop category.
 * \param aYear A GCAM model year in which to get data.  TODO: do we want to allow interpolating?
 * \return The requested crop production or -1 for error.
 * \warning Only Forest is currently available and it's units are in tC.
 */
double GetGLMData::getProductionInCarbon( const string& aGLMRegionName,
                                          const int aAEZ,
                                          const string& aGLMCrop,
                                          const int aYear ) const
{
    const Modeltime* modeltime = scenario->getModeltime();
    const int modelPeriod = modeltime->getyr_to_per( aYear );
    if( modelPeriod == 0 ) {
        return -1;
    }
    RegAEZLandMap::const_iterator regionIt = mProductionData.find( aGLMRegionName );
    if( regionIt != mProductionData.end() ) {
        map<int, map<string, PeriodVector<double> > >::const_iterator aezIt =
            (*regionIt).second.find( aAEZ );
        if( aezIt != (*regionIt).second.end() ) {
            map<string, PeriodVector<double> >::const_iterator categoryIt =
                (*aezIt).second.find( aGLMCrop );
            if( categoryIt != (*aezIt).second.end() ) {
                return (*categoryIt).second[ modelPeriod ];
            }
        }
    }

    // Did not find the region/category
    return -1;
}

/*!
 * \brief Get the above and below carbon densities for a given GLM region and land category in the given year.
 * \details If data was requested for and unknown region/AEZ/category/year -1 will be returned.
 * \param aGLMRegionName A region name.
 * \param aAEZ The AEZ number.
 * \param aGLMLandCategory A land cover category.
 * \param aYear A GCAM model year in which to get data.  TODO: do we want to allow interpolating?
 * \return A pair of (above, below) carbon densities.  In case of error both values with be -1.
 */
pair<double, double> GetGLMData::getCarbonDensity( const string& aGLMRegionName,
                                                   const int aAEZ,
                                                   const string& aGLMLandCategory,
                                                   const int aYear ) const
{
    CarbonDensityMap::const_iterator regionIt = mCarbonDensityData.find( aGLMRegionName );
    if( regionIt != mCarbonDensityData.end() ) {
        map<int, map<string, map<int, pair<double, double> > > >::const_iterator aezIt =
            (*regionIt).second.find( aAEZ );
        if( aezIt != (*regionIt).second.end() ) {
            map<string, map<int, pair<double, double> > >::const_iterator categoryIt =
                (*aezIt).second.find( aGLMLandCategory );
            if( categoryIt != (*aezIt).second.end() ) {
                map<int, pair<double, double> >::const_iterator yearIt =
                    (*categoryIt).second.find( aYear );
                if( yearIt != (*categoryIt).second.end() ) {
                    return (*yearIt).second;
                }
            }
        }
    }

    // Did not find the region/category
    return make_pair<double, double>( -1, -1 );
}

void GetGLMData::startVisitRegion( const Region* aRegion, const int aPeriod ) {
    // Store GLM region for later use when we have data to store
    mCurrentGLMRegionName = doMapLookup( mRegionMap, aRegion->getName() );
}

void GetGLMData::startVisitLandLeaf( const LandLeaf* aLandLeaf, const int aPeriod ) {
    const Modeltime* modeltime = scenario->getModeltime();
    // Allow summing of individual periods or all periods (the -1 flag)
    const int startPeriod = aPeriod == -1 ? 0 : aPeriod;
    const int endPeriod = aPeriod == -1 ? modeltime->getmaxper() - 1 : aPeriod;
    pair<string, int> nameAEZ = parseTypeAndAEZFromName( aLandLeaf->getName() );
    PeriodVector<double>& data = mLandCoverData[ mCurrentGLMRegionName ][ nameAEZ.second ][ doMapLookup( mLandMap, nameAEZ.first ) ];
    for( int period = startPeriod; period <= endPeriod; ++period ) {
        data[ period ] += aLandLeaf->getLandAllocation( aLandLeaf->getName(), period );
    }

    // TODO: behavior if different GCAM land types that are mapped to the same GLM land type
    // have different carbon densities this will likely not give the expeceted result.
    map<int, pair<double, double> >& carbonDensities = mCarbonDensityData[ mCurrentGLMRegionName ][ nameAEZ.second ][ doMapLookup( mLandMapForDensities, nameAEZ.first ) ];
    const int startYear = modeltime->getper_to_yr( startPeriod );
    const int endYear = modeltime->getper_to_yr( endPeriod );
    for( int year = startYear; year <= endYear; ++year ) {
        pair<double, double>& density = carbonDensities[ year ];
        density.first = aLandLeaf->getCarbonContentCalc()->getActualAboveGroundCarbonDensity( year );
        density.second = aLandLeaf->getCarbonContentCalc()->getActualBelowGroundCarbonDensity( year );
    }
}

void GetGLMData::startVisitAgProductionTechnology( const AgProductionTechnology* aAgTech, const int aPeriod ) {
    pair<string, int> nameAEZ = parseTypeAndAEZFromName( aAgTech->getName() );
    if( nameAEZ.first == "Forest" ) {
        // Note the hard coding of the forest to carbon conversion factor
        // The output of the forest technology is in billion m^3 and we want
        // the production in tC so this conversion factor is tC / billion m^3
        const double B_CUBIC_METER_FOREST_TO_CARBON = 0.288 * 1e9;

        const Modeltime* modeltime = scenario->getModeltime();
        // Technologies have special behavior when visiting such that aPeriod is really
        // the end period and we must loop from 0.
        PeriodVector<double>& data = mProductionData[ mCurrentGLMRegionName ][ nameAEZ.second ][ doMapLookup( mCropMap, nameAEZ.first ) ];
        for( int period = 0; period <= aPeriod; ++period ) {
            data[ period ] +=
                aAgTech->getOutput( period ) * B_CUBIC_METER_FOREST_TO_CARBON;
        }
    }
}

/*!
 * \brief A helper method to do a rename lookup from a GCAM name to a GLM name.
 * \details If an explicit rename was not provided the GCAM name will be used.
 * \param The name lookup map.
 * \param The name to translate.
 * \return The GLM name if a lookup exists otherwise just the GCAM name.
 */
const string& GetGLMData::doMapLookup( const map<string, string>& aLookupMap, const string& aKey ) {
    map<string, string>::const_iterator lookupIt = aLookupMap.find( aKey );
    if( lookupIt != aLookupMap.end() ) {
        return (*lookupIt).second;
    }
    else {
        // TODO: warn?
        return aKey;
    }
}

pair<string, int> GetGLMData::parseTypeAndAEZFromName( const string& aGCAMName ) {
    size_t index = aGCAMName.find( "AEZ" );
    if( index == string::npos ) {
        // TODO: warn?
        return make_pair<string, int>( aGCAMName, 0 );
    }
    else {
        string typeName( aGCAMName, 0, index);
        string aezStr( aGCAMName, index + 3, string::npos );
        int aez = boost::lexical_cast<int>( aezStr );
        return make_pair<string, int>( typeName, aez );
    }
}


//=============================================================================================================
/**
* @file     fiff_coord_trans_old.cpp
* @author   Christoph Dinh <chdinh@nmr.mgh.harvard.edu>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     January, 2017
*
* @section  LICENSE
*
* Copyright (C) 2017, Christoph Dinh and Matti Hamalainen. 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 MNE-CPP authors 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.
*
*
* @brief    Implementation of the FiffCoordTransOld Class.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "fiff_coord_trans_old.h"

#include <fiff/fiff_tag.h>

#include <QFile>

#include <Eigen/Dense>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace FIFFLIB;
using namespace FIFFLIB;

#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

#ifndef FAIL
#define FAIL -1
#endif

#ifndef OK
#define OK 0
#endif

#define X_20 0
#define Y_20 1
#define Z_20 2

#define FREE_20(x) if ((char *)(x) != NULL) free((char *)(x))

#define MALLOC_20(x,t) (t *)malloc((x)*sizeof(t))

#define MAXWORD 1000

static void skip_comments(FILE *in)

{
    int c;

    while (1) {
        c = fgetc(in);
        if (c == '#') {
            for (c = fgetc(in); c != EOF && c != '\n'; c = fgetc(in))
                ;
        }
        else {
            ungetc(c,in);
            return;
        }
    }
}

static int whitespace(int c)

{
    if (c == '\t' || c == '\n' || c == ' ')
        return TRUE;
    else
        return FALSE;
}

static int whitespace_quote(int c, int inquote)

{
    if (inquote)
        return (c == '"');
    else
        return (c == '\t' || c == '\n' || c == ' ');
}

static char *next_word_20(FILE *in)

{
    char *next = MALLOC_20(MAXWORD,char);
    int c;
    int  p,k;
    int  inquote;

    skip_comments(in);

    inquote = FALSE;
    for (k = 0, p = 0, c = fgetc(in); c != EOF && !whitespace_quote(c,inquote) ; c = fgetc(in), k++) {
        if (k == 0 && c == '"')
            inquote = TRUE;
        else
            next[p++] = c;
    }
    if (c == EOF && k == 0) {
        FREE_20(next);
        return NULL;
    }
    else
        next[p] = '\0';
    if (c != EOF) {
        for (k = 0, c = fgetc(in); whitespace(c) ; c = fgetc(in), k++)
            ;
        if (c != EOF)
            ungetc(c,in);
    }
#ifdef DEBUG
    if (next)
        printf("<%s>\n",next);
#endif
    return next;
}

static int get_fval_20(FILE *in, float *fval)
{
    char *next = next_word_20(in);
    if (next == NULL) {
        qWarning("bad integer");
        return FAIL;
    }
    else if (sscanf(next,"%g",fval) != 1) {
        qWarning("bad floating point number : %s",next);
        FREE_20(next);
        return FAIL;
    }
    FREE_20(next);
    return OK;
}

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

FiffCoordTransOld::FiffCoordTransOld()
{

}

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

FiffCoordTransOld::FiffCoordTransOld(const FiffCoordTransOld &p_FiffCoordTransOld)
{
    this->from = p_FiffCoordTransOld.from;
    this->to = p_FiffCoordTransOld.to;

    for (int j = 0; j < 3; j++) {
        this->move[j] = p_FiffCoordTransOld.move[j];
        this->invmove[j] = p_FiffCoordTransOld.invmove[j];
        for (int k = 0; k < 3; k++) {
            this->rot(j,k) = p_FiffCoordTransOld.rot(j,k);
            this->invrot(j,k) = p_FiffCoordTransOld.invrot(j,k);
        }
    }
}

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

FiffCoordTransOld *FiffCoordTransOld::catenate(FiffCoordTransOld *t1, FiffCoordTransOld *t2)
{
    FiffCoordTransOld* t = new FiffCoordTransOld;
    int j,k,p;

    t->to   = t1->to;
    t->from = t2->from;

    for (j = 0; j < 3; j++) {
        t->move[j] = t1->move[j];
        for (k = 0; k < 3; k++) {
            t->rot(j,k) = 0.0;
            t->move[j] += t1->rot(j,k)*t2->move[k];
            for (p = 0; p < 3; p++)
                t->rot(j,k) += t1->rot(j,p)*t2->rot(p,k);
        }
    }
    add_inverse(t);
    return (t);
}

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

FiffCoordTransOld::FiffCoordTransOld(int from, int to, float rot[3][3], float move[3])
{
    this->from = from;
    this->to   = to;

    for (int j = 0; j < 3; j++) {
        this->move[j] = move[j];
        for (int k = 0; k < 3; k++)
            this->rot(j,k) = rot[j][k];
    }
    add_inverse(this);
}

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

FiffCoordTransOld::~FiffCoordTransOld()
{

}

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

int FiffCoordTransOld::add_inverse(FiffCoordTransOld *t)
{
    int   j,k;
    Matrix4f m;

    for (j = 0; j < 3; j++) {
        for (k = 0; k < 3; k++)
            m(j,k) = t->rot(j,k);
        m(j,3) = t->move[j];
    }
    for (k = 0; k < 3; k++)
        m(3,k) = 0.0;
    m(3,3) = 1.0;

    m = m.inverse().eval();

    for (j = 0; j < 3; j++) {
        for (k = 0; k < 3; k++)
            t->invrot(j,k) = m(j,k);
        t->invmove[j] = m(j,3);
    }
    return OK;
}

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

FiffCoordTransOld *FiffCoordTransOld::fiff_invert_transform() const
{
    FiffCoordTransOld* ti = new FiffCoordTransOld;
    int j,k;

    for (j = 0; j < 3; j++) {
        ti->move[j] = this->invmove[j];
        ti->invmove[j] = this->move[j];
        for (k = 0; k < 3; k++) {
            ti->rot(j,k)    = this->invrot(j,k);
            ti->invrot(j,k) = this->rot(j,k);
        }
    }
    ti->from = this->to;
    ti->to   = this->from;
    return (ti);
}

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

void FiffCoordTransOld::fiff_coord_trans(float r[], const FiffCoordTransOld *t, int do_move)
/*
* Apply coordinate transformation
*/
{
    int j,k;
    float res[3];

    for (j = 0; j < 3; j++) {
        res[j] = (do_move ? t->move[j] :  0.0);
        for (k = 0; k < 3; k++)
            res[j] += t->rot(j,k)*r[k];
    }
    for (j = 0; j < 3; j++)
        r[j] = res[j];
}

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

FiffCoordTransOld *FiffCoordTransOld::fiff_combine_transforms(int from, int to, FiffCoordTransOld *t1, FiffCoordTransOld *t2)
/*
* Combine two coordinate transformations
* to yield a transform from 'from' system
* to 'to' system.
*
* Return NULL if this fails
*
*/
{
    FiffCoordTransOld* t = NULL;
    int swapped = 0;
    FiffCoordTransOld* temp;
    /*
       * We have a total of eight possibilities:
       * four without swapping and four with
       */
    while (1) {
        if (t1->to == to && t2->from == from) {
            t1 = new FiffCoordTransOld(*t1);
            t2 = new FiffCoordTransOld(*t2);
            break;
        }
        else if (t1->from == to && t2->from == from) {
            FiffCoordTransOld* tmp_t1 = t1;
            t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
            delete tmp_t1;
            t2 = new FiffCoordTransOld(*t2);
            break;
        }
        else if (t1->to == to && t2->to == from) {
            t1 = new FiffCoordTransOld(*t1);
            FiffCoordTransOld* tmp_t2 = t2;
            t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
            delete tmp_t2;
            break;
        }
        else if (t1->from == to && t2->to == from) {
            FiffCoordTransOld* tmp_t1 = t1;
            t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
            delete tmp_t1;
            FiffCoordTransOld* tmp_t2 = t2;
            t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
            delete tmp_t2;
            break;
        }
        if (swapped) {  /* Already swapped and not found */
            qCritical("Cannot combine coordinate transforms");
            return (t);
        }
        temp = t1;      /* Try it swapped as well */
        t1 = t2;
        t2 = temp;
        swapped = 1;
    }
    if (t1->from  != t2->to)    /* Transforms must match on the other side as well */
        qCritical ("Cannot combine coordinate transforms");
    else                        /* We can do it directly */
        t = catenate(t1,t2);
    FREE_20(t1); FREE_20(t2);
    return (t);
}

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

void FiffCoordTransOld::fiff_coord_trans_inv(float r[], FiffCoordTransOld *t, int do_move)
/*
* Apply inverse coordinate transformation
*/
{
    int j,k;
    float res[3];

    for (j = 0; j < 3; j++) {
        res[j] = (do_move ? t->invmove[j] :  0.0);
        for (k = 0; k < 3; k++)
            res[j] += t->invrot(j,k)*r[k];
    }
    for (j = 0; j < 3; j++)
        r[j] = res[j];
}

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

typedef struct {
    int frame;
    const char *name;
} frameNameRec;

const char *FiffCoordTransOld::mne_coord_frame_name(int frame)
{
    static frameNameRec frames[] = {
        {FIFFV_COORD_UNKNOWN,"unknown"},
        {FIFFV_COORD_DEVICE,"MEG device"},
        {FIFFV_COORD_ISOTRAK,"isotrak"},
        {FIFFV_COORD_HPI,"hpi"},
        {FIFFV_COORD_HEAD,"head"},
        {FIFFV_COORD_MRI,"MRI (surface RAS)"},
        {FIFFV_MNE_COORD_MRI_VOXEL, "MRI voxel"},
        {FIFFV_COORD_MRI_SLICE,"MRI slice"},
        {FIFFV_COORD_MRI_DISPLAY,"MRI display"},
        {FIFFV_MNE_COORD_CTF_DEVICE,"CTF MEG device"},
        {FIFFV_MNE_COORD_CTF_HEAD,"CTF/4D/KIT head"},
        {FIFFV_MNE_COORD_RAS,"RAS (non-zero origin)"},
        {FIFFV_MNE_COORD_MNI_TAL,"MNI Talairach"},
        {FIFFV_MNE_COORD_FS_TAL_GTZ,"Talairach (MNI z > 0)"},
        {FIFFV_MNE_COORD_FS_TAL_LTZ,"Talairach (MNI z < 0)"},
        {-1,"unknown"}
    };
    int k;
    for (k = 0; frames[k].frame != -1; k++) {
        if (frame == frames[k].frame)
            return frames[k].name;
    }
    return frames[k].name;
}

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

void FiffCoordTransOld::mne_print_coord_transform_label(FILE *log, char *label, FiffCoordTransOld *t)
{
    int k,p;
    int frame;
    if (!label || strlen(label) == 0)
        fprintf(log,"Coordinate transformation: ");
    else
        fprintf(log,"%s",label);
    for (frame = t->from, k = 0; k < 2; k++) {
        if (k == 0) {
            fprintf(log,"%s -> ",mne_coord_frame_name(frame));
            frame = t->to;
        }
        else {
            fprintf(log,"%s\n",mne_coord_frame_name(frame));
            for (p = 0; p < 3; p++)
                fprintf(log,"\t% 8.6f % 8.6f % 8.6f\t% 7.2f mm\n",
                        t->rot(p,X_20),t->rot(p,Y_20),t->rot(p,Z_20),1000*t->move[p]);
            fprintf(log,"\t% 8.6f % 8.6f % 8.6f  % 7.2f\n",0.0,0.0,0.0,1.0);
        }
    }
}

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

void FiffCoordTransOld::mne_print_coord_transform(FILE *log, FiffCoordTransOld *t)
{
    mne_print_coord_transform_label(log,NULL,t);
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform(const QString &name, int from, int to)
/*
          * Read the specified coordinate transformation
          */
{
    QFile file(name);
    FiffStream::SPtr stream(new FiffStream(&file));

    FiffCoordTransOld* res = NULL;
    //    fiffFile       in = NULL;
    FiffTag::SPtr t_pTag;
    //    fiffTagRec     tag;
    //    fiffDirEntry   dir;
    fiff_int_t kind, pos;
    int k;

    //    tag.data = NULL;
    //    if ((in = fiff_open(name.toUtf8().data())) == NULL)
    //        goto out;
    if(!stream->open())
        goto out;

    for (k = 0; k < stream->dir().size(); k++) {
        kind = stream->dir()[k]->kind;
        pos  = stream->dir()[k]->pos;
        if (kind == FIFF_COORD_TRANS) {
            //            if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
            //                goto out;
            //            res = (fiffCoordTrans)tag.data;
            if (!stream->read_tag(t_pTag,pos))
                goto out;

            res = FiffCoordTransOld::read_helper( t_pTag );
            if (res->from == from && res->to == to) {
                //                tag.data = NULL;
                goto out;
            }
            else if (res->from == to && res->to == from) {
                FiffCoordTransOld* tmp_res = res;
                res = tmp_res->fiff_invert_transform();//Memory leak here!!
                delete tmp_res;
                goto out;
            }
            res = NULL;
        }
    }
    qCritical("No suitable coordinate transformation found in %s.",name.toUtf8().data());
    goto out;

out : {
        //        FREE(tag.data);
        //        fiff_close(in);
        stream->close();
        return res;
    }

    return res;
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_from_node(FiffStream::SPtr &stream, const FiffDirNode::SPtr &node, int from, int to)
/*
* Read the specified coordinate transformation
*/
{
    FiffCoordTransOld* res = NULL;
    FiffTag::SPtr t_pTag;
    //    fiffTagRec     tag;
    //    fiffDirEntry   dir;
    fiff_int_t kind, pos;
    int k;

    //    tag.data = NULL;
    for (k = 0; k < node->nent(); k++)
        kind = node->dir[k]->kind;
    pos  = node->dir[k]->pos;
    if (kind == FIFF_COORD_TRANS) {
        //            if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
        //                goto out;
        //            res = (fiffCoordTrans)tag.data;
        if (!stream->read_tag(t_pTag,pos))
            goto out;
        res = FiffCoordTransOld::read_helper( t_pTag );
        if (res->from == from && res->to == to) {
            //                tag.data = NULL;
            goto out;
        }
        else if (res->from == to && res->to == from) {
            FiffCoordTransOld* tmp_res = res;
            res = tmp_res->fiff_invert_transform();//Memory leak here!!
            delete tmp_res;
            goto out;
        }
        res = NULL;
    }
    printf("No suitable coordinate transformation found");
    goto out;

out : {
        //        FREE(tag.data);
        return res;
    }
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_mri_transform(const QString &name)
/*
          * Read the MRI -> HEAD coordinate transformation
          */
{
    return mne_read_transform(name,FIFFV_COORD_MRI,FIFFV_COORD_HEAD);
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_meas_transform(const QString &name)
/*
* Read the MEG device -> HEAD coordinate transformation
*/
{
    return mne_read_transform(name,FIFFV_COORD_DEVICE,FIFFV_COORD_HEAD);
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_ascii(char *name, int from, int to)
/*
* Read the Neuromag -> FreeSurfer transformation matrix
*/
{
    FILE *in = NULL;
    FiffCoordTransOld* t = NULL;
    float rot[3][3];
    float move[3];
    int   k;
    float dum;

    if ((in = fopen(name,"r")) == NULL) {
        qCritical(name);
        goto bad;
    }
    for (k = 0; k < 3; k++) {
        if (get_fval_20(in,rot[k]+X_20) == FAIL)
            goto noread;
        if (get_fval_20(in,rot[k]+Y_20) == FAIL)
            goto noread;
        if (get_fval_20(in,rot[k]+Z_20) == FAIL)
            goto noread;
        if (get_fval_20(in,move+k) == FAIL)
            goto noread;
    }
    for (k = 0; k < 4; k++) {
        if (get_fval_20(in,&dum) == FAIL)
            goto noread;
    }
    fclose(in);
    for (k = 0; k < 3; k++)
        move[k] = move[k]/1000.0;
    t  = new FiffCoordTransOld(from, to, rot, move );
    return t;

noread : {
        qCritical("Cannot read the coordinate transformation");
        goto bad;
    }

bad : {
        if(t)
            delete t;
        if (in != NULL)
            fclose(in);
        return NULL;
    }
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_read_FShead2mri_transform(char *name)
{
    return mne_read_transform_ascii(name,FIFFV_COORD_HEAD,FIFFV_COORD_MRI);
}

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

FiffCoordTransOld *FiffCoordTransOld::mne_identity_transform(int from, int to)
{
    float rot[3][3] = { { 1.0, 0.0, 0.0 },
                        { 0.0, 1.0, 0.0 },
                        { 0.0, 0.0, 1.0 } };
    float move[] = { 0.0, 0.0, 0.0 };
    return new FiffCoordTransOld(from,to,rot,move);
}

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

FiffCoordTransOld *FiffCoordTransOld::read_helper( FIFFLIB::FiffTag::SPtr& tag)
{
    FiffCoordTransOld* p_FiffCoordTrans = NULL;
    if(tag->isMatrix() || tag->getType() != FIFFT_COORD_TRANS_STRUCT || tag->data() == NULL)
        return p_FiffCoordTrans;
    else
    {
        p_FiffCoordTrans = new FiffCoordTransOld;
        qint32* t_pInt32 = (qint32*)tag->data();
        p_FiffCoordTrans->from = t_pInt32[0];
        p_FiffCoordTrans->to = t_pInt32[1];

        float* t_pFloat = (float*)tag->data();
        int count = 0;
        int r, c;
        for (r = 0; r < 3; ++r) {
            p_FiffCoordTrans->move[r] = t_pFloat[11+r];
            for (c = 0; c < 3; ++c) {
                p_FiffCoordTrans->rot(r,c) = t_pFloat[2+count];
                ++count;
            }
        }

        count = 0;
        for (r = 0; r < 3; ++r) {
            p_FiffCoordTrans->invmove[r] = t_pFloat[23+r];
            for (c = 0; c < 3; ++c) {
                p_FiffCoordTrans->invrot(r,c) = t_pFloat[14+count];
                ++count;
            }
        }

        return p_FiffCoordTrans;
    }
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2018 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 <deque>
#include <limits>
#include <boost/circular_buffer.hpp>
#include <comma/io/stream.h>
#include <comma/csv/stream.h>
#include <comma/csv/traits.h>
#include "../../visiting/traits.h"

namespace {

void bash_completion( unsigned const ac, char const * const * av )
{
    static char const* arguments[] =
    {
        " turn decelerate",
        " --binary -b --delimiter -d --fields -f --flush --format --full-xpath --help -h --quote --precision --verbose -v",
        " --input-fields --output-fields --output-format",
        " --deceleration -c",
        " --max-acceleration",
        " --min-points-separation --min-gap -g",
        " --approach-speed -s",
        " --sharp-turn-angle -t",
        " --stop-on-sharp-turn --pivot -p"
    };
    std::cout << arguments << std::endl;
    exit( 0 );
}

void operations( unsigned const indent_count = 0 )
{
    auto const indent = std::string( indent_count, ' ' );
    std::cerr << indent << "decelerate; moderate sudden decreases in speed with given deceleration." << std::endl;
    std::cerr << indent << "turn; set the speed on curves in the plan based on given centripetal acceleration." << std::endl;
}

void usage( bool const verbose )
{
    static const char* const indent="    ";

    std::cerr << std::endl;
    std::cerr << "read trajectory points, append speed moderated according to turn angle, max lateral acceleration, etc" << std::endl;
    std::cerr << std::endl;
    std::cerr << "Usage:" << std::endl;
    std::cerr << indent << comma::verbose.app_name() << " <operation> [<options>...]" << std::endl;
    std::cerr << std::endl;
    std::cerr << "Operations:" << std::endl;
    operations(4);
    std::cerr << std::endl;
    std::cerr << "Options:" << std::endl;
    std::cerr << "    decelerate:" << std::endl;
    std::cerr << "        --deceleration, -c=<deceleration>; deceleration by which to moderate sudden decreases in speed." << std::endl;
    std::cerr << std::endl;
    std::cerr << "    turn:" << std::endl;
    std::cerr << "        --approach-speed, -a=<speed>; approach with this speed for waypoints with sharp turns." << std::endl;
    std::cerr << "        --max-acceleration, -c=<acceleration>; maximum centripetal acceleration allowed." << std::endl;
    std::cerr << "        --min-points-separation, --min-gap, -g=<distance>; default=0.01; minimum distance between points for their consideration in the operation." << std::endl;
    std::cerr << "        --sharp-turn-angle, -t=<angle>; default=1.57; assign 'approach-speed' for waypoints having turn angle greater than this." << std::endl;
    std::cerr << "        --speed, -s=[<speed>]; use this speed if it is not in input fields." << std::endl;
    std::cerr << "        --stop-on-sharp-turn, --pivot, -p; on sharp turns (i.e. waypoints set with 'approach-speed'), output extra point with relative heading and no speed." << std::endl;
    std::cerr << std::endl;
    std::cerr << "Info:" << std::endl;
    std::cerr << "    --input-fields; print input fields to stdout and exit." << std::endl;
    std::cerr << "    --output-fields; print output fields to stdout and exit." << std::endl;
    std::cerr << "    --output-format; print output fields to stdout and exit." << std::endl;
    std::cerr << std::endl;
    if( verbose ) { std::cerr << "Csv options:" << std::endl << comma::csv::options::usage( "x,y,z,speed" ) << std::endl; }
    else { std::cerr << "Csv options: run --help --verbose for more info..." << std::endl; }
    std::cerr << std::endl;
    std::cerr << "Examples:" << std::endl;
    std::cerr << "    ( echo '0.0,0.0'; echo '0.3,0.3'; echo '0.6,0.6'; echo '0.6,0.9'; echo '0.6,1.2'; echo '0.9,1.2'; echo '1.2,1.2'; echo '1.5,0.9'; echo '1.8,0.6' ) > trajectory.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    control-speed turn --fields=x,y --max-acceleration=0.5 --approach-speed=0.2 --speed=1 --pivot < trajectory.csv > moderated.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    control-speed decelerate --fields=x,y,speed --deceleration=0.5 < moderated.csv > decelerated.csv" << std::endl;
    std::cerr << std::endl;
}

namespace generic
{
struct input_t
{
    Eigen::Vector3d coordinates;
    double speed;

    input_t( double sp = 0.0 ) : coordinates( Eigen::Vector3d::Zero() ), speed( sp ) {}
};
}

namespace turn
{
struct output_t
{
    double moderated_speed;
    double relative_heading;

    output_t( double sp = 0.0 ) : moderated_speed( sp ), relative_heading( 0.0 ) {}
};

struct record_t
{
    generic::input_t input;
    output_t output;
    std::string line;
    //...debug...
    //double angle;
    //double radius;

    record_t( generic::input_t const& in, comma::csv::input_stream< generic::input_t > const& istrm, comma::csv::options const& csv )
        : input( in )
        , output( in.speed )
        , line()
        //...debug...
        //, angle( 0 )
        //, radius( 0 )
    {
        if( csv.binary() )
        {
            line.resize( csv.format().size() );
            ::memcpy( &line[ 0 ], istrm.binary().last(), csv.format().size() );
        }
        else
        {
            line = comma::join( istrm.ascii().last(), csv.delimiter );
        }
    }

    void write( comma::csv::output_stream< output_t >& ostrm, comma::csv::options const& csv )
    {
        ostrm.append( line, output );
        if( csv.binary() && csv.flush ) { std::cout.flush(); }
        //...debug...
        //std::cerr << line << csv.delimiter << output.moderated_speed << csv.delimiter << angle << csv.delimiter << radius << std::endl;
    }
};
}

namespace deceleration
{
struct record_t
{
    generic::input_t input;
    std::string line;
    //...debug...
    //double angle;
    //double radius;

    record_t( generic::input_t const& in, comma::csv::input_stream< generic::input_t > const& istrm, comma::csv::options const& csv )
        : input( in )
        , line()
        //...debug...
        //, angle( 0 )
        //, radius( 0 )
    {
        if( csv.binary() )
        {
            line.resize( csv.format().size() );
            ::memcpy( &line[ 0 ], istrm.binary().last(), csv.format().size() );
        }
        else { line = comma::join( istrm.ascii().last(), csv.delimiter ); }
    }

    void write( comma::csv::output_stream< generic::input_t >& ostrm, comma::csv::options const& csv )
    {
        ostrm.write( input, line );
        if( csv.binary() && csv.flush ) { std::cout.flush(); }
        //...debug...
        //std::cerr << line << csv.delimiter << input.speed << csv.delimiter << angle << csv.delimiter << radius << std::endl;
    }
};
}

void handle_info_options( comma::command_line_options const& options )
{
    if( options.exists( "--input-fields" ) ) { std::cout << comma::join( comma::csv::names< generic::input_t >( false ), ',' ) << std::endl; exit( 0 ); }
}

double radius_calc( Eigen::Vector3d const& p1, Eigen::Vector3d const& p2, Eigen::Vector3d const& p3 )
{
    auto const side1 = ( p2 - p1 ).norm();
    auto const side2 = ( p3 - p2 ).norm();
    auto const side3 = ( p3 - p1 ).norm();
    auto const perimeter_half = ( side1 + side2 + side3 ) / 2;
    auto area = perimeter_half * ( perimeter_half - side1 ) * ( perimeter_half - side2 ) * ( perimeter_half - side3 ); // Heron's Equation

    if( comma::math::less( 0.0, area ) )
    {
        area = std::sqrt( area );
        return ( side1 * side2 * side3 ) / ( 4.0 * area );
    }
    else
    {
        return comma::math::less( side1, side3 ) && comma::math::less( side2, side3 ) ? std::numeric_limits< double >::max() : 0.0;
    }
}

double angle_calc( Eigen::Vector3d const& p1, Eigen::Vector3d const& p2, Eigen::Vector3d const& p3 )
{
    auto angle_axis = Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( p2 - p1, p3 - p2 ) );
    //...debug...
    //std::cerr << std::setprecision(16)
    //          << p1.x() << ',' << p1.y() << ',' << p1.z() << ','
    //          << p2.x() << ',' << p2.y() << ',' << p2.z() << ','
    //          << p3.x() << ',' << p3.y() << ',' << p3.z() << ','
    //          << angle_axis.angle() << ',' << angle_axis.axis().x() << ',' << angle_axis.axis().y() << ',' << angle_axis.axis().z() << std::endl;
    return angle_axis.angle() * ( comma::math::less( angle_axis.axis().z(), 0.0 ) ? -1 : 1 );
}

}

namespace comma { namespace visiting {

template <> struct traits< generic::input_t >
{
    template < typename K, typename V > static void visit ( const K&, generic::input_t& t, V& v )
    {
        v.apply( "coordinates", t.coordinates );
        v.apply( "speed", t.speed );
    }

    template < typename K, typename V > static void visit ( const K&, const generic::input_t& t, V& v )
    {
        v.apply( "coordinates", t.coordinates );
        v.apply( "speed", t.speed );
    }
};

template <> struct traits< turn::output_t >
{
    template < typename K, typename V > static void visit ( const K&, turn::output_t& t, V& v )
    {
        v.apply( "moderated_speed", t.moderated_speed );
        v.apply( "relative_heading", t.relative_heading );
    }

    template < typename K, typename V > static void visit ( const K&, const turn::output_t& t, V& v )
    {
        v.apply( "moderated_speed", t.moderated_speed );
        v.apply( "relative_heading", t.relative_heading );
    }
};

}}

int main( int ac, char* av[] )
{
    try
    {
        comma::command_line_options options( ac, av, usage );
        if( options.exists( "--bash-completion" ) ) bash_completion( ac, av );
        handle_info_options( options );

        std::vector< std::string > operations = options.unnamed( "","--binary,-b,--delimiter,-d,--fields,-f,--flush,--format,--full-xpath,--help,-h,--quote,--precision,--verbose,-v,--input-fields,--output-fields,--output-format,--deceleration,--max-acceleration,-c,--approach-speed,-a,--sharp-turn-angle,-t,--speed,-s,--stop-on-sharp-turn,--pivot,-p,--min-points-separation,--min-gap,-g" );
        if( operations.size() != 1 ) { std::cerr << comma::verbose.app_name() << ": expected one operation, got " << operations.size() << ": " << comma::join( operations, ' ' ) << std::endl; return 1; }

        comma::csv::options csv( options );
        if( csv.fields.empty() ) { csv.fields = "x,y,z,speed"; }
        csv.full_xpath = false;

        if( "turn" == operations[ 0 ] )
        {
            if( !( csv.has_field( "speed" ) || options.exists( "--speed,-s" ) ) )
            { std::cerr << comma::verbose.app_name() << ": speed must be present in input fields or command line options." << std::endl; return 1; }

            comma::csv::options out_csv( csv );
            auto const pivot = options.exists( "--stop-on-sharp-turn,--pivot,-p" );
            out_csv.fields = "moderated_speed"; if( pivot ) { out_csv.fields.append( ",relative_heading" ); }
            if( options.exists( "--output-fields" ) ) { std::cout << out_csv.fields << std::endl; return 0; }
            if( options.exists( "--output-format" ) ) { std::cout << comma::csv::format::value< turn::output_t >( out_csv.fields, true ) << std::endl; return 0; }

            auto const speed = options.value< double >( "--speed,-s", 0.0 );
            auto const max_acceleration = options.value< double >( "--max-acceleration,-c" );
            auto const approach_speed = options.value< double >( "--approach-speed,-a" );
            auto const sharp_turn_angle = options.value< double >( "--sharp-turn-angle,-t", 1.57 ); // near 90 degrees
            auto const min_points_gap = options.value< double >( "--min-points-separation,--min-gap,-g", 0.01 );
            auto const spot_turn_radius = approach_speed * approach_speed / max_acceleration;

            auto unique_points = 0U;
            std::deque< turn::record_t > que;

            comma::csv::input_stream< generic::input_t > istrm( std::cin, csv, generic::input_t( speed ) );
            comma::csv::output_stream< turn::output_t > ostrm( std::cout, out_csv );

            while( istrm.ready() || ( std::cin.good() && !std::cin.eof() ) )
            {
                auto input = istrm.read(); if( !input ) break;
                if( que.empty() || ( input->coordinates - que.back().input.coordinates ).norm() >= min_points_gap ) { unique_points++; }
                que.emplace_back( *input, istrm, csv );

                if( 3U == unique_points )
                {
                    auto relevant_index = que.size() - 2U;
                    auto const angle = angle_calc( que.front().input.coordinates, que[ relevant_index ].input.coordinates, que.back().input.coordinates );
                    auto const radius = radius_calc( que.front().input.coordinates, que[ relevant_index ].input.coordinates, que.back().input.coordinates );

                    if( comma::math::less( std::fabs( angle ), sharp_turn_angle ) && comma::math::less( spot_turn_radius, radius ) )
                    {
                        auto updated_speed = std::sqrt( max_acceleration * radius );
                        que[ relevant_index ].output.moderated_speed = std::max( approach_speed, std::min( que[ relevant_index ].input.speed, updated_speed ) );
                    }
                    else
                    {
                        que[ relevant_index ].output.moderated_speed = std::min( que[ relevant_index ].input.speed, approach_speed );
                        if( pivot )
                        {
                            auto temp = que.back();
                            que.back() = que[ relevant_index ];
                            que.back().output.moderated_speed = 0;
                            que.back().output.relative_heading = angle;
                            que.push_back( temp );
                        }
                    }
                    while( 2 < que.size() ) { que.front().write( ostrm, csv ); que.pop_front(); }
                    unique_points = 2;
                }
            }
            for( auto ii = 0U; ii < que.size(); ii++ ) { que[ ii ].write( ostrm, csv ); }
            return 0;
        }
        else if( "decelerate" == operations[ 0 ] )
        {
            comma::csv::input_stream< generic::input_t > istrm( std::cin, csv );

            comma::csv::output_stream< generic::input_t > ostrm( std::cout, csv );
            std::deque< deceleration::record_t > que;

            auto const deceleration = options.value< double >( "--deceleration,-c" );
            auto max_speed = 0.0;
            while( istrm.ready() || ( std::cin.good() && !std::cin.eof() ) )
            {
                auto input = istrm.read(); if( !input ) break;
                que.emplace_back( *input, istrm, csv );

                max_speed = std::max( que.back().input.speed, max_speed );
                auto const affected_distance = max_speed * max_speed / ( 2 * deceleration ); // kinematic equation with min_speed = 0.0

                auto curri = que.rbegin();
                auto previ = curri + 1;
                double updated_speed, distance;

                for( ; previ != que.rend()
                         && ( comma::math::less( 0.0, distance = ( curri->input.coordinates - previ->input.coordinates ).norm() )
                           || comma::math::less( 0.0, curri->input.speed ) )
                         && comma::math::less( updated_speed = std::sqrt( curri->input.speed * curri->input.speed + 2 * deceleration * distance )
                                             , previ->input.speed )
                     ; previ++, curri++ )
                {
                    previ->input.speed = updated_speed;
                }
                while(( que.back().input.coordinates - que.front().input.coordinates ).norm() > affected_distance )
                {
                    que.front().write( ostrm, csv );
                    que.pop_front();
                }
            }
            for( auto ii = 0U; ii < que.size(); ii++ ) { que[ ii ].write( ostrm, csv ); }
            return 0;
        }
        std::cerr << comma::verbose.app_name() << ": unknown operation: " <<  operations[ 0 ] << std::endl;
        return 1;
    }
    catch( std::exception& ex ) { std::cerr << comma::verbose.app_name() << ": " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << comma::verbose.app_name() << ": unknown exception" << std::endl; }
    return 1;
}