//---------------------------------------------------------------------------//
//!
//! \file   Utility_MaxwellFissionDistribution_def.hpp
//! \author Aaron Tumulak
//! \brief  Maxwell Fission distribution class definition. Modified by Alex
//!         Robinson to accommodate units.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP
#define UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP

// Boost Includes
#include <boost/units/cmath.hpp>

// FRENSIE Includes
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_ArrayString.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ExceptionTestMacros.hpp"
#include "Utility_ExceptionCatchMacros.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

// Constructor
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy,
  const double constant_multiplier )
  : d_incident_energy( incident_energy ),
    d_nuclear_temperature( nuclear_temperature ),
    d_restriction_energy( restriction_energy ),
    d_multiplier( DMQT::initializeQuantity( constant_multiplier ) ),
    d_norm_constant()
{
  // Make sure values are valid
  testPrecondition( !IQT::isnaninf( incident_energy ) );
  testPrecondition( !IQT::isnaninf( nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( restriction_energy ) );
  testPrecondition( !QT::isnaninf( constant_multiplier ) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy > IQT::zero() );
  testPrecondition( nuclear_temperature > IQT::zero() );
  // Make sure that the constant multiplier is positive
  testPrecondition( constant_multiplier > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Constructor
/*! \details This constructor will explicitly cast the input quantities to
 * the distribution quantity (which includes any unit-conversion). The
 * dimension type must match and there must be a unit-conversion defined using
 * the boost methodology.
 */
template<typename IndependentUnit, typename DependentUnit>
template<typename InputIndepQuantityA,
	 typename InputIndepQuantityB,
	 typename InputIndepQuantityC>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
				const InputIndepQuantityA incident_energy,
				const InputIndepQuantityB nuclear_temperature,
				const InputIndepQuantityC restriction_energy,
				const double constant_multiplier )
  : d_incident_energy( incident_energy ),
    d_nuclear_temperature( nuclear_temperature ),
    d_restriction_energy( restriction_energy ),
    d_multiplier( DMQT::initializeQuantity( constant_multiplier ) ),
    d_norm_constant()
{
  // Make sure values are valid
  testPrecondition( !QuantityTraits<InputIndepQuantityA>::isnaninf( incident_energy ) );
  testPrecondition( !QuantityTraits<InputIndepQuantityB>::isnaninf( nuclear_temperature ) );
  testPrecondition( !QuantityTraits<InputIndepQuantityC>::isnaninf( restriction_energy ) );
  testPrecondition( !QT::isnaninf( constant_multiplier ) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy >
		    QuantityTraits<InputIndepQuantityA>::zero() );
  testPrecondition( nuclear_temperature >
		    QuantityTraits<InputIndepQuantityB>::zero() );
  // Make sure that the constant multiplier is positive
  testPrecondition( constant_multiplier > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Copy constructor
/*! \details Just like boost::units::quantity objects, the unit-aware
 * distribution can be explicitly cast to a distribution with compatible
 * units. If the units are not compatible, this function will not compile. Note
 * that this allows distributions to be scaled safely (unit conversions
 * are completely taken care of by boost::units)!
 */
template<typename IndependentUnit, typename DependentUnit>
template<typename InputIndepUnit, typename InputDepUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
  const UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>& dist_instance )
  : d_incident_energy( dist_instance.d_incident_energy ),
    d_nuclear_temperature( dist_instance.d_nuclear_temperature ),
    d_restriction_energy( dist_instance.d_restriction_energy ),
    d_multiplier(),
    d_norm_constant()
{
  // Make sure the multipliers are valid
  remember( typedef QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::IndepQuantity> InputIQT );
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_incident_energy ) );
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_nuclear_temperature));
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_restriction_energy) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( dist_instance.d_incident_energy > InputIQT::zero() );
  testPrecondition( dist_instance.d_nuclear_temperature > InputIQT::zero() );

  // Calculate the scaled multiplier (for complex units, boost::units often has
  // problems doing the conversion so we will do it manually)
  d_multiplier = getRawQuantity( dist_instance.d_multiplier )*DepQuantity( QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::DepQuantity>::one() )/Utility::sqrt( IndepQuantity( QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::IndepQuantity>::one() ) );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Copy constructor
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_dist_instance, int )
  : d_incident_energy( IQT::initializeQuantity( unitless_dist_instance.d_incident_energy ) ),
    d_nuclear_temperature( IQT::initializeQuantity( unitless_dist_instance.d_nuclear_temperature ) ),
    d_restriction_energy( IQT::initializeQuantity( unitless_dist_instance.d_restriction_energy ) ),
    d_multiplier( DMQT::initializeQuantity( unitless_dist_instance.d_multiplier ) ),
    d_norm_constant()
{
  // Make sure the multipliers are valid
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_incident_energy ) );
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_nuclear_temperature) );
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_restriction_energy) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( unitless_dist_instance.d_incident_energy > 0.0 );
  testPrecondition( unitless_dist_instance.d_nuclear_temperature > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Construct distribution from a unitless dist. (potentially dangerous)
/*! \details Constructing a unit-aware distribution from a unitless
 * distribution is potentially dangerous. By forcing users to construct objects
 * using this method instead of a standard constructor we are trying to make
 * sure users are aware of the danger. This is designed to mimic the interface
 * of the boost::units::quantity, which also has to deal with this issue.
 */
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::fromUnitlessDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_distribution )
{
  return ThisType( unitless_distribution, 0 );
}

// Assignment operator
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>&
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::operator=(
    const UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>& dist_instance )
{
  // Make sure the distribution is valid
  testPrecondition( !IQT::isnaninf( dist_instance.d_incident_energy ) );
  testPrecondition( !IQT::isnaninf( dist_instance.d_nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( dist_instance.d_restriction_energy ) );
  testPrecondition( !DMQT::isnaninf( dist_instance.d_multiplier ) );
  testPrecondition( dist_instance.d_incident_energy > IQT::zero() );
  testPrecondition( dist_instance.d_nuclear_temperature > IQT::zero() );

  if( this != &dist_instance )
  {
    d_incident_energy = dist_instance.d_incident_energy;
    d_nuclear_temperature = dist_instance.d_nuclear_temperature;
    d_restriction_energy = dist_instance.d_restriction_energy;
    d_multiplier = dist_instance.d_multiplier;
    d_norm_constant = dist_instance.d_norm_constant;
  }

  return *this;
}

// Evaluate the distribution
/*! \details This is simply the unnormalized distribution. An implicit
 * conversion factor (1.0) is used to convert to the correct units.
 */
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::DepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::evaluate(
 const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity indep_var_value ) const
{
  if( indep_var_value < IQT::zero() )
    return DQT::zero();
  else
  {
    return d_multiplier*Utility::sqrt( indep_var_value )*
      exp( -indep_var_value / d_nuclear_temperature );
  }
}

// Evaluate the PDF
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::evaluatePDF(
const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity indep_var_value ) const
{
  return this->evaluate( indep_var_value )*d_norm_constant;
}

// Return a random sample from the distribution
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sample() const
{
  return ThisType::sample( d_incident_energy,
			   d_nuclear_temperature,
			   d_restriction_energy );
}

// Return a random sample from the corresponding CDF and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
inline typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sample(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy )
{
  unsigned trials = 0;

  return ThisType::sampleAndRecordTrials( incident_energy,
					  nuclear_temperature,
					  restriction_energy,
					  trials );
}

// Return a random sample and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sampleAndRecordTrials( unsigned& trials ) const
{
  return ThisType::sampleAndRecordTrials( d_incident_energy,
					  d_nuclear_temperature,
					  d_restriction_energy,
					  trials );
}

// Return a random sample from the corresponding CDF and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
inline typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sampleAndRecordTrials(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy,
  unsigned& trials )
{
  // Make sure values are valid
  testPrecondition( !IQT::isnaninf( incident_energy ) );
  testPrecondition( !IQT::isnaninf( nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( restriction_energy ) );

  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy > IQT::zero() );
  testPrecondition( nuclear_temperature > IQT::zero() );

  double random_number_1, random_number_2, random_number_3;
  double term_1, term_2, arg;
  IndepQuantity sample;

  // Use the method outlined in OpenMC documentation under "5.7.2.4. ACE Law 7 - UnitAwareMaxwell Fission Spectrum"
  while( true )
  {
    // Increment the trial counter
    ++trials;

    random_number_1 = RandomNumberGenerator::getRandomNumber<double>();
    random_number_2 = RandomNumberGenerator::getRandomNumber<double>();
    random_number_3 = RandomNumberGenerator::getRandomNumber<double>();

    term_1 = log(random_number_1);

    arg = cos( PhysicalConstants::pi * random_number_3 * 0.5 );

    term_2 = log(random_number_2)*arg*arg;

    sample = -nuclear_temperature * ( term_1 + term_2 );

    if( sample <= (incident_energy - restriction_energy) )
      break;
  }

  return sample;
}

// Calculate the normalization constant of the distribution
/*
 * As given by ENDF Law 7
 * c^(-1) = T^(3/2)*[(sqrt(pi)/2)*erf(sqrt((E-U)/T)) - sqrt((E-U)/T)*exp(-(E-U)/T)]
 */
template<typename IndependentUnit, typename DependentUnit>
void
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::calculateNormalizationConstant()
{
  double argument = (d_incident_energy - d_restriction_energy) / d_nuclear_temperature;

  d_norm_constant =  rpow<3,-2>(d_nuclear_temperature)/
    (d_multiplier*(sqrt( PhysicalConstants::pi ) * 0.5 * erf( sqrt(argument) )-
		   sqrt(argument) * exp(-argument) ) );
}

// Return the upper bound of the distribution independent variable
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getUpperBoundOfIndepVar() const
{
  return (d_incident_energy - d_restriction_energy);
}

// Return the lower bound of the distribution independent variable
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getLowerBoundOfIndepVar() const
{
  return IQT::zero();
}

// Return the distribution type
template<typename IndependentUnit, typename DependentUnit>
OneDDistributionType UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getDistributionType() const
{
  return ThisType::distribution_type;
}

// Test if the distribution is continuous
template<typename IndependentUnit, typename DependentUnit>
bool UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::isContinuous() const
{
  return true;
}

// Method for placing the object in an output stream
template<typename IndependentUnit, typename DependentUnit>
void UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::toStream( std::ostream& os ) const
{
  os << "{" << getRawQuantity( d_incident_energy )
     << "," << getRawQuantity( d_nuclear_temperature )
     << "," << getRawQuantity( d_restriction_energy );

  // Only print the multiplier when a scaling has been done
  if( d_multiplier != DMQT::one() )
    os << "," << getRawQuantity( d_multiplier ) << "}";
  else
    os << "}";
}

// Method for initializing the object from an input stream
template<typename IndependentUnit, typename DependentUnit>
void UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::fromStream( std::istream& is )
{
  // Read in the distribution representation
  std::string dist_rep;
  std::getline( is, dist_rep, '}' );
  dist_rep += '}';

  Teuchos::Array<std::string> distribution;
  try{
    distribution = Teuchos::fromStringToArray<std::string>( dist_rep );
  }
  catch( Teuchos::InvalidArrayStringRepresentation& error )
  {
    std::string message( "Error: the Maxwell Fission distribution cannot be "
                        "constructed because the representation is not valid "
                        "(see details below)!\n" );
    message += error.what();

    throw InvalidDistributionStringRepresentation( message );
  }

  TEST_FOR_EXCEPTION( distribution.size() > 4,
                     InvalidDistributionStringRepresentation,
                     "Error: the Maxwell Fission distribution cannot "
                     "be constructed because the representation is "
                     "not valid"
                     "(only 4 values or fewer  may be specified)!" );

  // Set the incient neutron energy
  if( distribution.size() > 0 )
  {
    TEST_FOR_EXCEPTION( distribution[0].find_first_not_of( " 0123456789.e" ) <
			distribution[0].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< distribution[0] );
    {
      double incident_energy;

      std::istringstream iss( distribution[0] );
      Teuchos::extractDataFromISS( iss, incident_energy );

      setQuantity( d_incident_energy, incident_energy );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_incident_energy ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< d_incident_energy );

    TEST_FOR_EXCEPTION( d_incident_energy < IQT::zero(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< d_incident_energy );
  }

  // Set the nuclear temperature
  if( distribution.size() > 1 )
  {
    TEST_FOR_EXCEPTION( distribution[1].find_first_not_of( " 0123456789.e" ) <
			distribution[1].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< distribution[1] );
    {
      double nuclear_temperature;

      std::istringstream iss( distribution[1] );
      Teuchos::extractDataFromISS( iss, nuclear_temperature );

      setQuantity( d_nuclear_temperature, nuclear_temperature );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_nuclear_temperature ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< d_nuclear_temperature );

    TEST_FOR_EXCEPTION( d_nuclear_temperature <= IQT::zero(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< d_nuclear_temperature );
  }

  // Set the restriction energy
  if( distribution.size() > 2 )
  {
    TEST_FOR_EXCEPTION( distribution[2].find_first_not_of( " 0123456789.e" ) <
			distribution[2].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"construcqted because of an invalid restriction energy "
			<< distribution[2] );
    {
      double restriction_energy;

      std::istringstream iss( distribution[2] );
      Teuchos::extractDataFromISS( iss, restriction_energy );

      setQuantity( d_restriction_energy, restriction_energy );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_restriction_energy ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid restriction energy "
			<< d_restriction_energy );
  }

  // Set the multiplier
  if( distribution.size() > 3 )
  {
    TEST_FOR_EXCEPTION( distribution[3].find_first_not_of( " 0123456789.e" ) <
			distribution[3].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"construcqted because of an invalid multiplier "
			<< distribution[3] );

    {
      double multiplier;

      std::istringstream iss( distribution[3] );
      Teuchos::extractDataFromISS( iss, multiplier );

      setQuantity( d_multiplier, multiplier );
    }

    TEST_FOR_EXCEPTION( DMQT::isnaninf( d_multiplier ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid multiplier "
			<< getRawQuantity( d_multiplier ) );
  }

  // Calculate the normalization constant
  this->calculateNormalizationConstant();
}

// Method for testing if two objects are equivalent
template<typename IndependentUnit, typename DependentUnit>
bool UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::isEqual( const UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>& other ) const
{
  return d_incident_energy == other.d_incident_energy &&
    d_nuclear_temperature == other.d_nuclear_temperature &&
    d_restriction_energy == other.d_restriction_energy &&
    d_multiplier == other.d_multiplier;
}

} // end Utility namespace

#endif // end UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP

//---------------------------------------------------------------------------//
// end Utility_MaxwellFissionDistribution_def.hpp
//---------------------------------------------------------------------------//


/*======================================================================
Copyright 2019 Google LLC

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    https://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "Servo_Timer.h"
#include "Home.h"
#include "Caster.h"
#include "Vehicle.h"

#define N_2X2_MATRIX  \
    double N_00 = -cstr[i]->Ns; \
    double N_10 = -cstr[i]->Nt; \
    double N_11 =  cstr[i]->Nt * cstr[i]->Nw;

#define Ni_2X2_MATRIX  \
    double Ni_00 = -1.0/cstr[i]->Ns; \
    double Ni_10 = -1.0/(cstr[i]->Ns * cstr[i]->Nw); \
    double Ni_11 =  1.0/(cstr[i]->Nt * cstr[i]->Nw);

using namespace std;
using namespace Eigen;

// SINGLETON
static Vehicle* gVehicle = NULL;

bool
Vehicle::IsValid() const
{ return gVehicle ? true : false;
}

Vehicle*
Vehicle::HandleGet()
{
  if( gVehicle == NULL )
  {
    if( !( gVehicle = new Vehicle() ) ||
        !gVehicle->IsValid() )
    {
      return NULL;
    }
  }
  return gVehicle;
}

Vehicle::Vehicle()
  :Lambda(M3Z),Mu(V3Z),
   X_veh(0),Y_veh(0),M_veh(0),I_veh(0),num_casters(0),
   L_veh(M3Z),Mu_veh(V3Z),M_gross(0.0),I_gross(0.0),X_gross(0.0),Y_gross(0.0)
{
  hwi_ = HWInterface::HandleGet();

  return;
}

Vehicle::~Vehicle()
{
  // SHOULD SET DAC's TO ZERO HERE,
  // BUT, FOR NOW, LET WATCHDOG DO IT FOR US

  gVehicle = NULL; // SINGLETON

  for( int i=0; i<num_casters; i++ )
  {
    delete cstr[i];
  }
  num_casters = 0;
}

void
Vehicle::Home()
{
  ::Home( cstr[0]->enc_offset,
          cstr[1]->enc_offset,
          cstr[2]->enc_offset,
          cstr[3]->enc_offset);
}

void
Vehicle::Add_Caster( double _Kx, double _Ky, double _ang,
                     double  _b, double  _r,
                     double  _f, double _Mf, double _If,
                     double _Ih, double _Ii, double _Is, double _It, double _Ij,
                     double _Ns, double _Nt, double _Nw,
                     double _px, double _py, double _Mp, double _Ip)
{
  cstr[num_casters] = new Caster( _Kx, _Ky, _ang,
                                   _b,  _r,
                                  _f, _Mf, _If,
                                  _Ih, _Ii, _Is, _It, _Ij,
                                  _Ns, _Nt, _Nw,
                                  _px, _py, _Mp, _Ip);
  num_casters++;

  Add_Gross( _Kx, _Ky, _Mf+_Mp, _Ih+_Ii+_Is+_It+_Ij); // approximate only
}

void
Vehicle::JointRad(Vector8d &jRad)
{
  jRad.setZero();
  return;
  ENC_REGISTER e[8];
  hwi_->EncReadAll(e); // LATCHed by HWInterface

  for( int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;

    int j = 2*i;

    jRad[ j ] = -(Ni_00*e[j]               )*ENC_CNT2RAD;
    jRad[j+1] = -(Ni_10*e[j] + Ni_11*e[j+1])*ENC_CNT2RAD;
// NOTE: MINUS SIGN DUE TO DIRECTION OF ENCODERS
// NOTE: ENCODERS GIVE OPPOSITE SIGN OF MOTOR ROTATION

    cstr[i]->a = jRad[j];
    cstr[i]->c = cos(jRad[j]);
    cstr[i]->s = sin(jRad[j]);
  }
}

void
Vehicle::MotorTq(Vector8d const &mtq)
/*** Each motor will produce the specified torque ***/
/*** tq should be a vector of 8 entries in Nm ***/
{
  long counts;

  for( unsigned char j=0; j<2*num_casters; j++ )
  {
    counts =(long)( mtq[j] * (1.0/Nm_PER_AMP) * (1.0/AMPS_PER_VOLT) * COUNTS_PER_VOLT );
    hwi_->RawDAC(j,counts); // RawDAC will check MAX/MIN values
  }
}

void
Vehicle::JointTq(Vector8d const &jtq)
/*** tq should be a vector of 8 entries in Nm ***/
{
  static Vector8d mtq;

  for(int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;

    int j = 2*i;
    mtq[ j ] = Ni_00 * jtq[ j ] + Ni_10 * jtq[j+1];
    mtq[j+1] =                    Ni_11 * jtq[j+1];
  }
  MotorTq(mtq);
}

// FOR DATA COLLECTION CONVENIENCE ONLY
void
Vehicle::JtTq2MotAmp(Vector8d const &jtq, Vector8d &motAmp)
{
  static Vector8d mtq;
  for(int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;
    int j = 2*i;
    mtq[ j ] = Ni_00 * jtq[ j ] + Ni_10 * jtq[j+1];
    mtq[j+1] =                    Ni_11 * jtq[j+1];
  }
  motAmp = mtq * (1.0/Nm_PER_AMP);
}

// CONSTRAINT MATRIX

void
Vehicle::Fill_C(Matrix3d &C)
{
  for(int i=0; i<num_casters ; i++ )
  {
    double bi = cstr[i]->bi;
    double ri = cstr[i]->ri;
    double c  = cstr[i]->c;
    double s  = cstr[i]->s;
    double Kx = cstr[i]->Kx;
    double Ky = cstr[i]->Ky;

    int j = 2*i;

    C( j ,0) =   bi*s;
    C( j ,1) =  -bi*c;
    C( j ,2) =  -bi*(Kx*c+Ky*s) - 1.0;

    C(j+1,0) =   ri*c;
    C(j+1,1) =   ri*s;
    C(j+1,2) =   ri*(Kx*s-Ky*c);
  }
}

// JACOBIAN VIA C.P.'s to minimize slip in odometry
// USE Jt_cp to minimize contact forces

void
Vehicle::Fill_Jcp(Matrix3x8d &J)
{
  int i;
  static Matrix8x3d  Cp;
  static Matrix3x8d  CptCli;
  static Matrix3d    J;

  for( i=0; i<num_casters; i++ )
  {   // Cli IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
      // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS
    double b = cstr[i]->b;
    double r = cstr[i]->r;
    double c = cstr[i]->c;
    double s = cstr[i]->s;
    double Kx= cstr[i]->Kx;
    double Ky= cstr[i]->Ky;

    int j = 2*i;

    // Note: sign of p-dot is: wheel as viewed from ground

    CptCli(0, j ) =  b*s;
    CptCli(0,j+1) =  r*c;
    CptCli(1, j ) = -b*c;
    CptCli(1,j+1) =  r*s;
    CptCli(2, j ) = -b*((Kx*c+Ky*s)+b);
    CptCli(2,j+1) =  r* (Kx*s-Ky*c);

    Cp( j ,0) =  1.0;
    Cp( j ,1) =  0.0;
    Cp( j ,2) = -b*s - Ky;

    Cp(j+1,0) =  0.0;
    Cp(j+1,1) =  1.0;
    Cp(j+1,2) =  b*c + Kx;
  }

  // J = inv(Cpt*Cp)*Cpt * inv(Cl)
  // J = inv(Cpt*Cp)*CptCli
  J = (Cp.transpose()*Cp).llt().solve(CptCli);
}

// JACOBIAN to Minimize Motor Torques

void
Vehicle::Fill_Jt_gamma(Matrix8x3d &Jt)
{
  int i;
  static Matrix8x3d C    = Matrix8x3d::Zero();
  static Matrix8x3d NC   = Matrix8x3d::Zero();
  static Matrix3x8d NCtN = Matrix3x8d::Zero();

  Fill_C( C );

  for( i=0; i<num_casters ; i++ )
  {   // N IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
      // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS
    N_2X2_MATRIX;

    int j = 2*i;

    NC( j ,0)   = N_00* C(j,0);
    NC( j ,1)   = N_00* C(j,1);
    NC( j ,2)   = N_00* C(j,2);
    NC(j+1,0)   = N_10* C(j,0) + N_11* C(j+1,0);
    NC(j+1,1)   = N_10* C(j,1) + N_11* C(j+1,1);
    NC(j+1,2)   = N_10* C(j,2) + N_11* C(j+1,2);

    NCtN(0, j ) = NC(j,0)*N_00 + NC(j+1,0)*N_10;
    NCtN(1, j ) = NC(j,1)*N_00 + NC(j+1,1)*N_10;
    NCtN(2, j ) = NC(j,2)*N_00 + NC(j+1,2)*N_10;
    NCtN(0,j+1) =                NC(j+1,0)*N_11;
    NCtN(1,j+1) =                NC(j+1,1)*N_11;
    NCtN(2,j+1) =                NC(j+1,2)*N_11;
  }

  // wdot = Cn xdot ; wdot = N qdot ; qdot = C xdot
  // J_gamma = Cn# N   ; Cn = N C
  // J_gamma  = [(NC)t * NC]i * (NC)t N
  // Jt_gamma = J_gamma.transpose()

  Jt = ( (NC.transpose()*NC).llt().solve(NCtN) ).transpose()

}

// JACOBIAN FOR VIRTUAL LINKAGE AT CONTACT POINTS

void
Vehicle::Fill_E_p(MatrixXd &E)
{
  int i,j;
  double ex, ey, mag_inv;
  double pix, pjx, piy, pjy;

  // DESCRIBE CONNECTIONS OF VIRTUAL TRUSS

//   int pi[NUM_TRUSS_LINKS]={0,1,2,3,1};
//   int pj[NUM_TRUSS_LINKS]={1,2,3,0,3};
  int pi[NUM_TRUSS_LINKS]={0,3,1,2,1,2};
  int pj[NUM_TRUSS_LINKS]={3,1,2,0,0,3};

  for( int k=0; k<NUM_TRUSS_LINKS; k++ )
  {
    i = pi[k];
    j = pj[k];

    pix = cstr[i]->Kx + cstr[i]->b*cstr[i]->c;
    pjx = cstr[j]->Kx + cstr[j]->b*cstr[j]->c;

    piy = cstr[i]->Ky + cstr[i]->b*cstr[i]->s;
    pjy = cstr[j]->Ky + cstr[j]->b*cstr[j]->s;

    ex = pix - pjx;
    ey = piy - pjy;
    mag_inv = 1.0/hypot(ex,ey);
    E(k, 2*i  ) = ex * mag_inv;
    E(k, 2*i+1) = ey * mag_inv;
    E(k, 2*j  ) =  -E(k, 2*i  );
    E(k, 2*j+1) =  -E(k, 2*i+1);
  }

}

void
Vehicle::Fill_E_q(MatrixXd &E)
{
  int j = 2*num_casters;

  MatrixXd Ep(NUM_TRUSS_LINKS,j);
  MatrixXd Cli(j,j);
  // q_dot = Cl * p_dot ==> p_dot = Cli * q_dot
  // eps_dot = Ep * p_dot
  // eps_dot = Ep * Cli * q_dot ==> E_q = Ep * Cli

  Fill_E_p( Ep );
  for(int i=0; i<num_casters; i++ )
  {
    j = 2*i;
    Cli( j , j ) =  cstr[i]->b*cstr[i]->s;
    Cli( j ,j+1) =  cstr[i]->r*cstr[i]->c;

    Cli(j+1, j ) = -cstr[i]->b*cstr[i]->c;
    Cli(j+1,j+1) =  cstr[i]->r*cstr[i]->s;
  }

  E = Ep * Cli;

}

void
Vehicle::Add_Solid(double _x, double _y,
                   double _M, double _I)
{

  // COMPUTE NEW veh QUANTITIES w/ ARGS AND PREVIOUS M & I
  X_veh  = (M_veh * X_veh + _M * _x) / (M_veh + _M);
  Y_veh  = (M_veh * Y_veh + _M * _y) / (M_veh + _M);
  M_veh += _M;  // TOTAL veh MASS
  I_veh += _I +_M*(pow(_x,2) + pow(_y,2));  // new I at (0,0) local coords

  // COMPUTE NEW VEHICLE MASS MATRIX L_veh
  // (ie Lambda_Vehicle for fixed parts not including Casters)
  L_veh(0,0) =  M_veh;
  L_veh(0,1) =  0.0;
  L_veh(0,2) = -M_veh * Y_veh;

  L_veh(1,0) =  0.0;
  L_veh(1,1) =  M_veh;
  L_veh(1,2) =  M_veh * X_veh;

  L_veh(2,0) =  L_veh.at(0,2);
  L_veh(2,1) =  L_veh.at(1,2);
  L_veh(2,2) =  I_veh;

  Add_Gross(_x,_y,_M,_I);
}

void
Vehicle::Add_Gross(double _x, double _y,
                   double _M, double _I)
{
  // COMPUTE NEW ESTIMATES FOR GROSS MASS AND INERTIA (includes Casters)
  X_gross  = (M_gross * X_gross + _M * _x) / (M_gross + _M);
  Y_gross  = (M_gross * Y_gross + _M * _y) / (M_gross + _M);
  M_gross += _M;
  I_gross += _I +_M*(pow(_x,2) + pow(_y,2));  // new I at (0,0) local coords
}

inline void
Vehicle::Fill_Mu_veh( double w )
{
  register double w2 = pow(w,2);

  // (ie Mu_Vehicle for fixed parts not including Casters)
  Mu_veh(0)  = -M_veh * X_veh * w2;
  Mu_veh(1)  = -M_veh * Y_veh * w2;
  Mu_veh(2)  =  0.0;
}

void
Vehicle::Dyn(Vector8d qd, double w)
{
  // INITIALIZE TO VEHICLE PROPERTIES
  Lambda = L_veh;
  Fill_Mu_veh( w );
  Mu = Mu_veh;

  // ADD DYNAMICS FROM THE CASTERS
  for(int i=0; i<num_casters; i++)
  { int j = 2*i;
    cstr[i]->Fill_LM(qd[j],qd[j+1],w);
    Lambda += cstr[i]->Lambda;
    Mu += cstr[i]->Mu;
  }
}

Vector3d
Vehicle::Fill_tqS(Vector8d const &qd, Vector8d const &tq,
                  Vector8d &tqS)
{
  // Experimental function to compute operational force to compensate for
  // rotational friction of the wheel contact patches.

  int i,j;
  double m,b,frd,atq,tqX;
//   double bM[4] = {0.35, 0.57, 0.35, 0.57};
//   double mM[4] = {0.21, 0.17, 0.21, 0.17};
//   double tqM   = 0.35;
  double rdM   = 25.0;
  double bM[4] = {0.20, 0.50, 0.20, 0.50};
  double mM[4] = {0.10, 0.10, 0.10, 0.10};
  double tqM   = 0.60;

  double tqA = 0.0;
  static Vector3d fS = Vector3d::Zero();

  for(i=0; i<num_casters; i++)
  { j = 2*i;
    frd = 1.0 - fabs(qd[j+1])/rdM;
    b = bM[i]*frd;
    m = mM[i]*frd;
    atq = fabs(tq[j]);
    tqX = atq<tqM ? atq/tqM : 1.0;
    tqS[j] = (m*qd[j] + (qd[j]>0?b:-b)) * tqX;
    tqA += tqS[j];
  }

  fS[2] = tqA;
  return fS;
}


//*****************************************************************************
//*****************************************************************************
// Class: CLaricobiusOsakensis
//
//
// Description: the CLaricobiusOsakensis represents a group of LNF insect. scale by m_ScaleFactor
//*****************************************************************************
// 07/07/2021   Rémi Saint-Amant    Creation
//*****************************************************************************

#include "LaricobiusOsakensisEquations.h"
#include "LaricobiusOsakensis.h"
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/logistic.hpp>

using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LOF;

namespace WBSF
{

	//*********************************************************************************
	//CLaricobiusOsakensis class

	//*****************************************************************************
	// Object creator
	//
	// Input: See CIndividual creator
	//
	// Note: m_RDR (relative Development Rate)  member is init with random values.
	//*****************************************************************************
	CLaricobiusOsakensis::CLaricobiusOsakensis(CHost* pHost, CTRef creationDate, double age, TSex sex, bool bFertil, size_t generation, double scaleFactor) :
		CIndividual(pHost, creationDate, age, sex, bFertil, generation, scaleFactor)
	{
		//reset creation date
		int year = creationDate.GetYear();
		m_creationDate = GetCreationDate(year);
		m_adult_emergence = GetAdultEmergence(year + 1);

		for (size_t s = 0; s < NB_STAGES; s++)
			m_RDR[s] = Equations().GetRelativeDevRate(s);

		m_t = 0;
		m_Fi = (m_sex == FEMALE) ? Equations().GetFecondity() : 0;
		m_bDeadByAttrition = false;
	}

	CTRef CLaricobiusOsakensis::GetCreationDate(int year)const
	{
		CTRef creationDate;
		double creationCDD = Equations().GetCreationCDD();

		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		//CTRef begin = CTRef(year, JANUARY, DAY_01);
		//CTRef end = CTRef(year, JUNE, DAY_30);

		CTRef begin = CTRef(year, DECEMBER, DAY_01);
		CTRef end = CTRef(year+1, AUGUST, DAY_31);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !creationDate.IsInit(); TRef++)
		{
			const CWeatherDay& wDay = weather_station.GetDay(TRef);
			double DD = GetStand()->m_DD.GetDD(wDay);
			CDD += DD;

			if (CDD  >= creationCDD)
			{
				creationDate = wDay.GetTRef();
			}
		}
		if (!creationDate.IsInit())
			creationDate = end;//for text only

		ASSERT(creationDate.IsInit());

		return creationDate;
	}

	CTRef CLaricobiusOsakensis::GetAdultEmergence(int year)const
	{
		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTPeriod p = weather_station.GetEntireTPeriod(CTM::DAILY);

		CTRef adult_emergence;
		double adult_emerging_CDD = Equations().GetAdultEmergingCDD();

		CTRef begin = GetStand()->m_diapause_end;
		CTRef end = p.End();
		if (weather_station[year].HaveNext())
			end = min(p.End(), CTRef(begin.GetYear() + 1, JANUARY, DAY_31));

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !adult_emergence.IsInit(); TRef++)
		{
			const CWeatherDay& wday = weather_station.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			double DD = max(0.0, T - Equations().m_EAS[Τᴴ]);
			CDD += DD;
			if (CDD >= adult_emerging_CDD)
			{
				adult_emergence = wday.GetTRef();
			}
		}

		//if (!adult_emergence.IsInit())
			//adult_emergence = CTRef(year, DECEMBER, DAY_31);//pour test seulement a revoir...

		//ASSERT(adult_emergence.IsInit());
		return adult_emergence;
	}

	CLaricobiusOsakensis& CLaricobiusOsakensis::operator=(const CLaricobiusOsakensis& in)
	{
		if (&in != this)
		{
			CIndividual::operator=(in);

			//new relative developement rate
			for (size_t s = 0; s < NB_STAGES; s++)
				m_RDR[s] = Equations().GetRelativeDevRate(s);

			m_dropToGroundDate = in.m_dropToGroundDate;
			m_adult_emergence = in.m_adult_emergence;
			m_reachDate = in.m_reachDate;
			m_t = in.m_t;
			m_Fi = in.m_Fi;
			m_bDeadByAttrition = in.m_bDeadByAttrition;
		}

		return *this;
	}

	//destructor
	CLaricobiusOsakensis::~CLaricobiusOsakensis(void)
	{}

	void CLaricobiusOsakensis::OnNewDay(const CWeatherDay& weather)
	{
		CIndividual::OnNewDay(weather);

		if (weather.GetTRef() == m_creationDate)
		{
			m_age = EGG;
		}
	}

	//*****************************************************************************
	// Develops all stages for one time step
	// Input:	weather: weather of the hour
	//			timeStep: timeStep [h]
	//*****************************************************************************
	void CLaricobiusOsakensis::Live(const CHourlyData& weather, size_t timeStep)
	{
		assert(IsAlive());
		assert(m_status == HEALTHY);

		CLNFHost* pHost = GetHost();
		CLNFStand* pStand = GetStand();

		double nb_steps = (24.0 / timeStep);
		size_t h = weather.GetTRef().GetHour();
		size_t s = GetStage();

		double T = weather[H_TAIR];
		//T = AdjustTLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), T);

		double day_length = weather.GetLocation().GetDayLength(weather.GetTRef()) / 3600.0;//[h]

		if (s < AESTIVAL_DIAPAUSE_ADULT || s == ACTIVE_ADULT)
		{
			//Time step development rate
			double r = Equations().GetRate(s, T) / nb_steps;

			//double corr_r = (s == EGG || s == LARVAE) ? : 1;

			//Relative development rate for this individual
			double rr = m_RDR[s];

			//Time step development rate for this individual
			r *= rr;
			ASSERT(r >= 0 && r < 1);

			//Adjust age
			m_age += r;

			if (!m_dropToGroundDate.IsInit() && m_age > LARVAE4 + 0.9)//drop to the soil when 90% competed (guess)
				m_dropToGroundDate = weather.GetTRef().as(CTM::DAILY);

			//evaluate attrition once a day
			if (GetStand()->m_bApplyAttrition)
			{
				if (IsDeadByAttrition(s, T, r))
					m_bDeadByAttrition = true;
			}

		}
		else if (s == AESTIVAL_DIAPAUSE_ADULT)
		{
			CTRef TRef = weather.GetTRef().as(CTM::DAILY);
			if (TRef == m_adult_emergence)
				m_age = ACTIVE_ADULT;
		}

		if (m_sex == FEMALE && GetStage() >= ACTIVE_ADULT)
		{
			double to = 0;
			double t = timeStep / 24.0;
			double λ = Equations().GetFecondityRate(GetAge(), weather[H_TAIR]);
			double brood = m_Fi * (exp(-λ * (m_t - to)) - exp(-λ * (m_t + t - to)));

			m_broods += brood;
			m_totalBroods += brood;

			m_t += t;
		}
		//else//ACTIVE_ADULT
		//{
		//	double r = (1.0 / m_adult_longevity) / nb_steps;
		//	ASSERT(r >= 0 && r < 1);

		//	m_age += r;
		//}
	}

	//*****************************************************************************
	// Develops all stages, including adults
	// Input:	weather: the weather of the day
	//*****************************************************************************
	void CLaricobiusOsakensis::Live(const CWeatherDay& weather)
	{
		CIndividual::Live(weather);

		ASSERT(IsCreated(weather.GetTRef()));

		if (!IsCreated(weather.GetTRef()))
			return;

		size_t nbSteps = GetTimeStep().NbSteps();
		for (size_t step = 0; step < nbSteps&&IsAlive(); step++)
		{
			size_t h = step * GetTimeStep();
			Live(weather[h], GetTimeStep());
		}

		if (weather.GetTRef() == m_creationDate || HasChangedStage())
			m_reachDate[GetStage()] = weather.GetTRef();

	}

	void CLaricobiusOsakensis::Brood(const CWeatherDay& weather)
	{
		assert(/*IsAlive() &&*/ m_sex == FEMALE);

		if (GetStage() == ACTIVE_ADULT)
		{
			//no brood process done

			//brooding
			//m_broods = m_F;
			//m_totalBroods = m_F;
			//m_F = 0;
			//m_F -= m_F * r;
		}
	}

	// kills by old age and frost
	// Output:  Individual's state is updated to follow update
	void CLaricobiusOsakensis::Die(const CWeatherDay& weather)
	{
		//attrition mortality. Killed at the end of time step

		if (m_bDeadByAttrition)
		{
			m_status = DEAD;
			m_death = ATTRITION;
		}
		else if (GetStage() == DEAD_ADULT)
		{
			//Old age
			m_status = DEAD;
			m_death = OLD_AGE;
		}
		else
		{
			//size_t s = GetStage();

			////Preliminary assessment of the cold tolerance of Laricobius Osakensis, a winter - active predator of the hemlock woolly adelgid from western canada
			////Leland M.Humble
			//static const double COLD_TOLERENCE_T[NB_STAGES] = { -27.5,-22.1, -99.0,-99.0,-19.0,-19.0 };
			////Toland:L. Osakensis was -13.6 oC (± 0.5) with temperatures that ranged from -6 oC to -21 oC.
			//if (weather[H_TMIN][MEAN] < COLD_TOLERENCE_T[s])
			//{
			//	m_status = DEAD;
			//	m_death = FROZEN;
			//}
		}
	}

	//s: stage
	//T: temperature for this time step
	//r: devlopement rate for this time step
	bool CLaricobiusOsakensis::IsDeadByAttrition(size_t s, double T, double r)const
	{
		bool bDeath = false;

		//daily survival
		double ds = GetStand()->m_equations.GetDailySurvivalRate(s, T);

		//time step survival
		double S = pow(ds, r);

		//Computes attrition (probability of survival in a given time step, based on development rate)
		if (RandomGenerator().RandUniform() > S)
			bDeath = true;

		return bDeath;
	}

	//*****************************************************************************
	// GetStat gather information of this object
	//
	// Input: stat: the statistic object
	// Output: The stat is modified
	//*****************************************************************************
	void CLaricobiusOsakensis::GetStat(CTRef d, CModelStat& stat)
	{
		if (IsCreated(d))
		{
			size_t s = GetStage();
			ASSERT(s <= DEAD_ADULT);

			if (IsAlive() || (s == DEAD_ADULT))
				stat[S_EGG + s] += m_scaleFactor;

			stat[S_LARVAE] = stat[S_L1]+ stat[S_L2]+ stat[S_L3]+ stat[S_L4];

			if (HasChangedStatus() && m_status == DEAD && m_death == ATTRITION)
				stat[S_DEAD_ATTRTION] += m_scaleFactor;

			if (HasChangedStage())
				stat[S_M_EGG + s] += m_scaleFactor;

			//if (s == ACTIVE_ADULT)
			//{
			//	stat[S_ADULT_ABUNDANCE] += m_scaleFactor * m_adult_abundance;
			//}
		}
	}

	void CLaricobiusOsakensis::Pack(const CIndividualPtr& pBug)
	{
		assert(m_sex == pBug->GetSex());

		CLaricobiusOsakensis* in = (CLaricobiusOsakensis*)(pBug.get());
		CIndividual::Pack(pBug);
	}

	double CLaricobiusOsakensis::GetInstar(bool includeLast)const
	{
		return (IsAlive() || m_death == OLD_AGE) ? GetStage() : CBioSIMModelBase::VMISS;
	}

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

	//*********************************************************************************
	//CLNFHost

	CLNFHost::CLNFHost(CStand* pStand) :
		CHost(pStand)
	{
	}

	void CLNFHost::Live(const CWeatherDay& weather)
	{
		CHost::Live(weather);
	}

	void CLNFHost::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CHost::GetStat(d, stat, generation);
	}

	//*************************************************
	//CLNFStand

	void CLNFStand::init(int year, const CWeatherYears& weather)
	{
		m_diapause_end = ComputeDiapauseEnd(weather[year]);
	}

	CTRef CLNFStand::ComputeDiapauseEnd(const CWeatherYear& weather)const
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM::DAILY);
		//CTPeriod p = weather.GetPrevious().GetEntireTPeriod(CTM::DAILY);

		double sumDD = 0;

		for (size_t ii = (m_equations.m_ADE[ʎ0] - 1); ii <= (m_equations.m_ADE[ʎ1] - 1); ii++)
		{
			CTRef TRef = p.Begin() + ii;
			const CWeatherDay& wday = weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			double DD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative
			sumDD += DD;
		}

		boost::math::logistic_distribution<double> diapause_end_dist(m_equations.m_ADE[ʎ2], m_equations.m_ADE[ʎ3]);
		int begin = (int)Round((m_equations.m_ADE[ʎ1] - 1) + m_equations.m_ADE[ʎa] * cdf(diapause_end_dist, sumDD), 0);

		return p.Begin() + begin;
	}

	void CLNFStand::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CStand::GetStat(d, stat, generation);

		const CWeatherStation& weather_station = GetModel()->m_weather;
		const CWeatherDay& wday = weather_station.GetDay(d);

		//use year of diapause to compute correctly the adult emergence cdd
		int year = m_diapause_end.GetYear();

		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, DECEMBER, DAY_31);

		if (d >= begin && d <= end)
		{
			//Egg creation DD (allen 1976)
			//m_egg_creation_CDD += m_DD.GetDD(wday);
			//stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;

			//diapause end negative DD
			double T = wday[H_TNTX][MEAN];
			//T = max(m_equations.m_ADE[ʎa], T);
			double NDD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative

			int ii = d - begin;
			if (ii >= int(m_equations.m_ADE[ʎ0] - 1) && ii <= int(m_equations.m_ADE[ʎ1] - 1))
				m_diapause_end_NCDD += NDD;

			stat[S_DIAPAUSE_END_NCDD] = m_diapause_end_NCDD;
		}

		begin = m_diapause_end;
		end = CTRef(m_diapause_end.GetYear() + 1, JANUARY, DAY_31);
		if (d >= begin && d <= end)
		{
			//adult emergence (growing DD)
			double T = wday[H_TNTX][MEAN];
			double GDD = max(0.0, T - m_equations.m_EAS[Τᴴ]);
			m_adult_emergence_CDD += GDD;
			stat[S_ADULT_EMERGENCE_CDD] = m_adult_emergence_CDD;
		}

		//compute egg creation
		begin = CTRef(year-1, DECEMBER, DAY_01);
		end = CTRef(year, AUGUST, DAY_31);

		if (d >= begin && d <= end)
		{
			//Egg creation DD (allen 1976)
			m_egg_creation_CDD += m_DD.GetDD(wday);
			stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;
		}

	}

}


#ifndef PBC_H
#define PBC_H

#include <math.h>
#include <vector>
#include <string>
#include <array>

//#include <Eigen/Dense> // For Supercell and Hessian

class MinImage {
public:
  double volume;
  std::array< std::array<double,3>, 3 > cell;
  std::array< std::array<double,3>, 3 > invcell;
  std::array<double,3> periodic;
  MinImage(){};
  void load(std::array<double,9> lcell);
  void minimumImageVector(double *dr);
  void wrap(double *dx,int dxsize);
};
#endif


//---------------------------------------------------------------------------//
//!
//! \file   Utility_UnitTraits.hpp
//! \author Alex Robinson
//! \brief  Unit traits class specializations
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_UNIT_TRAITS_HPP
#define UTILITY_UNIT_TRAITS_HPP

// Std Lib Includes
#include <type_traits>

// Boost Includes
#include <boost/units/make_system.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/dimensionless_unit.hpp>
#include <boost/units/is_dimensionless.hpp>

// FRENSIE Includes
#include "Utility_UnitTraitsDecl.hpp"

namespace Utility{

/*! The partial specialization of UnitTraits for boost::units::unit
 * \ingroup unit_traits
 */
template<typename Dim, typename Sys>
struct UnitTraits<boost::units::unit<Dim,Sys> >
{
  //! The unit type
  typedef boost::units::unit<Dim,Sys> Unit;

  //! The dimension of the unit
  typedef Dim Dimension;

  //! The unit system that the unit belongs to
  typedef Sys System;

  //! The inverse unit type
  typedef typename boost::units::power_typeof_helper<Unit,boost::units::static_rational<-1> >::type InverseUnit;

  //! The unit raised to rational power N/D type
  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetUnitToPowerType
  {
    typedef typename std::conditional<N!=0,typename boost::units::power_typeof_helper<Unit,boost::units::static_rational<N,D> >::type,void>::type type;
  };

  //! The unit multiplied by another unit type
  template<typename OtherUnit>
  struct GetMultipliedUnitType
  {
    typedef typename std::conditional<std::is_same<OtherUnit,void>::value,Unit,typename boost::units::multiply_typeof_helper<Unit,OtherUnit>::type>::type type;
  };

  //! The wrapped quantity type associated with the unit and value type T
  template<typename T>
  struct GetQuantityType
  { typedef boost::units::quantity<Unit,T> type; };

  //! The name string
  static inline std::string name()
  { return boost::units::name_string( Unit() ); }

  //! The symbol string
  static inline std::string symbol()
  { return boost::units::symbol_string( Unit() ); }
};

/*! The specialization of UnitTraits for void (no units)
 *
 * Note that void (no units) is different from dimensionless, which has a
 * distinct boost::units::unit class.
 * \ingroup unit_traits
 */
template<>
struct UnitTraits<void>
{
  // The dimensionless unit
  //typedef boost::units::dimensionless_unit<boost::units::make_system<>::type>::type UnitType;
  typedef void Unit;
  typedef void Dimension;
  typedef void System;
  typedef void InverseUnit;

  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetUnitToPowerType
  {
    typedef void type;
  };

  template<typename OtherUnit>
  struct GetMultipliedUnitType
  {
    typedef OtherUnit type;
  };

  template<typename T>
  struct GetQuantityType
  { typedef T type; };

  static inline std::string name()
  { return "void"; }

  static inline std::string symbol()
  { return "0"; }
};

} // end Utility namespace

#endif // end UTILITY_UNIT_TRAITS_HPP

//---------------------------------------------------------------------------//
// end Utility_UnitTraits.hpp
//---------------------------------------------------------------------------//


#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_typedefs.hpp>
#include <map>
#include <vector>

#ifndef IIWA_14_FK_FAST_HPP
#define IIWA_14_FK_FAST_HPP

namespace IIWA_14_FK_FAST {
const size_t IIWA_14_NUM_ACTIVE_JOINTS = 7;
const size_t IIWA_14_NUM_LINKS = 8;

const std::string IIWA_14_ACTIVE_JOINT_1_NAME = "iiwa_joint_1";
const std::string IIWA_14_ACTIVE_JOINT_2_NAME = "iiwa_joint_2";
const std::string IIWA_14_ACTIVE_JOINT_3_NAME = "iiwa_joint_3";
const std::string IIWA_14_ACTIVE_JOINT_4_NAME = "iiwa_joint_4";
const std::string IIWA_14_ACTIVE_JOINT_5_NAME = "iiwa_joint_5";
const std::string IIWA_14_ACTIVE_JOINT_6_NAME = "iiwa_joint_6";
const std::string IIWA_14_ACTIVE_JOINT_7_NAME = "iiwa_joint_7";

const std::string IIWA_14_LINK_1_NAME = "iiwa_link_0";
const std::string IIWA_14_LINK_2_NAME = "iiwa_link_1";
const std::string IIWA_14_LINK_3_NAME = "iiwa_link_2";
const std::string IIWA_14_LINK_4_NAME = "iiwa_link_3";
const std::string IIWA_14_LINK_5_NAME = "iiwa_link_4";
const std::string IIWA_14_LINK_6_NAME = "iiwa_link_5";
const std::string IIWA_14_LINK_7_NAME = "iiwa_link_6";
const std::string IIWA_14_LINK_8_NAME = "iiwa_link_7";

inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1575);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2025);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.2045, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.1845, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_6_joint_7_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.081, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  assert(configuration.size() == IIWA_14_NUM_ACTIVE_JOINTS);
  EigenHelpers::VectorIsometry3d link_transforms(IIWA_14_NUM_LINKS);
  link_transforms[0] = base_transform;
  link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
  link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
  link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
  link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
  link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
  link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
  link_transforms[7] = link_transforms[6] * Get_link_6_joint_7_LinkJointTransform(configuration[6]);
  return link_transforms;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  std::vector<double> configuration_vector(IIWA_14_NUM_ACTIVE_JOINTS);
  configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_1_NAME, 0.0);
  configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_2_NAME, 0.0);
  configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_3_NAME, 0.0);
  configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_4_NAME, 0.0);
  configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_5_NAME, 0.0);
  configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_6_NAME, 0.0);
  configuration_vector[6] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_7_NAME, 0.0);
  return GetLinkTransforms(configuration_vector, base_transform);
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}
}  // namespace IIWA_14_FK_FAST

#endif  // IIWA_14_FK_FAST_HPP


#include "tracer.hpp"
#include <fstream>
#include "fileIO.hpp"
#include "density_observer.hpp"

#include <boost/test/unit_test.hpp>

/*! tracer tests
	createTracer potential
	createTracer function
	create with/without monodromy
	check exception if derivatives are missing
	check thread safety
*/

BOOST_AUTO_TEST_SUITE(tracer_tests)

// helper functions
/// \todo this could be done with boost fixtures
Potential<grid<std::vector<double>, 1>> makeEmpty1DPot(int size)
{
	Potential<grid<std::vector<double>, 1>> pot(1.0, size, 2);
	std::vector<double> v(size);
	grid<std::vector<double>, 1> pg(size, v);
	for(auto& v : pg)
	{
		v = 0.0;
	}
	auto copy = v;
	pot.setPotential( std::move(v) );
	v = copy;
	pot.setDerivative( std::vector<int>{1}, std::move(v) );
	return pot;
}

Potential<grid<std::vector<double>, 2>> makeEmpty2DPot(int size)
{
	Potential<grid<std::vector<double>, 2>> pot(1.0, size, 2);
	std::vector<double> v(size*size);
	grid<std::vector<double>, 1> pg(size, v);
	for(auto& v : pg)
	{
		v = 0.0;
	}
	auto copy = v;
	pot.setPotential( std::move(v) );
	v = copy;
	pot.setDerivative( std::vector<int>{1,0}, std::move(v) );
	v = copy;
	pot.setDerivative( std::vector<int>{0,1}, std::move(v) );
	return pot;
}

BOOST_AUTO_TEST_CASE(tracer_ctor)
{
	auto pot = makeEmpty1DPot(128);

	auto tracer = createTracer( pot, false, false );

	// check energy functor
	auto ef = tracer->getEnergy();
	State tp(1);
	tp.editPos()[0] = 0.5;
	tp.editVel()[0] = 1;
	BOOST_CHECK_EQUAL( ef.getPotentialEnergy(tp), 0.0 );
	BOOST_CHECK_EQUAL( ef.getKineticEnergy(tp), 0.5 );
	BOOST_CHECK_EQUAL( ef.getEnergy(tp), 0.5 );

	BOOST_CHECK_EQUAL( tracer->getMaximumEnergyError(), 0.0 );
	BOOST_CHECK_EQUAL( tracer->getDimension(), 1);
	BOOST_CHECK_EQUAL( tracer->getPotentialSize(), 128);
	// no monodromy
	BOOST_CHECK_EQUAL( tracer->getTraceMonodromy(), false);
	BOOST_CHECK_EQUAL( tracer->getPeriodicBoundaries(), false);
	BOOST_CHECK_EQUAL( *tracer->getSecondOrderDerivatives(), (void*)0);

	/// \todo check first order derivative; thread count?
}

BOOST_AUTO_TEST_CASE(empty_pot_tests)
{
	/// \todo 1d trace test currently not available, because no IC can be generated

	double REQ_ACC = 0.1; // required accuracy, currently 0.1%, should be lower

	auto pot2d = makeEmpty2DPot(64);
	auto tr_2d = createTracer( pot2d, false, false );
	auto density_ob_2d = std::make_shared<DensityObserver<2>>( 64 );
	auto ig2d = createInitialConditionGenerator( 2, "planar" );
	ig2d->setParticleCount(640);
	tr_2d->addObserver( density_ob_2d );
	tr_2d->trace(ig2d);
	auto g= density_ob_2d->getDensity();
	for(int i=5; i < 30; ++i)
	for(int j=5; j < 60; ++j)
	{
		// boost require so test fails after first error -> does not spam error output
		/// \todo for some reaseon, this is 0.5 instead of 1
		BOOST_REQUIRE_CLOSE( g(i, j), 1.0, REQ_ACC );
	}
}

BOOST_AUTO_TEST_CASE( no_derivative_test )
{
	constexpr int size = 10;
	Potential<grid<std::vector<double>, 1>> pot(1.0, size, 2);
	std::vector<double> v(size);
	grid<std::vector<double>, 1> pg(size, v);
	for(auto& v : pg)
	{
		v = 0.0;
	}
	pot.setPotential( std::move(v) );

	BOOST_CHECK_THROW(createTracer( pot, false, false ), std::runtime_error);
}

BOOST_AUTO_TEST_CASE( multithread_test )
{
	auto pot2d = makeEmpty2DPot(64);
	auto tr_single = createTracer( pot2d, false, false );
	auto density_ob_single = std::make_shared<DensityObserver<2>>( 64 );
	auto ig2d_single = createInitialConditionGenerator( 2, "planar" );
	ig2d_single->setParticleCount(64);
	tr_single->setMaxThreads(1);
	tr_single->trace(ig2d_single);

	auto tr_multi = createTracer( pot2d, false, false );
	auto density_ob_multi = std::make_shared<DensityObserver<2>>( 64 );
	auto ig2d_multi = createInitialConditionGenerator( 2, "planar" );
	ig2d_multi->setParticleCount(64);
	tr_multi->trace(ig2d_multi);

	auto g1 = density_ob_single->getDensity();
	auto g2 = density_ob_multi->getDensity();
	for(unsigned i=0; i < g1.getGridSize(); ++i)
	for(unsigned j=0; j < g1.getGridSize(); ++j)
	{
		// boost require so test fails after first error -> does not spam error output
		BOOST_REQUIRE_EQUAL( g1(i, j), g2(i,j));
	}
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * ext.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/background/glm/robust_poisson_mean.h>
#include <dials/algorithms/background/glm/creator.h>

namespace dials { namespace algorithms { namespace background { namespace boost_python {

  using namespace boost::python;

  BOOST_PYTHON_MODULE(dials_algorithms_background_glm_ext) {
    class_<RobustPoissonMean>("RobustPoissonMean", no_init)
      .def(init<const af::const_ref<double>&, double, double, double, std::size_t>(
        (arg("Y"),
         arg("mean0"),
         arg("c") = 1.345,
         arg("tolerance") = 1e-3,
         arg("max_iter") = 100)))
      .def("mean", &RobustPoissonMean::mean)
      .def("niter", &RobustPoissonMean::niter)
      .def("error", &RobustPoissonMean::error)
      .def("converged", &RobustPoissonMean::converged);

    class_<GLMBackgroundCreator> creator("Creator", no_init);
    creator
      .def(init<GLMBackgroundCreator::Model, double, std::size_t, std::size_t>((
        arg("model"), arg("tuning_constant"), arg("max_iter"), arg("min_pixels") = 10)))
      .def("__call__", &GLMBackgroundCreator::shoebox)
      .def("__call__", &GLMBackgroundCreator::volume);

    scope in_creator = creator;

    enum_<GLMBackgroundCreator::Model>("model")
      .value("constant2d", GLMBackgroundCreator::Constant2d)
      .value("constant3d", GLMBackgroundCreator::Constant3d)
      .value("loglinear2d", GLMBackgroundCreator::LogLinear2d)
      .value("loglinear3d", GLMBackgroundCreator::LogLinear3d);
  }

}}}}  // namespace dials::algorithms::background::boost_python


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "consensus/yac/impl/supermajority_checker_impl.hpp"

#include <gtest/gtest.h>
#include <boost/range/adaptor/indirected.hpp>
#include "logger/logger.hpp"
#include "module/shared_model/interface_mocks.hpp"

using namespace iroha::consensus::yac;

using ::testing::ReturnRefOfCopy;

static logger::Logger log_ = logger::testLog("YacCommon");

class SupermajorityCheckerTest : public ::testing::Test,
                                 public SupermajorityCheckerImpl {
 public:
  void SetUp() override {}
};

/**
 * @given 2 participants
 * @when check range of voted participants
 * @then correct result
 */
TEST_F(SupermajorityCheckerTest, SuperMajorityCheckWithSize2) {
  log_->info("-----------| F(x, 2), x in {0..3} -----------");

  int N = 2;
  ASSERT_FALSE(checkSize(0, N));
  ASSERT_FALSE(checkSize(1, N));
  ASSERT_TRUE(checkSize(2, N));
  ASSERT_FALSE(checkSize(3, N));
}

/**
 * @given 4 participants
 * @when check range of voted participants
 * @then correct result
 */
TEST_F(SupermajorityCheckerTest, SuperMajorityCheckWithSize4) {
  log_->info("-----------| F(x, 4), x in {0..5} |-----------");

  int N = 4;
  ASSERT_FALSE(checkSize(0, N));
  ASSERT_FALSE(checkSize(1, N));
  ASSERT_FALSE(checkSize(2, N));
  ASSERT_TRUE(checkSize(3, N));
  ASSERT_TRUE(checkSize(4, N));
  ASSERT_FALSE(checkSize(5, N));
}

/**
 * @given 7 participants, 6 voted
 * @when check range of frequent elements
 * @then correct result
 */
TEST_F(SupermajorityCheckerTest, RejectProofSuccessfulCase) {
  log_->info("-----------| RejectProof(x, 6, 7) in {1..3} |-----------");

  ASSERT_TRUE(hasReject(1, 6, 7));
  ASSERT_TRUE(hasReject(2, 6, 7));
  ASSERT_TRUE(hasReject(3, 6, 7));
}

/**
 * @given 7 participants, 6 voted
 * @when check range of frequent elements
 * @then correct result
 */
TEST_F(SupermajorityCheckerTest, RejectProofNegativeCase) {
  log_->info("-----------| RejectProof(x, 6, 7) in {4..6}|-----------");

  ASSERT_FALSE(hasReject(4, 6, 7));
  ASSERT_FALSE(hasReject(5, 6, 7));
  ASSERT_FALSE(hasReject(6, 6, 7));
}

/**
 * @given a pair of peers and a pair different signatures by the first peer
 * @when hasSupermajority is called
 * @then it return false
 */
TEST_F(SupermajorityCheckerTest, PublicKeyUniqueness) {
  using namespace shared_model::crypto;
  using namespace std::string_literals;
  std::vector<std::shared_ptr<shared_model::interface::Peer>> peers;
  auto make_peer_key = [&peers](const std::string &key) {
    PublicKey pub_key(key);
    auto peer = std::make_shared<MockPeer>();
    EXPECT_CALL(*peer, pubkey()).WillRepeatedly(ReturnRefOfCopy(pub_key));

    peers.push_back(peer);
    return pub_key;
  };

  auto peer_key = make_peer_key(std::string(32, '0'));
  make_peer_key(std::string(32, '1'));

  auto sig = std::make_shared<MockSignature>();
  EXPECT_CALL(*sig, publicKey()).WillRepeatedly(ReturnRefOfCopy(peer_key));

  // previous version of the test relied on Block interface, which stored a set
  // of signatures by public key
  std::vector<decltype(sig)> sigs{1, sig};
  ASSERT_FALSE(hasSupermajority(sigs | boost::adaptors::indirected, peers));
}


#include "gr/io/io.h"
#include "gr/utils/geometry.h"
#include "gr/utils/sampling.h"
#include "gr/algorithms/match4pcsBase.h"
#include "gr/algorithms/Functor4pcs.h"
#include "gr/algorithms/FunctorSuper4pcs.h"
#include "gr/algorithms/FunctorBrute4pcs.h"
#include <gr/algorithms/PointPairFilter.h>

#include <Eigen/Dense>

#include <fstream>
#include <iostream>
#include <string>

#include "../demo-utils.h"

#define sqr(x) ((x) * (x))

using namespace std;
using namespace gr;
using namespace gr::Demo;

// data IO
IOManager ioManager;

static inline void printS4PCSParameterList(){
    fprintf(stderr, "\t[ -r result_file_name (%s) ]\n", output.c_str());
    fprintf(stderr, "\t[ -m output matrix file (%s) ]\n", outputMat.c_str());
    fprintf(stderr, "\t[ -x (use 4pcs: false by default) ]\n");
    fprintf(stderr, "\t[ --sampled1 (output sampled cloud 1 -- debug+super4pcs only) ]\n");
    fprintf(stderr, "\t[ --sampled2 (output sampled cloud 2 -- debug+super4pcs only) ]\n");
}
struct TransformVisitor {
    template <typename Derived>
    inline void operator()(
            float fraction,
            float best_LCP,
            const Eigen::MatrixBase<Derived>& /*transformation*/) const {
      if (fraction >= 0)
        {
          printf("done: %d%c best: %f                  \r",
               static_cast<int>(fraction * 100), '%', best_LCP);
          fflush(stdout);
        }
    }
    constexpr bool needsGlobalTransformation() const { return false; }
};

template <
    typename Matcher,
    typename PointType,
    typename Options,
    typename Range,
    template<typename> typename Sampler,
    typename TransformVisitor>
typename PointType::Scalar computeAlignment (
    const Options& options,
    const Utils::Logger& logger,
    const Range& P,
    const Range& Q,
    Eigen::Ref<Eigen::Matrix<typename PointType::Scalar, 4, 4>> mat,
    const Sampler<PointType>& sampler,
    TransformVisitor& visitor
    ) {
  Matcher matcher (options, logger);
  logger.Log<Utils::Verbose>( "Starting registration" );
  typename PointType::Scalar score = matcher.ComputeTransformation(P, Q, mat, sampler, visitor);

  logger.Log<Utils::Verbose>( "Score: ", score );
  logger.Log<Utils::Verbose>( "(Homogeneous) Transformation from ",
                              input2.c_str(),
                              " to ",
                              input1.c_str(),
                              ": \n",
                              mat);

  if(! outputSampled1.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Sampled cloud 1 to ",
                                  outputSampled1.c_str(),
                                  " ..." );
      ioManager.WriteObject((char *)outputSampled1.c_str(),
                             matcher.getFirstSampled(),
                             vector<Eigen::Matrix2f>(),
                             vector<typename Point3D<float>::VectorType>(), // dummy
                             vector<tripple>(),
                             vector<string>());
      logger.Log<Utils::Verbose>( "Export DONE" );
  }
  if(! outputSampled2.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Sampled cloud 2 to ",
                                  outputSampled2.c_str(),
                                  " ..." );
      ioManager.WriteObject((char *)outputSampled2.c_str(),
                             matcher.getSecondSampled(),
                             vector<Eigen::Matrix2f>(),
                             vector<typename Point3D<float>::VectorType>(), // dummy
                             vector<tripple>(),
                             vector<string>());
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  return score;
}

int main(int argc, char **argv) {
  using namespace gr;
  using Scalar = float;
  // Point clouds are read as gr::Point3D, then converted to other types if necessary to
  // emulate PointAdapter usage
  vector<Point3D<Scalar> > set1, set2;
  vector<Eigen::Matrix2f> tex_coords1, tex_coords2;
  vector<typename Point3D<Scalar>::VectorType> normals1, normals2;
  vector<tripple> tris1, tris2;
  vector<std::string> mtls1, mtls2;

  // Match and return the score (estimated overlap or the LCP).
  typename Point3D<Scalar>::Scalar score = 0;

  constexpr Utils::LogLevel loglvl = Utils::Verbose;

  using TrVisitorType = typename std::conditional <loglvl==Utils::NoLog,
                            DummyTransformVisitor,
                            TransformVisitor>::type;
  using PairFilter = gr::AdaptivePointFilter;

  TrVisitorType visitor;
  Utils::Logger logger(loglvl);

  /// TODO Add proper error codes
  if(argc < 4){
    Demo::printUsage(argc, argv);
    printS4PCSParameterList();
    exit(-2);
  }
  if(int c = Demo::getArgs(argc, argv) != 0)
  {
    Demo::printUsage(argc, argv);
    printS4PCSParameterList();
    exit(std::max(c,0));
  }

  // prepare matcher ressourcesoutputSampled2
  using MatrixType = Eigen::Matrix<typename Point3D<Scalar>::Scalar, 4, 4>;
  MatrixType mat (MatrixType::Identity());

  // Read the inputs.
  if (!ioManager.ReadObject((char *)input1.c_str(), set1, tex_coords1, normals1, tris1,
                  mtls1)) {
    logger.Log<Utils::ErrorReport>("Can't read input set1");
    exit(-1);
  }

  if (!ioManager.ReadObject((char *)input2.c_str(), set2, tex_coords2, normals2, tris2,
                  mtls2)) {
    logger.Log<Utils::ErrorReport>("Can't read input set2");
    exit(-1);
  }

  // clean only when we have pset to avoid wrong face to point indexation
  if (tris1.size() == 0)
    Utils::CleanInvalidNormals(set1, normals1);
  if (tris2.size() == 0)
    Utils::CleanInvalidNormals(set2, normals2);

  try {
    if (use_super4pcs) {
      using PointType    = gr::Point3D<Scalar>;
      using MatcherType  = gr::Match4pcsBase<gr::FunctorSuper4PCS, PointType,
                                             TrVisitorType, gr::AdaptivePointFilter,
                                             gr::AdaptivePointFilter::Options>;
      using OptionType   = typename MatcherType::OptionsType;

      UniformDistSampler<PointType> sampler;

      OptionType options;
      if(! Demo::setOptionsFromArgs(options, logger))
        exit(-2); /// \FIXME use status codes for error reporting

      score = computeAlignment<MatcherType, PointType> (options, logger, set1, set2,
                                                        mat, sampler, visitor);
    }
    else {
      // 4PCS
      using PointType    = gr::Point3D<Scalar>;
      using MatcherType  = gr::Match4pcsBase<gr::Functor4PCS, PointType,
                                             TrVisitorType, gr::AdaptivePointFilter,
                                             gr::AdaptivePointFilter::Options>;
      using OptionType   = typename MatcherType::OptionsType;

      UniformDistSampler<PointType> sampler;

      OptionType options;
      if(! Demo::setOptionsFromArgs(options, logger))
        exit(-2); /// \FIXME use status codes for error reporting

      score = computeAlignment<MatcherType, PointType> (options, logger, set1, set2,
                                                        mat, sampler, visitor);
    }
  }
  catch (const std::exception& e) {
      logger.Log<Utils::ErrorReport>( "[Error]: " , e.what() );
      logger.Log<Utils::ErrorReport>( "Aborting with code -3 ..." );
      return -3;
  }
  catch (...) {
      logger.Log<Utils::ErrorReport>( "[Unknown Error]: Aborting with code -4 ..." );
      return -4;
  }

  if(! outputMat.empty() ){
      logger.Log<Utils::Verbose>( "Exporting Matrix to ",
                                  outputMat.c_str(),
                                  "..." );
      ioManager.WriteMatrix(outputMat, mat.cast<double>(), IOManager::POLYWORKS);
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  if (! output.empty() ){

      logger.Log<Utils::Verbose>( "Exporting Registered geometry to ",
                                  output.c_str(),
                                  "..." );
      Utils::TransformPointCloud(set2, mat);
      ioManager.WriteObject((char *)output.c_str(),
                             set2,
                             tex_coords2,
                             normals2,
                             tris2,
                             mtls2);
      logger.Log<Utils::Verbose>( "Export DONE" );
  }

  return 0;
}


// linux specific!
#include <endian.h>

#include <boost/filesystem.hpp>

#include <opencv2/core/core.hpp>

#include "dbglog/dbglog.hpp"

#include "utility/streams.hpp"
#include "utility/buildsys.hpp"
#include "utility/binaryio.hpp"

#include "service/cmdline.hpp"

#include "math/geometry_core.hpp"

namespace po = boost::program_options;
namespace fs = boost::filesystem;
namespace bin = utility::binaryio;

namespace {

class Asc2Gtx : public service::Cmdline {
public:
    Asc2Gtx(const std::string &name, const std::string &version)
        : service::Cmdline(name, version)
    {}

private:
    void configuration(po::options_description &cmdline
                       , po::options_description &config
                       , po::positional_options_description &pd)
        override;

    void configure(const po::variables_map &vars) override;

    int run();

    fs::path input_;
    fs::path output_;

    float nodataIn_ = -88.8888f;
    float nodataOut_ = -88.8888f;
};

void Asc2Gtx::configuration(po::options_description &cmdline
                                , po::options_description &config
                                , po::positional_options_description &pd)
{
    cmdline.add_options()
        ("input", po::value(&input_)->required()
         , "Input ASC file.")
        ("output", po::value(&output_)->required()
         , "Output GTX file.")

        ("nodata.in", po::value(&nodataIn_)->default_value(nodataIn_)
         , "No data value in the input file.")
        ("nodata.out", po::value(&nodataOut_)->default_value(nodataOut_)
         , "No data value in the output file.")
        ;

    pd
        .add("input", 1)
        .add("output", 1)
        ;

    (void) config;
}

void Asc2Gtx::configure(const po::variables_map &vars)
{
    (void) vars;
}

struct Header {
    math::Point2 ll;
    math::Size2f pixel;
    math::Size2 size;
};

struct Grid {
    Header header;
    cv::Mat_<float> data;
};

Grid loadAsc(std::istream &is)
{
    Grid grid;
    auto &header(grid.header);
    auto &data(grid.data);

    {
        std::string h;
        std::getline(is, h);
        std::istringstream his(h);
        his.exceptions(std::ifstream::failbit);

        his >> header.ll(1) >> header.ll(0)
            >> header.pixel.height >> header.pixel.width
            >> header.size.height >> header.size.width
            ;
    };

    grid.data.create(header.size.height, header.size.width);

    for (int j(0), je(data.rows); j != je; ++j) {
        for (int i(0), ie(data.cols); i != ie; ++i) {
            is >> data(j, i);
        }
    }

    return grid;
}

Grid loadAsc(const fs::path &path)
{
    std::ifstream f;
    f.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    Grid grid;

    try {
        f.open(path.string());

        grid = loadAsc(f);
    } catch(const std::ios_base::failure &e) {
        LOGTHROW(err3, std::runtime_error)
            << "Cannot read ASC file " << path << ": " << e.what();
    }
    f.close();

    return grid;
}

namespace be {

void writeImpl(std::ostream &os, const std::uint32_t &v)
{
    const auto bev(::htobe32(v));
    os.write(reinterpret_cast<const char*>(&bev), sizeof(bev));
}

void writeImpl(std::ostream &os, const std::uint64_t &v)
{
    const auto bev(::htobe64(v));
    os.write(reinterpret_cast<const char*>(&bev), sizeof(bev));
}

void writeImpl(std::ostream &os, const float &v)
{
    writeImpl(os, reinterpret_cast<const std::uint32_t&>(v));
}

void writeImpl(std::ostream &os, const double &v)
{
    writeImpl(os, reinterpret_cast<const std::uint64_t&>(v));
}

template <typename T>
void write(std::ostream &os, const T &v)
{
    writeImpl(os, v);
}

} // namespace be

void saveGtx(std::ostream &os, const Grid &grid
             , float nodataIn, float nodataOut)
{
    const auto &header(grid.header);

    be::write<double>(os, header.ll(1));
    be::write<double>(os, header.ll(0));
    be::write<double>(os, header.pixel.height);
    be::write<double>(os, header.pixel.width);
    be::write<std::uint32_t>(os, header.size.height);
    be::write<std::uint32_t>(os, header.size.width);

    for (const auto &value : grid.data) {
        if (value == nodataIn) {
            be::write<float>(os, nodataOut);
        } else {
            be::write<float>(os, value);
        }
    }
}

void saveGtx(const fs::path &path, const Grid &grid
             , float nodataIn, float nodataOut)
{
    std::ofstream f;
    f.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    try {
        f.open(path.string(), std::ios_base::out | std::ios_base::trunc
                | std::ios_base::binary);
        saveGtx(f, grid, nodataIn, nodataOut);
    } catch(const std::ios_base::failure &e) {
        LOGTHROW(err3, std::runtime_error)
            << "Cannot write GTX file " << path << ": " << e.what();
    }
    f.close();
}

int Asc2Gtx::run()
{
    const auto grid(loadAsc(input_));
    saveGtx(output_, grid, nodataIn_, nodataOut_);

    return EXIT_SUCCESS;
}

} // namespace

int main(int argc, char *argv[])
{
    return Asc2Gtx(BUILD_TARGET_NAME, BUILD_TARGET_VERSION)(argc, argv);
}