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

/*!
 * wind_technology.cpp
 * Created: 03/20/2007
 * Version: 04/16/2007
 *
 * This software, which is provided in confidence, was prepared by employees
 * of Pacific Northwest National Laboratory operated by Battelle Memorial
 * Institute. Battelle has certain unperfected rights in the software
 * which should not be copied or otherwise disseminated outside your
 * organization without the express written authorization from Battelle.
 * All rights to the software are reserved by Battelle.   Battelle makes no
 * warranty, express or implied, and assumes no liability or responsibility
 * for the use of this software.
 */

// include files ***********************************************************

#include "util/base/include/definitions.h"
#include "technologies/include/wind_technology.h"
#include "technologies/include/marginal_profit_calculator.h"
#include "technologies/include/iproduction_state.h"
#include "marketplace/include/marketplace.h"
#include "containers/include/iinfo.h"
#include "util/base/include/TValidatorInfo.h"
#include "util/base/include/util.h"
#include "util/base/include/xml_helper.h"
#include "technologies/include/ioutput.h"
#include "functions/include/non_energy_input.h"

#include <sstream>
#include <algorithm>
#include <cmath>

#include <boost/math/special_functions/erf.hpp>
using boost::math::erf;

// namespaces **************************************************************

// WindTechnology::kWhrtoGJ ************************************************

const double WindTechnology::kWhrtoGJ = 0.0036;

// WindTechnology::sXMLTagNames ********************************************

std::string WindTechnology::sXMLTagNames[] =
{
   std::string( "air-density" ),
   std::string( "average-wind-speed" ),
   std::string( "capital-cost" ),
   std::string( "cutout-speed" ),
   std::string( "fcr" ),
   std::string( "grid-connection-cost" ),
   std::string( "om" ),
   std::string( "reference-height" ),
   std::string( "rotor-diameter" ),
   std::string( "turbine-density" ),
   std::string( "turbine-derating" ),
   std::string( "turbine-hub-height" ),
   std::string( "turbine-rating" ),
   std::string( "wind-farm-loss" ),
   std::string( "wind-velocity-exponent" )
};

// Constructor: WindTechnology *********************************************

/*! Default constructor
 *  \param aName the name of the technology
 *  \param aYear the year
 */
WindTechnology::WindTechnology(
   const std::string& aName,
   const int          aYear )
   : IntermittentTechnology( aName, aYear )
{
    mCapitalCost = 261.83;
    mConnectCost = -1;
    mCutOutSpeed = -1;
    mFCR = 0.0856;
    mGenerationCost = -1;
    mGridConnectionCost = 392.75;
    mOM = 2.62;
    mRealizedTurbineOutput = -1;
    mRotorDiameter = -1;
    mTurbineDensity = -1;
    mTurbineDerating = -1;
    mTurbineHubHeight = -1;
    mTurbineRating = -1;
    mWindCapacityFactor = -1;
    mWindFarmLoss = -1;
}

// Destructor: WindTechnology **********************************************

WindTechnology::~WindTechnology(void)
{
}

void WindTechnology::copy( const WindTechnology& aOther ) {
    IntermittentTechnology::copy( aOther );
    mCapitalCost = aOther.mCapitalCost;
    mConnectCost = aOther.mConnectCost;
    mCutOutSpeed = aOther.mCutOutSpeed;
    mFCR = aOther.mFCR;
    mGenerationCost = aOther.mGenerationCost;
    mGridConnectionCost = aOther.mGridConnectionCost;
    mOM = aOther.mOM;
    mRealizedTurbineOutput = aOther.mRealizedTurbineOutput;
    mRotorDiameter = aOther.mRotorDiameter;
    mTurbineDensity = aOther.mTurbineDensity;
    mTurbineDerating = aOther.mTurbineDerating;
    mTurbineHubHeight = aOther.mTurbineHubHeight;
    mTurbineRating = aOther.mTurbineRating;
    mWindCapacityFactor = aOther.mWindCapacityFactor;
    mWindFarmLoss = aOther.mWindFarmLoss;
}

// WindTechnology::calcCost ************************************************

// Documentation is inherited
void WindTechnology::calcCost(
   const std::string& aRegionName,
   const std::string& aSectorName,
   const int          aPeriod )
{
    if( !mProductionState[ aPeriod ]->isOperating() ){
        return;
    }

   // Var to hold capital and operating costs
   double totalTechCapOMCost = 0;

   // Get marketplace and calculate costs
   Marketplace*       pMarketplace = scenario->getMarketplace();
   const IInfo*       pInfo        = pMarketplace->getMarketInfo( ( *mResourceInput )->getName(), aRegionName, aPeriod, true );

   // Equation 4:
   mRealizedTurbineOutput = calcRealizedTurbineOutput( pInfo );

   // Equation 3:
   // WindCapacityFactor = RealizedTurbineOutput / TurbineRating
   mWindCapacityFactor = mRealizedTurbineOutput / mTurbineRating;

   // Equation 6:
   // CConnect = FCR * DConnect * ( gridConnectionCost / ( WindCapacityFactor * 1000.0 * kWhrtoGJ * 24 * 365 * 1 ) )
   double dConnect = pMarketplace->getPrice( ( *mResourceInput )->getName(), aRegionName, aPeriod );
   mConnectCost    = mFCR * dConnect * ( mGridConnectionCost / ( mWindCapacityFactor * 1000.0 * kWhrtoGJ * 24.0 * 365.0 ) );

   // Equation 2:
   // CGeneration = ( FCR * CapitalCost + OM ) / ( 24 * 365 * WindCapacityFactor * kWhrtoGJ )
   mGenerationCost = ( mFCR * mCapitalCost + mOM ) / ( 24.0 * 365.0 * mWindCapacityFactor * kWhrtoGJ );

   if( !mProductionState[ aPeriod ]->isNewInvestment() ||
       mFixedOutput != IProductionState::fixedOutputDefault() )
   {
      // If an existing stock, only have O&M costs.
      totalTechCapOMCost = mOM /  ( 24.0 * 365.0 * mWindCapacityFactor * kWhrtoGJ );
   }
   else {
      // Generation plus connection costs.
      totalTechCapOMCost = std::max( mGenerationCost + mConnectCost, util::getSmallNumber() );
   }

   // Set tech capital and operating costs to input object
   ( *mTechCostInput )->setPrice( aRegionName, totalTechCapOMCost, aPeriod );

   // Call parent function to set costs and coefficients
   IntermittentTechnology::calcCost( aRegionName, aSectorName, aPeriod );

}

// WindTechnology::calcIdealTurbineOutput **********************************

/*!
 *  \param aAveWindSpeed the average Wind Speed
 *  \param aDiameter the turbine blade diameter (in meters)
 *  \param aAirDensity the average Air density (in g/m^3)
 */
double WindTechnology::calcIdealTurbineOutput(
   double aAveWindSpeed,
    double aDiameter,
    double aAirDensity )
{
   /*
    * The ideal maximum extractable power, known as the Betz limit, has a
    * coefficient of Cp = 16/27. At this limit, the integral evaluates to
    * (Equation A-6):
    *   Prb = p * ( 2 / 3 * D )^2 * V^3
    * where:
    *   p = the average Air density (in g/m^3)
    *   D = the turbine blade diameter (in meters)
    *   V = the average Wind Speed
    */
   return aAirDensity * std::pow( 2.0 / 3.0 * aDiameter, 2.0 ) * std::pow( aAveWindSpeed, 3.0 );
}

// WindTechnology::calcRealizedTurbineOutput *******************************
/*! Compute the realized turbine output
 *  \param apInfo pointer to the market info
 */
double WindTechnology::calcRealizedTurbineOutput( const IInfo* apInfo ) const
{
   // Equation 5:
   // aveWindSpeedAtHub = aveWindSpeed * ( turbineHubHeight / referenceHeight ) ^ windVelocityExponent
   double aveWindSpeed = apInfo->getDouble( sXMLTagNames[ AVERAGE_WIND_SPEED_KEY ], true );
   double referenceHeight = apInfo->getDouble( sXMLTagNames[ REFERENCE_HEIGHT_KEY ], true );
   double windVelocityExponent = apInfo->getDouble( sXMLTagNames[ WIND_VELOCITY_EXPONENT_KEY ], true );
   double aveWindSpeedAtHub = aveWindSpeed * std::pow( mTurbineHubHeight / referenceHeight, windVelocityExponent );

   // Equation 4:
   // RealizedTurbineOutput = ( IdealTurbineOutput / 10^6 ) * TurbineCoefficient * ( 1 - Derating ) * ( 1 - WindFarmLoss )
   double airDensity = apInfo->getDouble( sXMLTagNames[ AIR_DENSITY_KEY ], true );
   double realizedTurbineOutput = ( calcIdealTurbineOutput( aveWindSpeedAtHub, mRotorDiameter, airDensity ) / 1.0e6 ) * calcTurbineCoefficient( aveWindSpeedAtHub, mTurbineRating, mRotorDiameter, airDensity, mCutOutSpeed ) * ( 1.0 - mTurbineDerating ) * ( 1.0 - mWindFarmLoss );
   mWindPowerVariance = computeWindPowerVariance( aveWindSpeedAtHub, mTurbineRating, mRotorDiameter, airDensity, mCutOutSpeed );
   return realizedTurbineOutput;
}

// WindTechnology::calcResourceArea ****************************************

/*! Calculate the resource area in km^2
 *  NOTE: Unlike solar techs, wind does not exclusively use this land. This need to be
 *  accounted for when setting land costs.
 *  \param aRegionName the region name
 *  \param aSectorName the sector name
 *  \param aVariableDemand the variable demand
 *  \param aPeriod the period
 *  \return the resource area
 */
double WindTechnology::calcResourceArea(
   const std::string& aRegionName,
   const std::string& aSectorName,
   double             aVariableDemand,
   const int          aPeriod )
{
   // Get info object for this resource
   Marketplace*       pMarketplace = scenario->getMarketplace();
   const IInfo*       pInfo        = pMarketplace->getMarketInfo( ( *mResourceInput )->getName(), aRegionName, aPeriod, true );

   // Equation 4:
   mRealizedTurbineOutput = calcRealizedTurbineOutput( pInfo );

   // Equation 3:
   // WindCapacityFactor = RealizedTurbineOutput / TurbineRating
   mWindCapacityFactor = mRealizedTurbineOutput / mTurbineRating;

   // Equation 7:
   // WindGeneration / ( kWhrtoGJ * 10^-9 ) = TurbineDensity * ResourceArea * WindCapacityFactor * 1000 * 24 * 365
   // ResourceArea = ( WindGeneration / ( kWhrtoGJ * 10^-9 ) ) / ( TurbineDensity * WindCapacityFactor * 1000 * 24 * 365 )
   double windGeneration = aVariableDemand;
   double resourceArea = ( windGeneration / ( kWhrtoGJ * 1.0e-9 ) ) / ( mTurbineDensity * mWindCapacityFactor * 1000.0 * 24.0 * 365.0 );

   return resourceArea;
}

/*! \brief Return amount of resource needed per unit of energy output.
*  This method should be used when a technology uses a resource that is
*  not in energy units.
*  NOTE: Unlike solar techs, wind does not exclusively use this land. This need to be
*  accounted for when setting land costs.
* \author Steve Smith
* \param aPeriod Model period.
*/
double WindTechnology::getResourceToEnergyRatio( const std::string& aRegionName,
                                                 const std::string& aSectorName,
                                                 const int aPeriod )
{
    // Default assumpion is that resource is in energy units
   // Need to return the amount of resource (in km^2) needed per EJ of energy output
   return calcResourceArea( aRegionName, aSectorName, 1.0, aPeriod );
}

// WindTechnology::calcTurbineCoefficient **********************************

/*! Compute the capture coefficient for a turbine with a finite power rating.
 *  \param aAveWindSpeed the average Wind Speed
 *  \param aRating the turbine Rating (in MW)
 *  \param aDiameter the turbine Blade Diameter (in meters)
 *  \param aAirDensity the average Air density (in g/m^3)
 *  \param aCutoutSpeed the cut-out Speed (m/s)
 */
double WindTechnology::calcTurbineCoefficient(
   double aAveWindSpeed,
    double aRating,
    double aDiameter,
    double aAirDensity,
    double aCutoutSpeed )
{
   static const double PI     = 2.0 * std::asin( 1.0 );
   static const double sqrtPI = std::sqrt( PI );

   /*
    * Equation A-8
    * Calculate the final cutout speed (Xf)
    *   Xs = ( sqrt( pi ) / 2) * ( Vs / Vave )
    */
   double Xf = ( sqrtPI / 2.0 ) * ( aCutoutSpeed / aAveWindSpeed );

   /*
    * Equation A-10
    * Calculate the velocity (solve for v)
    * 1/8pD^2(16/27)v^3 = Prated
    * v = cube-root( Prated / ( 1/8pD^2(16/27) )
    */
   double aRatingInWatts = aRating * 1.0e6;
   double velocity = std::pow( aRatingInWatts / ( 1.0 / 8.0 * aAirDensity * PI * std::pow( aDiameter, 2.0 ) * 16.0 / 27.0 ), 1.0 / 3.0 );

   /*
    * Equation A-11
    * Calculate the finite power rating (Xr)
    * Xr = ( sqrt( pi ) / 2 ) * V / Vave
    */
   double Xr = ( sqrtPI / 2.0 ) * velocity / aAveWindSpeed;

   /*
    * Equation A-7:
    * Compute the power up to a finite cutout for Xr
    * speed is ():
    *
    *   CCfinite_cutout( Xs ) =
    *      Erf( Xs ) - ( 4 / ( 3 * sqrt( pi ) ) ) * Xs * ( Xs^2 + 3 / 2 ) * e^-Xs^2
    */
   double CCfiniteCutout = erf( Xr ) - ( 4.0 / ( 3.0 * sqrtPI ) ) * Xr * ( std::pow( Xr, 2.0 ) + 3.0 / 2.0 ) * std::exp( -std::pow( Xr, 2.0 ) );

   /*
    * Equation A-9:
    * Finally, compute the capture coefficient
    * CCfinite_power( Xr, Xf ) =
    *    CCfinite_cutout( Xr ) + ( Xr^3 / ( 3 / 4 * sqrt( pi ) ) ) * ( e ^-Xr^2 - e^-Xf^2 )
    */
   double CCfinitePower = CCfiniteCutout + ( std::pow( Xr, 3.0 ) / ( 3.0 / 4.0 * sqrtPI ) ) * ( std::exp( -std::pow( Xr, 2.0 ) ) - std::exp( -std::pow( Xf, 2.0 ) ) );

   return CCfinitePower;
}

// WindTechnology::clone ***************************************************

// Documentation is inherited
WindTechnology* WindTechnology::clone( void ) const
{
    WindTechnology* clone = new WindTechnology( mName, mYear );
    clone->copy( *this );
    return clone;
}

// WindTechnology::completeInit ********************************************

// Documentation is inherited
void WindTechnology::completeInit(
   const std::string&              aRegionName,
   const std::string&              aSectorName,
   const std::string&              aSubsectorName,
   const IInfo*                    aSubsectorIInfo,
   ILandAllocator*                 aLandAllocator )
{
   // Initialize a non-energy input to hold technology costs
   if( util::searchForValue( mInputs, getTechCostName() ) == mInputs.end() ){
        mInputs.push_back( new NonEnergyInput( getTechCostName() ) );
    }

   IntermittentTechnology::completeInit( aRegionName, aSectorName, aSubsectorName, aSubsectorIInfo, aLandAllocator );

   // Validate input parameters
   typedef ObjECTS::TValidatorInfo<> validator_type;
   validator_type   validator[] =
   {
      validator_type( mCapitalCost, sXMLTagNames[ CAPITAL_COST_KEY ], mCapitalCost > 0 ),
      validator_type( mCutOutSpeed, sXMLTagNames[ CUTOUT_SPEED_KEY ], mCutOutSpeed > 0 ),
      validator_type( mFCR, sXMLTagNames[ FCR_KEY ], mFCR > 0 ),
      validator_type( mRotorDiameter, sXMLTagNames[ ROTOR_DIAMETER_KEY ], mRotorDiameter > 0 ),
      validator_type( mTurbineDensity, sXMLTagNames[ TURBINE_DENSITY_KEY ], mTurbineDensity > 0 ),
      validator_type( mTurbineDerating, sXMLTagNames[ TURBINE_DERATING_KEY ], mTurbineDerating > 0 ),
      validator_type( mTurbineHubHeight, sXMLTagNames[ TURBINE_HUB_HEIGHT_KEY ], mTurbineHubHeight > 0 ),
      validator_type( mTurbineRating, sXMLTagNames[ TURBINE_RATING_KEY ], mTurbineRating > 0 ),
      validator_type( mWindFarmLoss, sXMLTagNames[ WIND_FARM_LOSS_KEY ], mWindFarmLoss > 0 )
   };

   unsigned short numParams = sizeof( validator ) / sizeof( validator[0] );
   std::string    msg       = ObjECTS::getInvalidNames(
      &validator[0],
      &validator[numParams] );

   if ( msg.length() )
   // Invalid input parameter
   {
      ILogger& mainLog = ILogger::getLogger( "main_log" );
      mainLog.setLevel( ILogger::ERROR );
      mainLog << "Invalid input parameter(s) to "
         << getXMLNameStatic()
         << " in sector " << aSectorName
         << ": " << msg << std::endl;
      exit( -1 );
   }
}

// WindTechnology::getXMLName1D ********************************************

// Documentation is inherited
const std::string& WindTechnology::getXMLName1D( void ) const
{
   return getXMLNameStatic();
}

// WindTechnology::getXMLNameStatic1D **************************************

// Documentation is inherited
const std::string& WindTechnology::getXMLNameStatic1D( void )
{
   static const std::string XML_NAME1D = "wind-technology";

   return XML_NAME1D;
}

/*! \brief Return name to be used for input object containing technology costs.
*
* This input object will contain technology capital, operation, and any other costs
* exclusive of backup or fuel costs. Setting to blank indicates that this object does
* not use this cost.
*
* \author Steve Smith
* \return The constant XML_NAME as a static.
*/
const std::string& WindTechnology::getTechCostName( ) const {
   const static std::string TECH_COST_NAME = "turbine-and-connect-costs";
   return TECH_COST_NAME;
}

// WindTechnology::initCalc ************************************************

// Documentation is inherited
void WindTechnology::initCalc(
   const std::string& aRegionName,
   const std::string& aSectorName,
   const IInfo*       aSubsectorIInfo,
   const Demographic* aDemographics,
   PreviousPeriodInfo& aPrevPeriodInfo,
   const int          aPeriod )
{
   IntermittentTechnology::initCalc( aRegionName, aSectorName, aSubsectorIInfo, aDemographics,
       aPrevPeriodInfo, aPeriod );

   // Get marketplace and make sure we have the correct type of resource
   Marketplace*       pMarketplace = scenario->getMarketplace();
   const IInfo*       pInfo        = pMarketplace->getMarketInfo( ( *mResourceInput )->getName(), aRegionName, aPeriod, true );
   std::string        msg          = "";

   if ( !pInfo )
   {
      std::ostringstream ostr;
      ostr << "Error getting marketplace info for ( "
           << ( *mResourceInput )->getName() << ", "
           << aRegionName << ", "
           << aPeriod << " )"
           << std::ends;
      msg = ostr.str();
   }
   else
   {
      typedef ObjECTS::TValidatorInfo<> validator_type;
      double           notUsed=0.0;
      validator_type   validator[] =
      {
         validator_type( notUsed, sXMLTagNames[ AVERAGE_WIND_SPEED_KEY ], pInfo->hasValue( sXMLTagNames[ AVERAGE_WIND_SPEED_KEY ] ) ),
         validator_type( notUsed, sXMLTagNames[ AIR_DENSITY_KEY ], pInfo->hasValue( sXMLTagNames[ AIR_DENSITY_KEY ] ) ),
         validator_type( notUsed, sXMLTagNames[ REFERENCE_HEIGHT_KEY ], pInfo->hasValue( sXMLTagNames[ REFERENCE_HEIGHT_KEY ] ) ),
         validator_type( notUsed, sXMLTagNames[ WIND_VELOCITY_EXPONENT_KEY ], pInfo->hasValue( sXMLTagNames[ WIND_VELOCITY_EXPONENT_KEY ] ) )
      };

      unsigned short numParams = sizeof( validator ) / sizeof( validator[0] );
      msg = ObjECTS::getInvalidNames(
         &validator[0],
         &validator[numParams] );
      if ( msg.length() )
      {
         std::ostringstream ostr;
         ostr << "Invalid input parameter(s) to "
              << getXMLNameStatic()
              << " in sector " << aSectorName
              << ": " << msg << std::ends;
         msg = ostr.str();
      }
   }

   if ( msg.length() )
      // Invalid input parameter
   {
      ILogger& mainLog = ILogger::getLogger( "main_log" );
      mainLog.setLevel( ILogger::ERROR );
      mainLog << msg << std::endl;
   }
}

// WindTechnology::production **********************************************

double WindTechnology::getCalibrationOutput( const bool aHasRequiredInput,
                                         const std::string& aRequiredInput,
                                         const int aPeriod ) const
{
   return IntermittentTechnology::getCalibrationOutput( aHasRequiredInput, aRequiredInput, aPeriod );
}

// WindTechnology::toDebugXMLDerived ***************************************

// Documentation is inherited
void WindTechnology::toDebugXMLDerived(
   const int     period,
   std::ostream& out,
   Tabs*         tabs ) const
{
   IntermittentTechnology::toDebugXMLDerived( period, out,  tabs );
   XMLWriteElement( mCapitalCost, sXMLTagNames[ CAPITAL_COST_KEY ], out, tabs );
   XMLWriteElement( mConnectCost, "connection-cost", out, tabs );
   XMLWriteElement( mCutOutSpeed, sXMLTagNames[ CUTOUT_SPEED_KEY ], out, tabs );
   XMLWriteElement( mFCR, sXMLTagNames[ FCR_KEY ], out, tabs );
   XMLWriteElement( mGenerationCost, "generation-cost", out, tabs );
   XMLWriteElement( mGridConnectionCost, sXMLTagNames[ GRID_CONNECTION_COST_KEY ], out, tabs );
   XMLWriteElement( mOM, sXMLTagNames[ OM_KEY ], out, tabs );
   XMLWriteElement( mRealizedTurbineOutput, "realized-turbine-output", out, tabs );
   XMLWriteElement( mRotorDiameter, sXMLTagNames[ ROTOR_DIAMETER_KEY ], out, tabs );
   XMLWriteElement( mTurbineDensity, sXMLTagNames[ TURBINE_DENSITY_KEY ], out, tabs );
   XMLWriteElement( mTurbineDerating, sXMLTagNames[ TURBINE_DERATING_KEY ], out, tabs );
   XMLWriteElement( mTurbineHubHeight, sXMLTagNames[ TURBINE_HUB_HEIGHT_KEY ], out, tabs );
   XMLWriteElement( mTurbineRating, sXMLTagNames[ TURBINE_RATING_KEY ], out, tabs );
   XMLWriteElement( mWindCapacityFactor, "wind-capacity-factor", out, tabs );
   XMLWriteElement( mWindFarmLoss, sXMLTagNames[ WIND_FARM_LOSS_KEY ], out, tabs );
   XMLWriteElement( mWindPowerVariance, "wind-power-variance", out, tabs );
}

// WindTechnology::toInputXMLDerived ***************************************

// Documentation is inherited
void WindTechnology::toInputXMLDerived(
   std::ostream& out,
   Tabs*         tabs ) const
{
   IntermittentTechnology::toInputXMLDerived( out,  tabs );
   XMLWriteElementCheckDefault( mCapitalCost, sXMLTagNames[ CAPITAL_COST_KEY ], out, tabs, double( 1000.0 ) );
   XMLWriteElement( mCutOutSpeed, sXMLTagNames[ CUTOUT_SPEED_KEY ], out, tabs );
   XMLWriteElementCheckDefault( mFCR, sXMLTagNames[ FCR_KEY ], out, tabs, double( 0.0856 ) );
   XMLWriteElementCheckDefault( mGridConnectionCost, sXMLTagNames[ GRID_CONNECTION_COST_KEY ], out, tabs, double( 1500.0 ) );
   XMLWriteElementCheckDefault( mOM, sXMLTagNames[ OM_KEY ], out, tabs, double( 10.0 ) );
   XMLWriteElement( mRotorDiameter, sXMLTagNames[ ROTOR_DIAMETER_KEY ], out, tabs );
   XMLWriteElement( mTurbineDensity, sXMLTagNames[ TURBINE_DENSITY_KEY ], out, tabs );
   XMLWriteElement( mTurbineDerating, sXMLTagNames[ TURBINE_DERATING_KEY ], out, tabs );
   XMLWriteElement( mTurbineHubHeight, sXMLTagNames[ TURBINE_HUB_HEIGHT_KEY ], out, tabs );
   XMLWriteElement( mTurbineRating, sXMLTagNames[ TURBINE_RATING_KEY ], out, tabs );
   XMLWriteElement( mWindFarmLoss, sXMLTagNames[ WIND_FARM_LOSS_KEY ], out, tabs );
}

// WindTechnology::XMLDerivedClassParse ************************************

// Documentation is inherited
bool WindTechnology::XMLDerivedClassParse(
   const std::string&      nodeName,
   const xercesc::DOMNode* curr )
{
   if ( nodeName == sXMLTagNames[ CAPITAL_COST_KEY ] )
   {
      mCapitalCost = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ CUTOUT_SPEED_KEY ] )
   {
      mCutOutSpeed = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ FCR_KEY ] )
   {
      mFCR = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ GRID_CONNECTION_COST_KEY ] )
   {
      mGridConnectionCost = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ OM_KEY ] )
   {
      mOM = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ ROTOR_DIAMETER_KEY ] )
   {
      mRotorDiameter = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ TURBINE_DENSITY_KEY ] )
   {
      mTurbineDensity = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ TURBINE_DERATING_KEY ] )
   {
      mTurbineDerating = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ TURBINE_HUB_HEIGHT_KEY ] )
   {
      mTurbineHubHeight = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ TURBINE_RATING_KEY ] )
   {
      mTurbineRating = XMLHelper<double>::getValue( curr );
   }
   else if ( nodeName == sXMLTagNames[ WIND_FARM_LOSS_KEY ] )
   {
      mWindFarmLoss = XMLHelper<double>::getValue( curr );
   }
   else if ( IntermittentTechnology::XMLDerivedClassParse( nodeName, curr ) ) {
   }
   else
   {
      return false;
   }

   return true;
}

/*
   The one parameter Weibull distribution is (Equation A-1):
   f( x ) = ( B / c ) * ( x / c )^(B - 1) * e^-( x / c )^B

   The mean of the distribution is (Equation A-2):
   X = c * gamma * ( 1 / B + 1)

   The mean wind speed is (Equation A-3):
   X = c * gamma * 1.5 = ( sqrt( pi ) / 2 ) * c
   or
   c = X / a = 2X / sqrt( pi ) * a = sqrt( pi ) / 2 = 0.88623

   The wind distribution for a mean wind speed of is (Equation A-4):
   f( v ) = ( pi * v / 2X^2 ) * e^( -pi * ( v / 2X )^2

   Equation A-8:
   Xf = ( sqrt( pi ) / 2 ) * ( Vf / Vave )

   Equation A-10:
   1/2 * p * A * Cp * v^3 = 1 / 8 * p * D^2 * ( 16 / 27 ) * v^3 = Prated

   Equation A-18:
   (16 / 9 * pi) * Prb^2 * { 6 - [ Xr^6 + 3 Xr^4 + 6Xr^2 + 6 ]e^(-(Xr)^2) + Xr^6[ e^(-(Xr)^2) - e^(-(Xf)^2)]} = (16 / 9 * pi) * G( Xr, Xf ) * Prb^2

   Equation A-19:
   Var(P^2) = [ ( 16 / 9 * pi ) * G( Xr, Xf ) - CCfinite-power^2( Xr, Xf ) ] * Prb^2
*/
double WindTechnology::computeWindPowerVariance(
   double aAveWindSpeed,
    double aRating,
    double aDiameter,
    double aAirDensity,
    double aCutoutSpeed ) const
{
   static const double PI     = 2.0 * std::asin( 1.0 );

   // Equation A-18:
   // (16 / 9 * pi) * Prb^2 * { 6 - [ Xr^6 + 3 Xr^4 + 6Xr^2 + 6 ]e^(-(Xr)^2) + Xr^6[ e^(-(Xr)^2) - e^(-(Xf)^2)]} = (16 / 9 * pi) * G( Xr, Xf ) * Prb^2
   // Xr = rating
   // Xf = cutout speed
   double G = ( 6.0 - ( std::pow( aRating, 6.0 ) + 3.0 * std::pow( aRating, 4.0 ) + 6.0 * std::pow( aRating, 2.0 ) + 6.0 ) * std::exp( -std::pow( aRating, 2.0) ) + std::pow( aRating, 6.0 ) * ( std::exp( -std::pow( aRating, 2.0) ) - std::exp( -std::pow( aCutoutSpeed, 2.0 ) ) ) );

   // Equation A-19:
   // Var(P^2) = [ ( 16 / 9 * pi ) * G( Xr, Xf ) - CCfinite-power^2( Xr, Xf ) ] * Prb^2
   // Xr = rating
   // Xf = cutout speed
   double windPowerVariance = ( 16.0 / 9.0 * PI * G - std::pow( WindTechnology::calcTurbineCoefficient( aAveWindSpeed, aRating, aDiameter, aAirDensity, aCutoutSpeed ), 2.0 ) ) * std::pow( WindTechnology::calcIdealTurbineOutput( aAveWindSpeed, aDiameter, aAirDensity ), 2.0 );

   return windPowerVariance;
}

// end of wind_technology.cpp **********************************************


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <qle/pricingengines/crossccyswapengine.hpp>
#ifdef QL_USE_INDEXED_COUPON
#include <ql/cashflows/floatingratecoupon.hpp>
#endif

#include <qle/termstructures/crossccybasismtmresetswaphelper.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

namespace {
void no_deletion(YieldTermStructure*) {}
} // namespace

CrossCcyBasisMtMResetSwapHelper::CrossCcyBasisMtMResetSwapHelper(
    const Handle<Quote>& spreadQuote, const Handle<Quote>& spotFX, Natural settlementDays,
    const Calendar& settlementCalendar, const Period& swapTenor, BusinessDayConvention rollConvention,
    const boost::shared_ptr<QuantLib::IborIndex>& foreignCcyIndex,
    const boost::shared_ptr<QuantLib::IborIndex>& domesticCcyIndex,
    const Handle<YieldTermStructure>& foreignCcyDiscountCurve,
    const Handle<YieldTermStructure>& domesticCcyDiscountCurve,
    const Handle<YieldTermStructure>& foreignCcyFxFwdRateCurve,
    const Handle<YieldTermStructure>& domesticCcyFxFwdRateCurve, bool eom, bool spreadOnForeignCcy, bool invertFxIndex,
    boost::optional<Period> foreignTenor, boost::optional<Period> domesticTenor)
    : RelativeDateRateHelper(spreadQuote), spotFX_(spotFX), settlementDays_(settlementDays),
      settlementCalendar_(settlementCalendar), swapTenor_(swapTenor), rollConvention_(rollConvention),
      foreignCcyIndex_(foreignCcyIndex), domesticCcyIndex_(domesticCcyIndex),
      foreignCcyDiscountCurve_(foreignCcyDiscountCurve), domesticCcyDiscountCurve_(domesticCcyDiscountCurve),
      foreignCcyFxFwdRateCurve_(foreignCcyFxFwdRateCurve), domesticCcyFxFwdRateCurve_(domesticCcyFxFwdRateCurve),
      eom_(eom), spreadOnForeignCcy_(spreadOnForeignCcy), invertFxIndex_(invertFxIndex),
      foreignTenor_(foreignTenor ? *foreignTenor : foreignCcyIndex_->tenor()),
      domesticTenor_(domesticTenor ? *domesticTenor : domesticCcyIndex_->tenor()) {

    foreignCurrency_ = foreignCcyIndex_->currency();
    domesticCurrency_ = domesticCcyIndex_->currency();
    QL_REQUIRE(foreignCurrency_ != domesticCurrency_,
               "matching currencies not allowed on CrossCcyBasisMtMResetSwapHelper");

    bool foreignIndexHasCurve = !foreignCcyIndex_->forwardingTermStructure().empty();
    bool domesticIndexHasCurve = !domesticCcyIndex_->forwardingTermStructure().empty();
    bool haveForeignDiscountCurve = !foreignCcyDiscountCurve_.empty();
    bool haveDomesticDiscountCurve = !domesticCcyDiscountCurve_.empty();

    QL_REQUIRE(
        !(foreignIndexHasCurve && domesticIndexHasCurve && haveForeignDiscountCurve && haveDomesticDiscountCurve),
        "CrossCcyBasisMtMResetSwapHelper - Have all curves, nothing to solve for.");

    /* Link the curve being bootstrapped to the index if the index has
    no projection curve */
    if (foreignIndexHasCurve && haveForeignDiscountCurve) {
        if (!domesticIndexHasCurve) {
            domesticCcyIndex_ = domesticCcyIndex_->clone(termStructureHandle_);
            domesticCcyIndex_->unregisterWith(termStructureHandle_);
        }
        // if we have both index and discounting curve on foreign leg,
        // check foreignCcyFxFwdRateCurve and link it to foreign discount curve if empty
        // (we are bootstrapping on domestic leg in this instance, so foreign leg needs to be fully determined
        if (foreignCcyFxFwdRateCurve_.empty())
            foreignCcyFxFwdRateCurve_ = foreignCcyDiscountCurve_;
    } else if (domesticIndexHasCurve && haveDomesticDiscountCurve) {
        if (!foreignIndexHasCurve) {
            foreignCcyIndex_ = foreignCcyIndex_->clone(termStructureHandle_);
            foreignCcyIndex_->unregisterWith(termStructureHandle_);
        }
        // if we have both index and discounting curve on domestic leg,
        // check domesticCcyFxFwdRateCurve and link it to domestic discount curve if empty
        // (we are bootstrapping on foreign leg in this instance, so domestic leg needs to be fully determined
        if (domesticCcyFxFwdRateCurve_.empty())
            domesticCcyFxFwdRateCurve_ = domesticCcyDiscountCurve_;
    } else {
        QL_FAIL("Need one leg of the cross currency basis swap to "
                "have all of its curves.");
    }

    registerWith(spotFX_);
    registerWith(domesticCcyIndex_);
    registerWith(foreignCcyIndex_);
    registerWith(foreignCcyDiscountCurve_);
    registerWith(domesticCcyDiscountCurve_);
    registerWith(foreignCcyFxFwdRateCurve_);
    registerWith(domesticCcyFxFwdRateCurve_);

    initializeDates();
}

void CrossCcyBasisMtMResetSwapHelper::initializeDates() {

    Date refDate = evaluationDate_;
    // if the evaluation date is not a business day
    // then move to the next business day
    refDate = settlementCalendar_.adjust(refDate);

    Date settlementDate = settlementCalendar_.advance(refDate, settlementDays_, Days);
    Date maturityDate = settlementDate + swapTenor_;

    Schedule foreignLegSchedule = MakeSchedule()
                                      .from(settlementDate)
                                      .to(maturityDate)
                                      .withTenor(foreignTenor_)
                                      .withCalendar(settlementCalendar_)
                                      .withConvention(rollConvention_)
                                      .endOfMonth(eom_);

    Schedule domesticLegSchedule = MakeSchedule()
                                       .from(settlementDate)
                                       .to(maturityDate)
                                       .withTenor(domesticTenor_)
                                       .withCalendar(settlementCalendar_)
                                       .withConvention(rollConvention_)
                                       .endOfMonth(eom_);

    Real foreignNominal = 1.0;
    // build an FX index for forward rate projection (TODO - review settlement and calendar)
    boost::shared_ptr<FxIndex> fxIdx = boost::make_shared<FxIndex>(
        "dummy", settlementDays_, foreignCurrency_, domesticCurrency_, settlementCalendar_, spotFX_,
        foreignCcyFxFwdRateCurveRLH_, domesticCcyFxFwdRateCurveRLH_, invertFxIndex_);

    swap_ = boost::shared_ptr<CrossCcyBasisMtMResetSwap>(
        new CrossCcyBasisMtMResetSwap(foreignNominal, foreignCurrency_, foreignLegSchedule, foreignCcyIndex_, 0.0,
                                      domesticCurrency_, domesticLegSchedule, domesticCcyIndex_, 0.0, fxIdx));

    boost::shared_ptr<PricingEngine> engine;
    if (invertFxIndex_) {
        engine.reset(new CrossCcySwapEngine(foreignCurrency_, foreignDiscountRLH_, domesticCurrency_,
                                            domesticDiscountRLH_, spotFX_));
    } else {
        engine.reset(new CrossCcySwapEngine(domesticCurrency_, domesticDiscountRLH_, foreignCurrency_,
                                            foreignDiscountRLH_, spotFX_));
    }
    swap_->setPricingEngine(engine);

    earliestDate_ = swap_->startDate();
    latestDate_ = swap_->maturityDate();

/* May need to adjust latestDate_ if you are projecting libor based
   on tenor length rather than from accrual date to accrual date. */
#ifdef QL_USE_INDEXED_COUPON
    if (termStructureHandle_ == foreignCcyIndex_->forwardingTermStructure()) {
        Size numCashflows = swap_->leg(0).size();
        Date endDate = latestDate_;
        if (numCashflows > 0) {
            for (Size i = numCashflows - 1; i >= 0; i--) {
                boost::shared_ptr<FloatingRateCoupon> lastFloating =
                    boost::dynamic_pointer_cast<FloatingRateCoupon>(swap_->leg(0)[i]);
                if (!lastFloating)
                    continue;
                else {
                    Date fixingValueDate = foreignCcyIndex_->valueDate(lastFloating->fixingDate());
                    endDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    Date endValueDate = foreignCcyIndex_->maturityDate(fixingValueDate);
                    latestDate_ = std::max(latestDate_, endValueDate);
                    break;
                }
            }
        }
    }
    if (termStructureHandle_ == domesticCcyIndex_->forwardingTermStructure()) {
        Size numCashflows = swap_->leg(1).size();
        Date endDate = latestDate_;
        if (numCashflows > 0) {
            for (Size i = numCashflows - 1; i >= 0; i--) {
                boost::shared_ptr<FloatingRateCoupon> lastFloating =
                    boost::dynamic_pointer_cast<FloatingRateCoupon>(swap_->leg(1)[i]);
                if (!lastFloating)
                    continue;
                else {
                    Date fixingValueDate = domesticCcyIndex_->valueDate(lastFloating->fixingDate());
                    endDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    Date endValueDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    latestDate_ = std::max(latestDate_, endValueDate);
                    break;
                }
            }
        }
    }
#endif
}

void CrossCcyBasisMtMResetSwapHelper::setTermStructure(YieldTermStructure* t) {

    bool observer = false;
    boost::shared_ptr<YieldTermStructure> temp(t, no_deletion);

    termStructureHandle_.linkTo(temp, observer);

    if (foreignCcyDiscountCurve_.empty())
        foreignDiscountRLH_.linkTo(temp, observer);
    else
        foreignDiscountRLH_.linkTo(*foreignCcyDiscountCurve_, observer);

    if (domesticCcyDiscountCurve_.empty())
        domesticDiscountRLH_.linkTo(temp, observer);
    else
        domesticDiscountRLH_.linkTo(*domesticCcyDiscountCurve_, observer);

    // the below are the curves used for FX forward rate projection (for the resetting cashflows)
    if (foreignCcyFxFwdRateCurve_.empty())
        foreignCcyFxFwdRateCurveRLH_.linkTo(temp, observer);
    else
        foreignCcyFxFwdRateCurveRLH_.linkTo(*foreignCcyFxFwdRateCurve_, observer);

    if (domesticCcyFxFwdRateCurve_.empty())
        domesticCcyFxFwdRateCurveRLH_.linkTo(temp, observer);
    else
        domesticCcyFxFwdRateCurveRLH_.linkTo(*domesticCcyFxFwdRateCurve_, observer);

    RelativeDateRateHelper::setTermStructure(t);
}

Real CrossCcyBasisMtMResetSwapHelper::impliedQuote() const {
    QL_REQUIRE(termStructure_, "Term structure needs to be set");
    swap_->recalculate();
    if (spreadOnForeignCcy_)
        return swap_->fairForeignSpread();
    else
        return swap_->fairDomesticSpread();
}

void CrossCcyBasisMtMResetSwapHelper::accept(AcyclicVisitor& v) {
    Visitor<CrossCcyBasisMtMResetSwapHelper>* v1 = dynamic_cast<Visitor<CrossCcyBasisMtMResetSwapHelper>*>(&v);
    if (v1)
        v1->visit(*this);
    else
        RateHelper::accept(v);
}
} // namespace QuantExt


/** \file itasc/kdl/inertia.cpp
 *  \ingroup itasc
 */
// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#include "inertia.hpp"

#include <Eigen/Core>

namespace KDL {
using namespace Eigen;

Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz):
data(Matrix<double,6,6>::Zero())
{
    data(0,0)=Ixx;
    data(1,1)=Iyy;
    data(2,2)=Izz;
    data(2,1)=data(1,2)=Ixy;
    data(3,1)=data(1,3)=Ixz;
    data(3,2)=data(2,3)=Iyz;

	data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity();
}

Inertia::~Inertia()
{
}

}


// ************************
// ************************
// Edited by CCJ:
// evaldisp_lib.cpp,
// which can be called
// by python code;
// ************************
// ************************

// evaluate disparity map
// simple version for SDK
// supports upsampling of disp map if GT has higher resolution

// DS 7/2/2014
// 10/14/2014 changed computation of average error
// 1/27/2015 added clipping of valid (non-INF) disparities to [0 .. maxdisp]
//    in fairness to those methods that do not utilize the given disparity range
//    (maxdisp is specified at disp resolution, NOT GT resolution)

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include "imageLib/imageLib.h"
#include <stdint.h> /*for int32_t*/

//added by CCJ for Python + CPP coding;
#include <boost/python.hpp>
#include "boost/python/extract.hpp"
//#include "boost/python/numeric.hpp"
/* Updated by CCJ for Boost version 1.65:
 * Boost 1.65 removes boost/python/numeric.hpp
 */
// BOOST_LIB_VERSION and BOOST_VERSION are defined in this header file;
#include <boost/version.hpp>
#if BOOST_VERSION >= 106300 // >= 1.63.0
   #include "boost/python/numpy.hpp"
   namespace np = boost::python::numpy;
#else
   #include "boost/python/numeric.hpp"
	 namespace np = boost::python::numeric;
#endif

#include <numpy/ndarrayobject.h>
using namespace boost::python;

// see Fastest way to check if a file exist using standard C++/C++11/C?
// at https://stackoverflow.com/questions/12774207/fastest-way-to-check-if-a-file-exist-using-standard-c-c11-c;
#include <sys/stat.h>
inline bool exists_file_test(const std::string & name){
	struct stat buffer;
	return (stat (name.c_str(), &buffer) == 0);
}

const int verbose = 0;

/* The `occlusion mask` for the left image is given as a file "mask0nocc.png":
 * - Pixels without ground truth have the color (0, 0, 0).
 * - Pixels which are only observed by the left image have the color (128, 128, 128).
 * - Pixels which are observed by both images have the color (255, 255, 255).
 * - For the "non-occluded" evaluation, the evaluation is limited to the pixels observed by both images.
*/

void evaldisp(
		CFloatImage disp,
		CFloatImage gtdisp,
		CByteImage mask,
		float badthresh,
		int maxdisp,
		int rounddisp,
		float * statistics
		){

    CShape sh = gtdisp.Shape();
    CShape sh2 = disp.Shape();
    CShape msh = mask.Shape();

    int width = sh.width, height = sh.height;
    int width2 = sh2.width, height2 = sh2.height;
    int scale = width / width2;
		//std::cout << "scale = " << scale << "\n";

    if ((!(scale == 1 || scale == 2 || scale == 4))
				|| (scale * width2 != width)
				|| (scale * height2 != height)){
			printf("   disp size = %4d x %4d\n", width2, height2);
			printf("GT disp size = %4d x %4d\n", width,  height);
			throw CError("GT disp size must be exactly 1, 2, or 4 * disp size");
    }

    int usemask = (msh.width > 0 && msh.height > 0);
    if (!usemask)
			throw CError("No mask image's been read yet!\n");
    if (usemask && (msh != sh))
			throw CError("mask image must have same size as GT!\n");

		// all region;
    int n_all = 0;
    int bad_all = 0;
    int invalid_all = 0;
    float err_all = 0;
		// non-occluded region;
    int invalid_noc = 0;
		int n_noc = 0;
    int bad_noc = 0;
    float err_noc = 0;
		// updated for mae and rmse error metric, on 2019/08/31
    float err2_all = 0; // square
    float err2_noc = 0;

    for (int y = 0; y < height; y++) {
			for (int x = 0; x < width; x++) {
				float gt = gtdisp.Pixel(x, y, 0);
				if (gt == INFINITY) // unknown
					continue;
				float d = scale * disp.Pixel(x / scale, y / scale, 0);
	      int valid = (d != INFINITY);
				if (valid) {
					float maxd = scale * maxdisp; // max disp range
		      d = __max(0, __min(maxd, d)); // clip disps to max disp range
				}
	      if (valid && rounddisp){
					d = round(d);
				}
				float err = fabs(d - gt);

	      if (mask.Pixel(x,y,0) == 0){ // no ground truth;
					continue;
				}

				if (mask.Pixel(x, y, 0) == 255){ // non-occluded region;
					n_noc++;
					if (valid) {
						err_noc += err;
						err2_noc += err*err;

						if (err > badthresh) {
							bad_noc++;
						}
					}
					else {// invalid (i.e. hole in sparse disp map)
						invalid_noc++;
					}
				}

				n_all++;
				if(valid){
					err_all += err;
					err2_all += err * err;

					if (err > badthresh){
						bad_all++;
					}
				}
				else {// invalid (i.e. hole in sparse disp map)
					invalid_all++;
				}
			}/*end of width x*/
		}/*end of height y*/

		float badpercent_all =  (float)bad_all / (float)n_all;
    float avgErr_all = err_all / (float)(n_all - invalid_all); // CHANGED 10/14/2014 -- was: serr / n
		float badpercent_noc = (float) bad_noc / (float) n_noc;
		float avgErr_noc = err_noc /(float)(n_noc - invalid_noc);
		float rmse_all = sqrt(err2_all /(float)(n_all - invalid_all));
		float rmse_noc = sqrt(err2_noc /(float)(n_noc - invalid_noc));
#if 0
    float invalidpercent_all =  (float)invalid_all/(float)n_all;
    float totalbadpercent_all =  (float)(bad_all+invalid_all)/(float)n_all;
		printf("n_all = %d, n_noc = %d\n", n_all, n_noc);
    printf("%4.1f%(N-rate-all)  %6.2f%(bad-%2.1f-all)  %6.2f%(invalid-all)  %6.2f%(totalBad-all)  %6.2f(avgErr-all)\n",
				100.0*n_all/(width * height), 100.0*badpercent_all, badthresh, 100.0*invalidpercent_all,
				100.0*totalbadpercent_all, avgErr_all);
    float invalidpercent_noc =  (float)invalid_noc / (float)n_noc;
    float totalbadpercent_noc =  (float)(bad_noc+invalid_noc)/ (float)n_noc;
    printf("%4.1f%(N-rate-noc)  %6.2f%(bad-%2.1f-noc)  %6.2f%(invalid-noc)  %6.2f%(totalBad-noc)  %6.2f(avgErr-noc)\n",
				100.0*n_noc/(width * height), 100.0*badpercent_noc, badthresh, 100.0*invalidpercent_noc,
				100.0*totalbadpercent_noc, avgErr_noc);
#endif

		statistics[0] = badpercent_all;
		statistics[1] = avgErr_all;

		statistics[2] = badpercent_noc;
		statistics[3] = avgErr_noc;

		statistics[4] = rmse_all;
		statistics[5] = rmse_noc;
}

// C++ code
typedef std::vector<std::string> StringList;
class IMG_NAMES{
	private:
		StringList myvec;
	public:
		// constructor
		IMG_NAMES(){
			this -> myvec = StringList(0);
		}
	void set_img_names(boost::python::list & ns){
		for(int i = 0; i < len(ns); ++i){
			this -> myvec.push_back(boost::python::extract<std::string>(ns[i]));
		}
	}

	void show_img_names(){
		std:: cout << "Images :\n";
		for(auto i: this-> myvec){
			std::cout << i << ", ";
		}
		std:: cout << "\n";
	}

	StringList get_img_names(){
		return this -> myvec;
	}
};

PyObject * eval_disp_mbv3_1_img(
		const string & dispname,                // your predicted disparity results dir;
	  const string & disp_gt_trainingF_dir, 	// ground truth disparity dir to trainingF;
		float badthresh,                        // e.g., == 1.0 for Half resolution;
		const int &  ndisp,                      // max_disp in groud truth disparity;
		const int & rounddisp,
    const std::string & img_name           // 1 image to evaluate;
		){

		npy_intp * dims = new npy_intp[1];
		dims[0] = 4;
		PyObject * errs_A = PyArray_SimpleNew(1, dims, NPY_FLOAT32);
	  //std::cout << "new PyObject errs_A, img_N = " << img_N << std::endl;
		float * err_result = static_cast<float*>(PyArray_DATA(reinterpret_cast<PyArrayObject*>(errs_A)));

		//std::string dispname = disp_dir + img_name + "/" + disp_name; //"disp0.pfm"
		std::string gtdispname = disp_gt_trainingF_dir + img_name + "/disp0GT.pfm";
		// updated for middlebury additional data, due to the lack of mask0nocc.png file;
		std::string maskname = disp_gt_trainingF_dir + img_name + "/mask0nocc.png";
		bool readMask = true;
		if (!exists_file_test(maskname)){
			std::cout << "No mask0noc.png exists!\n";
			readMask = false;
		}

		//std::cout << "maxdisp = " << ndisp << ", "<< dispname << ", " << gtdispname << ", " << maskname << "\n";

		CFloatImage disp, gtdisp, gtdisp1;
		ReadImageVerb(disp, dispname.c_str(), verbose);
		ReadImageVerb(gtdisp, gtdispname.c_str(), verbose);
		CByteImage mask;
		if (readMask){
			//std::cout << "read mask0noc.png!\n";
			ReadImageVerb(mask, maskname.c_str(), verbose);
		}

		float statistics[4]= {-1.0, -1.0, -1.0, -1.0 };
		evaldisp(disp, gtdisp, mask, badthresh, ndisp, rounddisp, statistics);
		err_result[0] =  statistics[0];
		err_result[1] =  statistics[1];
		err_result[2] =  statistics[2];
		err_result[3] =  statistics[3];

		// release the memory
		disp.DeAllocate();
		gtdisp.DeAllocate();
		gtdisp1.DeAllocate();
		if (maskname.c_str())
			mask.DeAllocate();
		//exit(0);
		return errs_A;
}

PyObject * eval_disp_mbv3(
		const string & disp_dir, // your prediction disparity results dir;
	  const string & disp_gt_trainingF_dir, 	// ground truth disparity dir to trainingF;
		float badthresh, // e.g., == 1.0 for Half resolution;
		const string & disp_name, // e.g., == "_disp0" or "_disp0_post", or "_rf_disp0PKLS", and so on;
		PyObject * ndisps, // max_disp for each groud truth disparity
		PyObject * rounddisps, // disparity is int or float, specified in calib.txt files;
    boost::python::list img_names // img_names to evaluate;
		){

		/*vector<std::string> mb_v3_imgs_1 = {
			"Adirondack",
			"ArtL",
			"Jadeplant",
			"Motorcycle",
			"MotorcycleE",
			"Piano",
			"PianoL",
			"Pipes",
			"Playroom",
			"Playtable",
		 	"PlaytableP",
			"Recycle",
			"Shelves",
			"Teddy",
			"Vintage",
		};
		*/
    IMG_NAMES mb_v3;
		mb_v3.set_img_names(img_names);
		//mb_v3.show_img_names();
		vector<std::string> mb_v3_imgs = mb_v3.get_img_names();

    PyArrayObject* ndispsA = reinterpret_cast<PyArrayObject*>(ndisps);
    PyArrayObject* rounddispsA = reinterpret_cast<PyArrayObject*>(rounddisps);
		//int32_t * p_ndisps = reinterpret_cast<int32_t*>(PyArray_DATA(ndispsA));
		int * p_ndisps = reinterpret_cast<int*>(PyArray_DATA(ndispsA));
		int * p_rounddisps = reinterpret_cast<int*>(PyArray_DATA(rounddispsA));

		const int img_N = mb_v3_imgs.size();
		npy_intp * dims = new npy_intp[1];
		dims[0] = (4+2)*img_N;
		PyObject * errs_A = PyArray_SimpleNew(1, dims, NPY_FLOAT32);
	  //std::cout << "new PyObject errs_A, img_N = " << img_N << std::endl;
		float * err_result = static_cast<float*>(PyArray_DATA(reinterpret_cast<PyArrayObject*>(errs_A)));
		for (int i = 0; i < img_N; i++){
			std::string dispname = disp_dir + mb_v3_imgs[i] + "/" + disp_name + ".pfm";
			// check if a file exists or not;
			// > see: https://stackoverflow.com/questions/12774207/fastest-way-to-check-if-a-file-exist-using-standard-c-c11-c;
		  if (!exists_file_test(dispname)){
				//std::cout << "Not exist : " << dispname << ", changed it to : ";

				//dispname = disp_dir + mb_v3_imgs[i] + "_" + disp_name;
				//Updated: delete "_" here;
				dispname = disp_dir + mb_v3_imgs[i] + disp_name + ".pfm";
				//std::cout << dispname << " ";
				if (!exists_file_test(dispname)){
					std::cout << "loading " << dispname << " ... but failed!\n";
				}
			}

			std::string gtdispname = disp_gt_trainingF_dir + mb_v3_imgs[i] + "/disp0GT.pfm";
			std::string maskname = disp_gt_trainingF_dir + mb_v3_imgs[i] + "/mask0nocc.png";
			bool readMask = true;
			// updated for middlebury additional data, due to the lack of mask0nocc.png file;
			if (!exists_file_test(maskname)){
			  std::cout << "No mask0noc.png exists!\n";
				readMask = false;
			}
	    int maxdisp = p_ndisps[i];
	    int rounddisp = p_rounddisps[i];
      //std::cout << "maxdisp = " << maxdisp << ", rounddisp = " << rounddisp << ", "<< dispname << ", " << gtdispname << ", " << maskname << "\n";

	    CFloatImage disp, gtdisp, gtdisp1;
	    ReadImageVerb(disp, dispname.c_str(), verbose);
	    ReadImageVerb(gtdisp, gtdispname.c_str(), verbose);
	    CByteImage mask;
	    if (readMask){
				//std::cout << "read mask0noc.png!\n";
				ReadImageVerb(mask, maskname.c_str(), verbose);
			}

      //float statistics[4]= {-1.0, -1.0, -1.0, -1.0 };
			// updated for rmse metric on 2019/08/31;
	    float statistics[4 + 2]= {-1.0, -1.0, -1.0, -1.0, -1.0, -1.0 };
			evaldisp(disp, gtdisp, mask, badthresh, maxdisp, rounddisp, statistics);
#if 0
			printf("Processing: %10s, %6.2f%(bad-%2.1f-all), %6.2f(avgErr-all), %6.2f%(bad-%2.1f-noc), %6.2f(avgErr-nov)\n",
					mb_v3_imgs[i].c_str(), statistics[0]*100.0, badthresh,
					statistics[1], statistics[2]*100.0, badthresh, statistics[3]);
#endif
			err_result[6*i]     =  statistics[0];
			err_result[6*i + 1] =  statistics[1];
			err_result[6*i + 2] =  statistics[2];
			err_result[6*i + 3] =  statistics[3];
			err_result[6*i + 4] =  statistics[4];
			err_result[6*i + 5] =  statistics[5];

			// release the memory
			disp.DeAllocate();
			gtdisp.DeAllocate();
			gtdisp1.DeAllocate();
	    if (maskname.c_str())
				mask.DeAllocate();

			//exit(0);

		}/*end of each image*/
		return errs_A;
}

BOOST_PYTHON_MODULE(libevaldisp_mbv3){
	/* for Boost <= version 1.63*/
	//numeric::array::set_module_and_type("numpy", "ndarray");
  /* for Boost > version 1.63*/
	//np::initialize();
#if BOOST_VERSION >= 106300 // >= 1.63.0
	np::initialize();
#else
	np::array::set_module_and_type("numpy", "ndarray");
#endif
	def("evaluate_mbv3", eval_disp_mbv3);
	def("evaluate_mbv3_1_img", eval_disp_mbv3_1_img);
	/* Error: return-statement with a value, in function returning 'void' [-fpermissive]
	 *        #define NUMPY_IMPORT_ARRAY_RETVAL NULL
	 * > See solution: https://github.com/numpy/numpy/issues/10486
	 * > 1) Solution1: Okay, so the issue occurs only on py2 + py3c (the initialization function is nonstandard, and has py3 semantics). Solution appears to be to use `import_array1()`;
	 * > 2) Solution2: Or call _import_array(), which allows you more control than just `return`;
	 */
  //import_array(); // work well for Python2.7;
	import_array1(); // work well for python3.7;
	//_import_array(); // work well for python3.7;
}


/*
  Copyright (c) 2012,2013,2014 Matthew H. Reilly (kb1vc)
  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.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "ReSampler.hxx"
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <boost/format.hpp>

static void bindump(char * fn, std::complex<float> * buf, unsigned int num_elts, int append = 0) __attribute__ ((unused));
static void bindump(char * fn, std::complex<float> * buf, unsigned int num_elts, int append)
{
  FILE * of;
  if(append != 0) {
    of = fopen(fn, "a");
  }
  else {
    of = fopen(fn, "w");
  }
  unsigned int i;
  for(i = 0; i < num_elts; i++) {
    fprintf(of, "%d %g %g\n", i + (append * num_elts), buf[i].real(), buf[i].imag());
  }
  fclose(of);
}

static void bindump_2(char * fn, std::complex<float> * buf, unsigned int num_elts, unsigned int num_cols) __attribute__ ((unused));
static void bindump_2(char * fn, std::complex<float> * buf, unsigned int num_elts, unsigned int num_cols)
{
  FILE * of;
  of = fopen(fn, "w");
  unsigned int i, j;
  for(i = 0; i < num_elts; i++) {
    fprintf(of, "%d ", i);
    for(j = 0; j < num_cols; j++) {
      fprintf(of, "%g %g ", buf[i * num_cols + j].real(), buf[i * num_cols + j].imag());
    }
    fprintf(of, "\n");
  }
  fclose(of);
}

static unsigned int ipow(unsigned int a, unsigned int b)
{
  unsigned int ret = 1;
  unsigned int i ;

  for(i = 0; i < b; i++) {
    ret *= a;
  }

  return ret;
}

SoDa::ReSampler::ReSampler(unsigned int interpolate_ratio,
			   unsigned int decimate_ratio,
			   unsigned int _inlen,
			   unsigned int _filter_len)
{
  // save the parameters
  inlen = _inlen;
  iM = interpolate_ratio;
  dN = decimate_ratio;
  filter_len = _filter_len;
  Q = filter_len;
  M = _inlen;

  // give preferences to convenient powers of two.
  // but let's search for the nearest solution that is
  // a multiple of 2^a * 3^b * 5^c where b and c are in the range 0..3
  // and a is in the range 1..16
  unsigned int N_guess, N_best, E_best;
  N_best = 2;
  E_best = 0x80000000;
  unsigned int i;
  for(i = 1; i <= 0xff; i++) {
    unsigned int a = i & 0xf;
    unsigned int b = (i >> 4) & 0x3;
    unsigned int c = (i >> 6) & 0x3;
    N_guess = ipow(2, a) * ipow(3, b) * ipow(5, c);
    if(N_guess >= (M + Q - 1)) {
      unsigned slop = N_guess - (M + Q - 1);
      if(slop < E_best) {
	N_best = N_guess;
	E_best = slop;
      }
    }
  }
  N = N_best;

  // now that we have N, we can back-calculate Q.
  Q = N - M;

  filter_len = Q;

  tail_index = M - (Q - 1);

  // create the buffers.
  // a copy of the input sequence
  inbuf = (std::complex<float> *) fftwf_alloc_complex(N);
  // the transformed input
  in_fft = (std::complex<float> *) fftwf_alloc_complex(N);

  // zero out the whole of the input buffer
  for(i = 0; i < N; i++) inbuf[i] = std::complex<float>(0.0,0.0);

  // the filter bank is really iM interleaved filters
  // but we can't seem to make that work right now, so we're
  // backing off to a simpler way.
  c_filt = new std::complex<float>*[iM];
  for(i = 0; i < iM; i++) {
    c_filt[i] = (std::complex<float> *) fftwf_alloc_complex(N);
  }

  // the upsampled result
  filt_fft = (std::complex<float> *) fftwf_alloc_complex(iM*N);
  // the inverse transform of the upsampled result
  interp_res = (std::complex<float> *) fftwf_alloc_complex(iM*N);

  // the first plan is pretty simple.
  in_fft_plan = fftwf_plan_dft_1d(N,
				  (fftwf_complex *) inbuf,
				  (fftwf_complex *) in_fft,
				  FFTW_FORWARD, FFTW_ESTIMATE);

  // now create the inverting plans
  int n[1];
  n[0] = N;
  mid_ifft_plan = fftwf_plan_many_dft(1, n, iM,
				      (fftwf_complex *) filt_fft, NULL, iM, 1,
				      (fftwf_complex *) interp_res, NULL, iM, 1,
				      FFTW_BACKWARD, FFTW_ESTIMATE);

  // now create the polyphase filter banks.
  CreateFilter(filter_len);

  transform_gain = 1.0 / ((float) N);
}

void SoDa::ReSampler::CreateFilter(unsigned int filter_len)
{
  // filter_len must be odd
  int fN = filter_len;
  if((fN & 1) == 0) fN--;

  unsigned int i, j, k;

  // Use the plan of fischer in "The mkfilter Digital Filter Generation Program" mkshape...
  // This is borrowed quite heavily and with tremendous gratitude from
  // http://www-users.cs.york.ac.uk/~fisher   Thanks, Prof. Fisher...
  float alpha, beta;
  float cutdown = ((float) ((iM > dN) ? iM : dN));
  beta = 1.0 / (2.2 * cutdown); // This is the transition freq -- a little shy of 0.5/cutdown
  alpha = 0.1; // this is a shape factor

  // create a frequency domain image of the filter.
  float f1 = (1.0 - alpha) * beta;
  float f2 = (1.0 + alpha) * beta;
  float tau = 0.5 / alpha;

  // now create the filter banks.
  std::complex<float> * filt_FD = (std::complex<float>*) fftwf_alloc_complex(N);
  std::complex<float> * filt_td = (std::complex<float>*) fftwf_alloc_complex(N);
  std::complex<float> * filt_td2 = (std::complex<float>*) fftwf_alloc_complex(N);

  float f;
  float f_incr = 1.0 / ((float) (N));
  unsigned int hlim = (N);
  float gain_corr = 1.0 / ((float) hlim);
  //  gain_corr = gain_corr * gain_corr;
  for(i = 0, f = 0.0; i <= hlim / 2; i++, f += f_incr) {
    if(f <= f1) {
      filt_FD[i] = std::complex<float>(1.0, 0.0);
    }
    else if (f <= f2) {
      float h = 0.5 * (1.0 + cos(M_PI * tau * (f - f1) / beta));
      filt_FD[i] = std::complex<float>(h, 0.0);
    }
    else {
      filt_FD[i] = std::complex<float>(0.0, 0.0);
    }

    filt_FD[i] *= tau * gain_corr;

    if(i != 0) filt_FD[hlim - i] = filt_FD[i];
  }

  // then ifft it to time domain
  fftwf_plan filt_ifft_plan = fftwf_plan_dft_1d(hlim,
						(fftwf_complex *) filt_FD,
						(fftwf_complex *) filt_td,
						FFTW_BACKWARD, FFTW_ESTIMATE);
  fftwf_execute(filt_ifft_plan);

  float sum = 0.0;
  for(i = 0; i < hlim; i++) sum += filt_td[i].real();

  float sgain = 1.0 / sum;
  for(i = 0; i < hlim; i++) filt_td[i] *= sgain;

  fftwf_destroy_plan(filt_ifft_plan);

  // now truncate the filter
  // We've got an image that is symmetric around 0.
  for(i = 0; i < N; i++) {
    filt_td2[i] = std::complex<float>(0.0,0.0);
  }

  // shift it up to be symmetric around filt_len / 2
  for(i = 0, j = N - (filter_len/2); j < N; i++, j++) {
    filt_td2[i] = filt_td[j];
  }
  for(j = 0; j < filter_len / 2; j++, i++) {
    filt_td2[i] = filt_td[j];
  }

  //  bindump("filt_td2.dat", filt_td2, N);

  // and fft back to frequency domain for each of the iM filters.
  for(i = 0; i < iM; i++) {
    // grab the slice of the filter that we need to transform.

    for(j = i, k = 0; j < filter_len; k++, j += iM) {
      filt_td[k] = filt_td2[j];
    }
    for(; k < N; k++) filt_td[k] = std::complex<float>(0.0,0.0);
    fftwf_plan filt_fft_plan = fftwf_plan_dft_1d(N,
						 (fftwf_complex *) filt_td,
						 (fftwf_complex *) c_filt[i],
						 FFTW_FORWARD, FFTW_ESTIMATE);
    fftwf_execute(filt_fft_plan);
    fftwf_destroy_plan(filt_fft_plan);
  }

  // now free up buffers
  fftwf_free(filt_td2);
  fftwf_free(filt_td);
  fftwf_free(filt_FD);
}

unsigned int SoDa::ReSampler::apply(std::complex<float> * in,
				    std::complex<float> * out,
				    float gain)
{
  unsigned int i, j;
  // copy the input buffer
  memcpy(&(inbuf[Q-1]), in, sizeof(std::complex<float>) * M);

  // transform the input
  fftwf_execute(in_fft_plan); // , (fftwf_complex *) in, (fftwf_complex *) in_fft);

  // now multiply by the M filters
  for(i = 0; i < N; i++) {
    for(j = 0; j < iM; j++) {
      filt_fft[j + i*iM] = in_fft[i] * c_filt[j][i];
    }
  }
  // do M inverse transforms on the filter result ...
  // to produce M interleaved time domain vectors.
  fftwf_execute(mid_ifft_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase inbuf and outbuf are the same buffers.  (we may
  // over-write the outbuf with the downsample at the bottom.... )
  memcpy(inbuf, &(in[tail_index]), sizeof(std::complex<float>) * (Q-1));

  // now build the output buffer.

  // now downsample by a factor of dN.
  int idx = 0;
  unsigned int mctr = 0;
  unsigned int out_lim = (M * iM) / dN;
  std::complex<float> * ir_clean = &(interp_res[(Q-1)]);
  // std::cerr << " iM, dN, M, N, out_lim = "
  // 	    << iM << " " << dN << " " << M << " " << N << " " << out_lim << std::endl;
  for(j = 0; j < out_lim; j++) {
    out[j] = ir_clean[mctr + idx * iM] * gain * transform_gain * ((float) iM);
    mctr += dN;
    while(mctr >= iM) {
      mctr -= iM;
      idx += 1;
    }
  }
  //  bindump("out.dat", out, j);

  return 0;
}

unsigned int SoDa::ReSampler::apply(float * in,
				    float * out,
				    float gain)
{
  unsigned int i, j;

  for(i = 0; i < M; i++) {
    inbuf[i+(Q-1)] = std::complex<float>(in[i], 0.0);
  }

  // transform the input
  fftwf_execute(in_fft_plan); // , (fftwf_complex *) in, (fftwf_complex *) in_fft);

  // now multiply by the M filters
  for(i = 0; i < N; i++) {
    for(j = 0; j < iM; j++) {
      filt_fft[j + i*iM] = in_fft[i] * c_filt[j][i];
    }
  }
  // do M inverse transforms on the filter result ...
  // to produce M interleaved time domain vectors.
  fftwf_execute(mid_ifft_plan);

  // save the last bits of the input buffer to the Q-1 side of the FFT input vector
  // Do this now incase inbuf and outbuf are the same buffers.  (we may
  // over-write the outbuf with the downsample at the bottom.... )
  for(i = 0; i < (Q-1); i++) {
    inbuf[i] = std::complex<float>(in[i + tail_index], 0.0);
  }

  // now build the output buffer.

  // now downsample by a factor of dN.
  int idx = 0;
  unsigned int mctr = 0;
  unsigned int out_lim = (M * iM) / dN;
  std::complex<float> * ir_clean = &(interp_res[(Q-1)]);
  for(j = 0; j < out_lim; j++) {
    out[j] = ir_clean[mctr + idx * iM].real() * gain * transform_gain * ((float) iM);
    mctr += dN;
    while(mctr >= iM) {
      mctr -= iM;
      idx += 1;
    }
  }

  return 0;
}


// *****************************************************************************
/*!
  \file      src/RNG/RNGSSE.hpp
  \copyright 2012-2015 J. Bakosi,
             2016-2018 Los Alamos National Security, LLC.,
             2019 Triad National Security, LLC.
             All rights reserved. See the LICENSE file for details.
  \brief     Interface to RNGSSE random number generators
  \details   Interface to RNGSSE random number generators
*/
// *****************************************************************************
#ifndef RNGSSE_h
#define RNGSSE_h

#include <cstring>
#include <random>

#include "NoWarning/beta_distribution.hpp"
#include <boost/random/gamma_distribution.hpp>

#include "Make_unique.hpp"
#include "Exception.hpp"
#include "Macro.hpp"
#include "Options/RNGSSESeqLen.hpp"

namespace tk {

//! RNGSSE-based random number generator used polymorphically with tk::RNG
template< class State, typename SeqNumType, unsigned int (*Generate)(State*) >
class RNGSSE {

  private:
    using InitFn = void (*)( State*, SeqNumType );
    using ncomp_t = kw::ncomp::info::expect::type;

    //! Adaptor to use a std distribution with the RNGSSE generator
    //! \see C++ concepts: UniformRandomNumberGenerator
    struct Adaptor {
      using result_type = unsigned int;
      Adaptor( const std::unique_ptr< State[] >& s, int t ) : str(s), tid(t) {}
      static constexpr result_type min() { return 0u; }
      static constexpr result_type max() { return 4294967295u; }
      result_type operator()()
      { return Generate( &str[ static_cast<std::size_t>(tid) ] ); }
      const std::unique_ptr< State[] >& str;
      int tid;
    };

  public:
    //! Constructor
    //! \param[in] n Initialize RNG using this many independent streams
    //! \param[in] fnShort RNG initializer function for short streams
    //! \param[in] seqlen Sequence length enum: short, medium or long
    //! \param[in] fnLong RNG initializer function for long streams
    //! \param[in] fnMed RNG initializer function for medium streams
    explicit RNGSSE( SeqNumType n,
                     InitFn fnShort,
                     ctr::RNGSSESeqLenType seqlen = ctr::RNGSSESeqLenType::SHORT,
                     InitFn fnLong = nullptr,
                     InitFn fnMed = nullptr) :
       m_nthreads( n ),
       m_init( seqlen == ctr::RNGSSESeqLenType::LONG ? fnLong :
               seqlen == ctr::RNGSSESeqLenType::MEDIUM ? fnMed : fnShort ),
       m_stream()
    {
      Assert( m_init != nullptr, "nullptr passed to RNGSSE constructor" );
      Assert( n > 0, "Need at least one thread" );
      // Allocate array of stream-pointers for threads
      m_stream = tk::make_unique< State[] >( n );
      // Initialize thread-streams
      for (SeqNumType i=0; i<n; ++i) m_init( &m_stream[i], i );
    }

    //! Uniform RNG: Generate uniform random numbers
    //! \param[in] tid Thread (or more precisely) stream ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in,out] r Pointer to memory to write the random numbers to
    void uniform( int tid, ncomp_t num, double* r ) const {
      for (ncomp_t i=0; i<num; ++i)
        r[i] = static_cast<double>(
                 Generate( &m_stream[ static_cast<std::size_t>(tid) ] ) )
               / 4294967296.0;
    }

    //! Gaussian RNG: Generate Gaussian random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating Gaussian random numbers is implemented via an
    //!   adaptor, modeling std::UniformRandomNumberGenerator, outsourcing the
    //!   transformation of uniform random numbers to Gaussian ones, to the
    //!   standard library. The adaptor is instantiated here because a standard
    //!   distribution, such as e.g., std::normal_distribution, generates
    //!   numbers using operator() with no arguments, thus the RNG state and the
    //!   thread ID (this latter only known here) must be stored in the adaptor
    //!   functor's state. Even though creating the adaptor seems like a
    //!   potentially costly operation for every call, using the standard
    //!   library implementation is still faster than a hand-coded
    //!   implementation of the Box-Muller algorithm. Note that libc++ uses a
    //!   cache, as Box-Muller, implemented using the polar algorithm generates
    //!   2 Gaussian numbers for each pair of uniform ones, caching every 2nd.
    void gaussian( int tid, ncomp_t num, double* r ) const {
      Adaptor generator( m_stream, tid );
      std::normal_distribution<> gauss_dist( 0.0, 1.0 );
      for (ncomp_t i=0; i<num; ++i) r[i] = gauss_dist( generator );
    }

    //! \brief Multi-variate Gaussian RNG: Generate multi-variate Gaussian
    //!    random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] d Dimension d ( d ≥ 1) of output random vectors
    //! \param[in] mean Mean vector of dimension d
    //! \param[in] cov Lower triangle of covariance matrix, stored as a vector
    //!   of length d(d+1)/2
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \warning Not implemented!
    void gaussianmv( int tid, ncomp_t num, ncomp_t d, const double* const mean,
                     const double* const cov, double* r ) const
    {
      IGNORE(tid);
      IGNORE(num);
      IGNORE(d);
      IGNORE(mean);
      IGNORE(cov);
      IGNORE(r);
    }

    //! Beta RNG: Generate beta random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] p First beta shape parameter
    //! \param[in] q Second beta shape parameter
    //! \param[in] a Beta displacement parameter
    //! \param[in] b Beta scale factor
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating beta-distributed random numbers is implemented via
    //!   an adaptor, modeling boost::UniformRandomNumberGenerator, outsourcing
    //!   the transformation of uniform random numbers to beta-distributed ones,
    //!   to boost::random. The adaptor is instantiated here because a boost
    //!   random number distribution, such as e.g.,
    //!   boost::random::beta_distribution, generates numbers using operator()
    //!   with no arguments, thus the RNG state and the thread ID (this latter
    //!   only known here) must be stored in the adaptor functor's state.
    void beta( int tid, ncomp_t num, double p, double q, double a, double b,
               double* r ) const {
      Adaptor generator( m_stream, tid );
      boost::random::beta_distribution<> beta_dist( p, q );
      for (ncomp_t i=0; i<num; ++i) r[i] = beta_dist( generator ) * b + a;
    }

    //! Gamma RNG: Generate gamma random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] a Gamma shape parameter
    //! \param[in] b Gamma scale factor
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating gamma-distributed random numbers is implemented via
    //!   an adaptor, modeling boost::UniformRandomNumberGenerator, outsourcing
    //!   the transformation of uniform random numbers to gamma-distributed
    //!   ones, to boost::random. The adaptor is instantiated here because a
    //!   boost random number distribution, such as e.g.,
    //!   boost::random::gamma_distribution, generates numbers using operator()
    //!   with no arguments, thus the RNG state and the thread ID (this latter
    //!   only known here) must be stored in the adaptor functor's state.
    void gamma( int tid, ncomp_t num, double a, double b, double* r ) const {
      Adaptor generator( m_stream, tid );
      boost::random::gamma_distribution<> gamma_dist( a, b );
      for (ncomp_t i=0; i<num; ++i) r[i] = gamma_dist( generator );
    }

    //! Copy assignment
    RNGSSE& operator=( const RNGSSE& x ) {
      m_nthreads = x.m_nthreads;
      m_init = x.m_init;
      m_stream = tk::make_unique< State[] >( x.m_nthreads );
      for (SeqNumType i=0; i<x.m_nthreads; ++i) m_init( &m_stream[i], i );
      return *this;
    }

    //! Copy constructor: in terms of copy assignment
    RNGSSE( const RNGSSE& x ) { operator=(x); }

    //! Move assignment
    RNGSSE& operator=( RNGSSE&& x ) {
      m_nthreads = x.m_nthreads;
      m_init = x.m_init;
      m_stream = tk::make_unique< State[] >( x.m_nthreads );
      for (SeqNumType i=0; i<x.m_nthreads; ++i) {
        m_stream[i] = x.m_stream[i];
        std::memset( &x.m_stream[i], 0, sizeof(x.m_stream[i]) );
      }
      x.m_nthreads = 0;
      x.m_init = nullptr;
      x.m_stream.reset( nullptr );
      return *this;
    }

    //! Move constructor: in terms of move assignment
    RNGSSE( RNGSSE&& x ) :
      m_nthreads( 0 ),
      m_init( nullptr ),
      m_stream( nullptr )
    { *this = std::move( x ); }

    //! Accessor to the number of threads we operate on
    SeqNumType nthreads() const noexcept { return m_nthreads; }

  private:
    SeqNumType m_nthreads;                 //!< Number of threads
    InitFn m_init;                         //!< Sequence length initializer
    std::unique_ptr< State[] > m_stream;   //!< Random number stream for threads
};

} // tk::

#endif // RNGSSE_h


////////////////////////////////////////////////////////////////////////////
//! \file    robot.cpp
//! \date    February 2018
//! \author  Eric Barnett
//!          Marc-Antoine Lacasse
//!
//! Copyright (C) 2018 Eric Barnett         <e.barnett@robotiq.com>
//! Copyright (C) 2018 Marc-Antoine Lacasse <malacasse@robotiq.com>
//! Copyright (C) 2018 Clément Gosselin     <gosselin@gmc.ulaval.ca>
//!
//! \brief Robot-specific kinematics and dynamics
//!
//! This file is part of BA, a bisection algorithm for time-optimal trajectory
//! planning
//!
//! This Source Code Form is subject to the terms of the Berkeley Software
//! Distribution (BSD) 3-clause license . If a copy of the BSD license was not
//! distributed with this file, you can obtain one at
//! https://opensource.org/licenses/BSD-3-Clause.
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////
#include "util.h"
#include "robot.h"
#include "config.h"
#include <array>
#include <Eigen/Core> // for linear Algebra in fwdKinKuka()
#ifndef isWin
   #include <limits>
   #include "string.h"
#endif

using namespace Eigen;

namespace BATOTP{

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief set_robotType
//!
//! Set the robot type (define new robots here)
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_set_robotType(const std::string& robotTypeStr)
{
    return set_robotType(robotTypeStr);
}

int Robot::set_robotType(const std::string &robotTypeStr)
{
   _robotType=0;
   _robotTypeStr=robotTypeStr;
   if(_robotTypeStr=="KUKA")     _robotType=KUKA;
   if(_robotTypeStr=="UR")       _robotType=UR;
   if(_robotTypeStr=="RR")       _robotType=RR;
   if(_robotTypeStr=="CSPR3DOF") _robotType=CSPR3DOF;
   if(_robotTypeStr=="GENJNT")   _robotType=GENJNT;

   return(_robotType);
}

//! FORWARD KINEMATICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKin
//!
//! Forward displacement problem
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_fwdKin(const std::vector<std::vector<double> >& theta,
                       std::vector<std::vector<double>> &cart)
{return(fwdKin(theta,cart));}
int Robot::fwdKin(const std::vector<std::vector<double>> &theta,
                  std::vector<std::vector<double>> &cart)
{

   switch(_robotType)
   {
   case KUKA: //KUKA-LWR IV+
      fwdKinKuka(theta,cart);
      break;
   case RR: // planar RR positioning manipulator
      fwdKinRR(theta,cart);
      break;
   default:
      printf ("No forward Kinematics model provided for robotType=%s.\n",
              _robotTypeStr.c_str());
      return -1;
      assert(!"Unhandled case"); // Always assert unhandled cases
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKinKuka
//!
//! Forward displacement problem for the Kuka LWR IV+
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::fwdKinKuka(const std::vector<std::vector<double> >& theta,
                      std::vector<std::vector<double>> &cart)
{
   int i;
   Matrix3d Q12,Q34,Q567,Q1234,Q;
   //op=[0; 0; .282];  .204+78 Robotiq hand with Robotiq fingers (center of pads)
   //op=[0; 0; .3145]; Robotiq hand with Robotiq fingers (tips of pads)
   Vector3d ToolPointVector(0,-.08,.545); // .468+.078 Robotiq hand with long fingers

   int nPts=(int)theta[0].size();

   double a0=.3105,a1=.4,a2=.39; // link lengths

   double t1,t2,t3,t4,t5,t6,t7;
   double c1,c2,c3,c4,c5,c6,c7;
   double s1,s2,s3,s4,s5,s6,s7;
   double x1,y1,z1;
   double x2,y2,z2;
   double x3,y3,z3;

   cart.resize(3);
   for(i=0;i<3;i++) cart[i].resize(nPts);

   for(i=0; i<nPts; i++)
   {
      t1=_DEG2RAD*theta[0][i]; c1=cos(t1); s1=sin(t1);
      t2=_DEG2RAD*theta[1][i]; c2=cos(t2); s2=sin(t2);
      t3=_DEG2RAD*theta[2][i]; c3=cos(t3); s3=sin(t3);
      t4=_DEG2RAD*theta[3][i]; c4=cos(t4); s4=sin(t4);
      t5=_DEG2RAD*theta[4][i]; c5=cos(t5); s5=sin(t5);
      t6=_DEG2RAD*theta[5][i]; c6=cos(t6); s6=sin(t6);
      t7=_DEG2RAD*theta[6][i]; c7=cos(t7); s7=sin(t7);

      Q12 <<  c1*c2, -s1, -c1*s2,
            c2*s1,  c1, -s1*s2,
            s2,   0,     c2;

      Q34 << c3*c4, -s3, c3*s4,
            c4*s3,  c3, s3*s4,
            -s4,   0,    c4;

      Q567 << c5*c6*c7 - s5*s7, - c7*s5 - c5*c6*s7, -c5*s6,
            c5*s7 + c6*c7*s5,   c5*c7 - c6*s5*s7, -s5*s6,
            c7*s6,             -s6*s7,     c6;

      Q1234=Q12*Q34; Q=Q1234*Q567;

      // elbow
      x1=a1*Q12(0,2);
      y1=a1*Q12(1,2);
      z1=a1*Q12(2,2)+a0;

      // wrist
      x2=x1+a2*Q1234(0,2);
      y2=y1+a2*Q1234(1,2);
      z2=z1+a2*Q1234(2,2);

      // tool point
      x3=x2+Q.row(0)*ToolPointVector;
      y3=y2+Q.row(1)*ToolPointVector;
      z3=z2+Q.row(2)*ToolPointVector;

      // normal (tool-pointing direction)
      //n << x3-x2, y3-y2, z3-z2 ;
      //n=n.normalized();

      cart[0][i]=x3;
      cart[1][i]=y3;
      cart[2][i]=z3;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKinRR
//!
//! Forward displacement problem for a RR planar robot
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::fwdKinRR(const std::vector<std::vector<double> >& theta,
                    std::vector<std::vector<double>> &cart)
{
   int i,nPts=(int)theta[0].size();
   double a1=.4,a2=.6;
   double th1,th2;
   cart.resize(3);
   for(i=0;i<3;i++) cart[i].resize(nPts);

   for(i=0;i<nPts;i++)
   {
      th1=_DEG2RAD*theta[0][i];
      th2=_DEG2RAD*theta[1][i];
      cart[0][i]=a1*std::cos(th1)+a2*std::cos(th1+th2);
      cart[1][i]=a1*std::sin(th1)+a2*std::sin(th1+th2);
   }
   return 0;
}

//! INVERSE KINEMATICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief invKin
//!
//! Inverse displacement problem
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_invKin(std::vector<std::vector<double>> &theta,
                       const std::vector<std::vector<double>> &cart)
{return(invKin(theta,cart));}
int Robot::invKin(std::vector<std::vector<double>> &theta,
                  const std::vector<std::vector<double>> &cart)
{
   switch(_robotType)
   {
   case CSPR3DOF:
      invKinCSPR3DOF(theta,cart);
      break;
   default:
      printf ("No inverse Kinematics model provided for robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief invKinCSPR3DOF
//!
//! Inverse kinematics for the large 3-DOF point-mass CSPR in PLT-00370:
//! finds cable lengths (theta) from Cartesian EE positions (cart) and
//! cable attacment points (pmat)
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::invKinCSPR3DOF(std::vector<std::vector<double>> &theta,
                          const std::vector<std::vector<double>> &cart)
{

   int i;
   int nDim=(int)cart.size();

   int nPts=(int)cart[0].size();
   theta.resize(3);

   for(i=0;i<nDim;i++) theta[i].resize(nPts);
   double x,y,z,rho1,rho2,rho3;
   std::array<double,3> rv1,rv2,rv3;

   if(_pmat.size()==0)
   {
      findCSPR3DOFpmat(_pmat);
   }

   for(i=0;i<nPts;i++)
   {
      x=cart[0][i];
      y=cart[1][i];
      z=cart[2][i];
      rv1={x-_pmat[0][0], y-_pmat[1][0], z-_pmat[2][0]};
      rv2={x-_pmat[0][1], y-_pmat[1][1], z-_pmat[2][1]};
      rv3={x-_pmat[0][2], y-_pmat[1][2], z-_pmat[2][2]};
      rho1=norm(rv1);
      rho2=norm(rv2);
      rho3=norm(rv3);
      theta[0][i]=rho1;
      theta[1][i]=rho2;
      theta[2][i]=rho3;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief findCSPR3DOFpmat
//!
//! Compute the cable attachment points for the large point-mass 3-DOF CSPR
//! in PLT-00370 at Laval University
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::findCSPR3DOFpmat(std::vector<std::vector<double>> &pmat)
{
   int i,j;
   std::array<double,3> cible1={1.0941, -4.9074, 2.5542};
   std::array<double,3> delta1={-0.765, 0.112, 3.74};
   std::array<double,3> cible3={0.2098, 5.3409, 2.6236};
   std::array<double,3> delta2={0.43, 0.125, 3.615};

   std::array<double,3> p1=cible1+delta1;
   std::array<double,3> p2=cible3+delta2;
   std::array<double,3> p3={-5.9751, 0.1399, 6.1543};

   pmat.resize(3); for(i=0;i<3;i++) pmat[i].resize(3);

   std::array<int,3> ind={1,0,2};
   for(i=0;i<3;i++)
   {
      int it=ind[i];
      pmat[i][0]=-p1[it];
      pmat[i][1]=-p2[it];
      pmat[i][2]=-p3[it];
   }

   std::array<double,3> centroide;
   for(i=0;i<3;i++) centroide[i]=1/3.0*(pmat[i][0]+pmat[i][1]+pmat[i][2]);

   for(i=0;i<3;i++)
   {
      for(j=0;j<3;j++) pmat[i][j]-=centroide[i];
   }
   return 0;
}

//! DYNAMICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynSerial
//!
//! Dynamics model for a serial robot
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_dynSerial(std::vector<std::vector<double>> &a1,
                   std::vector<std::vector<double>> &a2,
                   std::vector<std::vector<double>> &a3,
                   std::vector<std::vector<double>> &a4,
                   const std::vector<std::vector<double>> &theta,
                   const std::vector<std::vector<double>> &thetaD,
                   const std::vector<std::vector<double>> &thetaD2)
{return(dynSerial(a1,a2,a3,a4,theta,thetaD,thetaD2));}
int Robot::dynSerial(std::vector<std::vector<double>> &a1,
                     std::vector<std::vector<double>> &a2,
                     std::vector<std::vector<double>> &a3,
                     std::vector<std::vector<double>> &a4,
                     const std::vector<std::vector<double>> &theta,
                     const std::vector<std::vector<double>> &thetaD,
                     const std::vector<std::vector<double>> &thetaD2)
{
   switch(_robotType)
   {
   case RR:
      dynRR(a1,a2,a3,a4,theta,thetaD,thetaD2);
      break;
   default:
      printf ("No dynamics model provided for serial robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynRR
//!
//! Dynamics model for an RR planar robot
//!  - links are approximated as point masses
//!  - tau = a1*sddot + a2*sdot^2 + a3*sdot + a4
//!  - thetaD and thetaD2 are partial derivatives wrt s
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::dynRR(
      std::vector<std::vector<double>> &a1,
      std::vector<std::vector<double>> &a2,
      std::vector<std::vector<double>> &a3,
      std::vector<std::vector<double>> &a4,
      const std::vector<std::vector<double>> &theta,
      const std::vector<std::vector<double>> &thetaD,
      const std::vector<std::vector<double>> &thetaD2)
{
   int i;
   int nPts=(int)theta[0].size();
   double A1=.4,A2=.6,m1=4,m2=8;
   double th1,th2,dth1,dth2,ddth1,ddth2,c1,c2,c12,ccFact;
   double A11,A12,A22;
   for(i=0;i<2;i++)
   {
      a1[i].resize(nPts);
      a2[i].resize(nPts);
      a3[i].resize(nPts);
      a4[i].resize(nPts);
   }

   for(i=0;i<nPts;i++)
   {
      th1  =_DEG2RAD*theta[0][i];
      th2  =_DEG2RAD*theta[1][i];
      dth1 =_DEG2RAD*thetaD[0][i];
      dth2 =_DEG2RAD*thetaD[1][i];
      ddth1=_DEG2RAD*thetaD2[0][i];
      ddth2=_DEG2RAD*thetaD2[1][i];

      c1 =std::cos(th1);
      c2 =std::cos(th2);
      c12=std::cos(th1+th2);

      A11=.25*m1*A1*A1 + m2*(A1*A1 + .25*A2*A2 + A1*A2*c2);
      A12=.5*m2*(.5*A2*A2 + A1*A2*c2);
      A22=.25*m2*A2*A2;

      a1[0][i]=A11*dth1 + A12*dth2;
      a1[1][i]=A12*dth1 + A22*dth2;

      ccFact=m2*A1*A2*sin(th2);
      a2[0][i]=A11*ddth1 + A12*ddth2-ccFact*dth2*(dth1+.5*dth2);
      a2[1][i]=A12*ddth1 + A22*ddth2-.5*ccFact*dth1*dth1;

      //friction terms: (friction is zero if left unspecified)
      a3[0][i]=10*dth1;
      a3[1][i]=10*dth2;

      a4[0][i]=.5*_g*(m1*A1*c1 + m2*(2.0*A1*c1 + A2*c12));
      a4[1][i]=.5*_g*m2*A2*c12;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynParallel
//!
//! Dynamics model for a parallel robot
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
///
int Robot::call_dynParallel(std::vector<std::vector<double>> &a1,
                   std::vector<std::vector<double>> &a2,
                   std::vector<std::vector<double>> &a3,
                   std::vector<std::vector<double>> &a4,
                   const std::vector<std::vector<double>> &cart,
                   const std::vector<std::vector<double>> &cartD,
                   const std::vector<std::vector<double>> &cartD2)
{
    return dynParallel(a1,a2,a3,a4,cart,cartD,cartD2);
}

int Robot::dynParallel(std::vector<std::vector<double>> &a1,
                     std::vector<std::vector<double>> &a2,
                     std::vector<std::vector<double>> &a3,
                     std::vector<std::vector<double>> &a4,
                     const std::vector<std::vector<double>> &cart,
                     const std::vector<std::vector<double>> &cartD,
                     const std::vector<std::vector<double>> &cartD2)
{
   (void)cart;

   switch(_robotType)
   {
   case CSPR3DOF: //CSPR3DOF
      dynCSPR3DOF(a1,a2,a3,a4,cartD,cartD2);
      break;
   default:
      printf ("No dynamics model provided for parallel robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynCSPR3DOF
//!
//! Dynamics model for the large 3-DOF point-mass CSPR in PLT-00370
//!  - A*tau = a1*sddot + a2*sdot^2 + a3*sdot + a4
//!  - cartD and cartD2 are partial derivatives wrt s
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::dynCSPR3DOF(
      std::vector<std::vector<double>> &a1,
      std::vector<std::vector<double>> &a2,
      std::vector<std::vector<double>> &a3,
      std::vector<std::vector<double>> &a4,
      const std::vector<std::vector<double>> &cartD,
      const std::vector<std::vector<double>> &cartD2)
{
   int i,j;
   int nCart=3;
   int nPts =(int)cartD[0].size();

   for(i=0;i<nCart;i++)
   {
      a1[i].resize(nPts);
      a2[i].resize(nPts);
      a3[i].assign(nPts,0.0);
      a4[i].assign(nPts,0.0);
   }

   for(i=0;i<nPts;i++)
   {
      for(j=0;j<nCart;j++)
      {
         a1[j][i]=-cartD[j][i];
         a2[j][i]=-cartD2[j][i];
      }
      a4[2][i]=_g;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief setA
//!
//! Find the A matrix for the dynamics equation of a parallel robot
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_setA(const std::vector<double> &theta,
         const std::vector<double> &cart,
         std::vector<std::vector<double>> &A)
{return(setA(theta,cart,A));}

int Robot::setA(const std::vector<double> &theta,
                const std::vector<double> &cart,
                std::vector<std::vector<double>> &A)
{
   switch(_robotType)
   {
   case CSPR3DOF:
      for(int i=0;i<3;i++)
      {
         for(int j=0;j<3;j++)
         {
            A[i][j]=(cart[i]-_pmat[i][j])/theta[j];
         }
      }
      break;

   default:
      printf ("isParallel=True and no code was provided to find the A matrix");
      printf("for robotType=%s.\n",_robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP

#include <boost/range.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/convert.hpp>

#include <boost/geometry/algorithms/detail/overlay/append_no_duplicates.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

/*!
    \brief Strategy: line clipping algorithm after Liang Barsky
    \ingroup overlay
    \details The Liang-Barsky line clipping algorithm clips a line with a clipping box.
    It is slightly adapted in the sense that it returns which points are clipped
    \tparam B input box type of clipping box
    \tparam P input/output point-type of segments to be clipped
    \note The algorithm is currently only implemented for 2D Cartesian points
    \note Though it is implemented in namespace strategy, and theoretically another
        strategy could be used, it is not (yet) updated to the general strategy concepts,
        and not (yet) splitted into a file in folder strategies
    \author Barend Gehrels, and the following recourses
    - A tutorial: http://www.skytopia.com/project/articles/compsci/clipping.html
    - a German applet (link broken): http://ls7-www.cs.uni-dortmund.de/students/projectgroups/acit/lineclip.shtml
*/
template<typename Box, typename Point>
class liang_barsky
{
private:
    typedef model::referring_segment<Point> segment_type;

    template <typename T>
    inline bool check_edge(T const& p, T const& q, T& t1, T& t2) const
    {
        bool visible = true;

        if(p < 0)
        {
            T const r = q / p;
            if (r > t2)
                visible = false;
            else if (r > t1)
                t1 = r;
        }
        else if(p > 0)
        {
            T const r = q / p;
            if (r < t1)
                visible = false;
            else if (r < t2)
                t2 = r;
        }
        else
        {
            if (q < 0)
                visible = false;
        }

        return visible;
    }

public:

    inline bool clip_segment(Box const& b, segment_type& s, bool& sp1_clipped, bool& sp2_clipped) const
    {
        typedef typename select_coordinate_type<Box, Point>::type coordinate_type;

        coordinate_type t1 = 0;
        coordinate_type t2 = 1;

        coordinate_type const dx = get<1, 0>(s) - get<0, 0>(s);
        coordinate_type const dy = get<1, 1>(s) - get<0, 1>(s);

        coordinate_type const p1 = -dx;
        coordinate_type const p2 = dx;
        coordinate_type const p3 = -dy;
        coordinate_type const p4 = dy;

        coordinate_type const q1 = get<0, 0>(s) - get<min_corner, 0>(b);
        coordinate_type const q2 = get<max_corner, 0>(b) - get<0, 0>(s);
        coordinate_type const q3 = get<0, 1>(s) - get<min_corner, 1>(b);
        coordinate_type const q4 = get<max_corner, 1>(b) - get<0, 1>(s);

        if (check_edge(p1, q1, t1, t2)      // left
            && check_edge(p2, q2, t1, t2)   // right
            && check_edge(p3, q3, t1, t2)   // bottom
            && check_edge(p4, q4, t1, t2))   // top
        {
            sp1_clipped = t1 > 0;
            sp2_clipped = t2 < 1;

            if (sp2_clipped)
            {
                set<1, 0>(s, get<0, 0>(s) + t2 * dx);
                set<1, 1>(s, get<0, 1>(s) + t2 * dy);
            }

            if(sp1_clipped)
            {
                set<0, 0>(s, get<0, 0>(s) + t1 * dx);
                set<0, 1>(s, get<0, 1>(s) + t1 * dy);
            }

            return true;
        }

        return false;
    }

    template<typename Linestring, typename OutputIterator>
    inline void apply(Linestring& line_out, OutputIterator out) const
    {
        if (!boost::empty(line_out))
        {
            *out = line_out;
            ++out;
            geometry::clear(line_out);
        }
    }
};

}} // namespace strategy::intersection

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

/*!
    \brief Clips a linestring with a box
    \details A linestring is intersected (clipped) by the specified box
    and the resulting linestring, or pieces of linestrings, are sent to the specified output operator.
    \tparam OutputLinestring type of the output linestrings
    \tparam OutputIterator an output iterator which outputs linestrings
    \tparam Linestring linestring-type, for example a vector of points, matching the output-iterator type,
         the points should also match the input-iterator type
    \tparam Box box type
    \tparam Strategy strategy, a clipping strategy which should implement the methods "clip_segment" and "apply"
*/
template
<
    typename OutputLinestring,
    typename OutputIterator,
    typename Range,
    typename Box,
    typename Strategy
>
OutputIterator clip_range_with_box(Box const& b, Range const& range,
            OutputIterator out, Strategy const& strategy)
{
    if (boost::begin(range) == boost::end(range))
    {
        return out;
    }

    typedef typename point_type<OutputLinestring>::type point_type;

    OutputLinestring line_out;

    typedef typename boost::range_iterator<Range const>::type iterator_type;
    iterator_type vertex = boost::begin(range);
    for(iterator_type previous = vertex++;
            vertex != boost::end(range);
            ++previous, ++vertex)
    {
        point_type p1, p2;
        geometry::convert(*previous, p1);
        geometry::convert(*vertex, p2);

        // Clip the segment. Five situations:
        // 1. Segment is invisible, finish line if any (shouldn't occur)
        // 2. Segment is completely visible. Add (p1)-p2 to line
        // 3. Point 1 is invisible (clipped), point 2 is visible. Start new line from p1-p2...
        // 4. Point 1 is visible, point 2 is invisible (clipped). End the line with ...p2
        // 5. Point 1 and point 2 are both invisible (clipped). Start/finish an independant line p1-p2
        //
        // This results in:
        // a. if p1 is clipped, start new line
        // b. if segment is partly or completely visible, add the segment
        // c. if p2 is clipped, end the line

        bool c1 = false;
        bool c2 = false;
        model::referring_segment<point_type> s(p1, p2);

        if (!strategy.clip_segment(b, s, c1, c2))
        {
            strategy.apply(line_out, out);
        }
        else
        {
            // a. If necessary, finish the line and add a start a new one
            if (c1)
            {
                strategy.apply(line_out, out);
            }

            // b. Add p1 only if it is the first point, then add p2
            if (boost::empty(line_out))
            {
                detail::overlay::append_no_duplicates(line_out, p1, true);
            }
            detail::overlay::append_no_duplicates(line_out, p2);

            // c. If c2 is clipped, finish the line
            if (c2)
            {
                strategy.apply(line_out, out);
            }
        }

    }

    // Add last part
    strategy.apply(line_out, out);
    return out;
}

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP