#include "flareProcesses.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "flareTimeDependent.h"
#include <flosses/lossesSyn.h>

#include "write.h"
#include <fparameters/SpaceIterator.h>
#include <inout/ioutil.h>
#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <fmath/RungeKutta.h>

using namespace std;

void flareProcesses(State& st) {

	maxRadius = GlobalConfig.get<double>("nonThermal.flare.maxRadius")*schwRadius;
	minRadius = GlobalConfig.get<double>("nonThermal.flare.minRadius")*schwRadius;
	etaInj = GlobalConfig.get<double>("nonThermal.flare.injection.energyFraction");
	pIndex = GlobalConfig.get<double>("nonThermal.flare.injection.primaryIndex");

	double t1 = 0.0;
	ofstream vel_file;
	vel_file.open("vel_r.dat",ios::out);
	double r1 = exp(logr.front())*schwRadius;
	double pasoR = pow(maxRadius/exp(logr.front())/schwRadius,1.0/1000);
	for (size_t jT=0;jT<1000;jT++) {
		r1 *= pasoR;
		t1 += r1*(pasoR-1.0)/(-radialVel(r1));
	}
	cout << "tiempo en llegar al horizonte = " << int(t1/3600) << "h" << int(t1/60)-60*int(t1/3600)
			<< "m" << endl;

	timeAfterFlare = 0.0;
	double tMax = 3.0*3600;
	size_t nTime = 50;
	double dt = tMax/nTime;
	ofstream file_cm,file_mm,fileNIR,fileX;
	multiZoneInjection(st.ntPair,st);
	file_cm.open("lightCurve_cm.txt");
	file_mm.open("lightCurve_mm.txt");
	fileNIR.open("lightCurveNIR.txt");
	fileX.open("lightCurveX.txt");

	size_t nEth = 100;
	size_t nRth = 40;
	Vector energies(nEth,0.0);
	Vector radii(nRth,0.0);
	Matrix num_dens;
	if (calculateThermal==0) {
		ifstream file_dens;
		file_dens.open("lumER.txt");
		file_dens >> nEth >> nRth;
		energies.resize(nEth);
		radii.resize(nRth);
		matrixInit(num_dens,nEth,nRth,0.0);
		for (size_t jE=0;jE<nEth;jE++) {
			for (size_t jR=0;jR<nRth;jR++) {
					file_dens >> energies[jE] >> radii[jR] >> num_dens[jE][jR];
			}
		}
		file_dens.close();
	}

	st.photon.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		size_t jR = 0;
		while (r > radii[jR])
			jR++;
		st.photon.ps.iterate([&](const SpaceIterator& iRE) {
			size_t jE = iRE.coord[DIM_E];
			double dist = 0.0;
			if (jR>0) {
				double n1 = num_dens[jE][jR-1];
				double n2 = num_dens[jE][jR];
				double r1 = radii[jR-1];
				double r2 = radii[jR];
				double m = safeLog10(n2/n1)/safeLog10(r2/r1);
				dist = pow(10.0,safeLog10(n1)+m*safeLog10(r/r1));
			} else
				dist = num_dens[jE][0];
			st.photon.distribution.set(iRE,dist);
		},{-1,iR.coord[DIM_R],0});
	},{0,-1,0});

	double Eradiated = 0.0;
	for (size_t i=0;i<nTime;i++) {
		timeAfterFlare += dt;
		//////////////////////////////////////////////////////

		multiZoneDist(st.ntPair,st,-timeAfterFlare);

		if (i % 10 == 0) {
			ofstream fileR;
			fileR.open("energyTime_"+to_string(i)+".txt");
			st.ntPair.ps.iterate([&](const SpaceIterator& iR) {
				double rB2 = iR.val(DIM_R)*sqrt(paso_r);
				double rB1 = rB2/paso_r;
				double vol = (4.0/3.0)*pi*costhetaH(iR.val(DIM_R))*(rB2*rB2*rB2-rB1*rB1*rB1);
				double energy = vol * RungeKuttaSimple(st.ntPair.emin(),st.ntPair.emax(),
					[&](double e) {return st.ntPair.distribution.interpolate({{0,e}},
					&iR.coord)*e;});
				fileR << safeLog10(iR.val(DIM_R)/schwRadius) << "\t" << safeLog10(energy) << endl;
			},{0,-1,0});
			fileR.close();
		}

		// TOTAL RADIATED ENERGY
		double dEdt = 0.0;
		st.ntPair.ps.iterate([&](const SpaceIterator& iR) {
			double rB2 = iR.val(DIM_R)*sqrt(paso_r);
			double rB1 = rB2/paso_r;
			double vol = (4.0/3.0)*pi*costhetaH(iR.val(DIM_R))*(rB2*rB2*rB2-rB1*rB1*rB1);
			double B = st.magf.get(iR);
			double dEdtLocal = vol * RungeKuttaSimple(st.ntPair.emin(),st.ntPair.emax(),
				[&](double e) {return st.ntPair.distribution.interpolate({{0,e}},
				&iR.coord)*lossesSyn(e,B,st.ntPair);});
			dEdt += dEdtLocal;
		},{0,-1,0});

		Eradiated += dEdt * dt;
		//////////////////////////////////////////////////////
		flareEmission2(st,st.ntPair,file_cm,file_mm,fileNIR,fileX);

		if (i==10) writeEandRParamSpace("electronDis",st.ntPair.distribution,0,0);
	}
	cout << "Total energy radiated = " << Eradiated << endl;

	file_cm.close();
	file_mm.close();
	fileNIR.close();
	fileX.close();
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "core_renderer.hpp"

#include <libefl/vector_functions.hpp>

#include <libobjectmodel/point_source_with_reverb.hpp>

#include <libpanning/XYZ.h>
#include <libpanning/LoudspeakerArray.h>

#include <librbbl/biquad_coefficient.hpp>
#include <librbbl/filter_routing.hpp>

#include <librcl/biquad_iir_filter.hpp>

#include <libreverbobject/reverb_object_renderer.hpp>

#include <boost/filesystem.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

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

namespace visr
{
namespace signalflows
{

namespace // unnamed
{
  /**
   * Create a routing that routes \p numChannels input signals to the corresponding output using an individual filter with the same index.
   * @param numChannels The number of input signals, which are translated to the indices 0..numChannels-1
   * @todo Consider to make this an utility function (e.g., in librbbl)
   */
  rbbl::FilterRoutingList oneToOneRouting(std::size_t numChannels)
  {
    rbbl::FilterRoutingList res;
    for (std::size_t chIdx(0); chIdx < numChannels; ++chIdx)
    {
      res.addRouting(chIdx, chIdx, chIdx, 1.0f);
    }
    return res;
  }
}
// unnamed namespace

CoreRenderer::CoreRenderer( SignalFlowContext const & context,
                            char const * name,
                            CompositeComponent * parent,
                            panning::LoudspeakerArray const & loudspeakerConfiguration,
                            std::size_t numberOfInputs,
                            std::size_t numberOfOutputs,
                            std::size_t interpolationPeriod,
                            efl::BasicMatrix<SampleType> const & diffusionFilters,
                            std::string const & trackingConfiguration,
                            std::size_t numberOfObjectEqSections,
                            std::string const & reverbConfig,
                            bool frequencyDependentPanning )
 : CompositeComponent( context, name, parent )
 , mObjectSignalInput( "audioIn", *this, numberOfInputs )
 , mLoudspeakerOutput( "audioOut", *this, numberOfOutputs )
 , mObjectVectorInput( "objectDataInput", *this, pml::EmptyParameterConfig() )
 , mObjectInputGainEqCalculator( context, "ObjectGainEqCalculator", this, numberOfInputs, numberOfObjectEqSections )
 , mObjectGain( context, "ObjectGain", this )
 , mObjectEq( context, "ObjectEq", this )
 , mOutputAdjustment( context, "OutputAdjustment", this )
 , mGainCalculator( context, "VbapGainCalculator", this, numberOfInputs, loudspeakerConfiguration, not trackingConfiguration.empty(),
                    frequencyDependentPanning ? rcl::PanningCalculator::PanningMode::All : (rcl::PanningCalculator::PanningMode::LF | rcl::PanningCalculator::PanningMode::Diffuse) )
 , mVbapMatrix( context, "VbapGainMatrix", this )
 , mVbipMatrix( frequencyDependentPanning ? new rcl::GainMatrix( context, "VbipMatrix", this ) : nullptr)
 , mPanningFilterbank( frequencyDependentPanning ? new rcl::BiquadIirFilter( context, "PanningFilterbank", this ) : nullptr )
 , mDiffuseMatrix( context, "DiffuseMatrix", this )
 , mDecorrelator( context, "DiffusePartDecorrelator", this, loudspeakerConfiguration.getNumRegularSpeakers(),
                  loudspeakerConfiguration.getNumRegularSpeakers(), diffusionFilters.numberOfColumns(),
                  diffusionFilters.numberOfRows(), diffusionFilters.numberOfRows(), diffusionFilters,
                  oneToOneRouting(loudspeakerConfiguration.getNumRegularSpeakers()) )
 , mDirectDiffuseMix( context, "DirectDiffuseMixer", this,
                      loudspeakerConfiguration.getNumRegularSpeakers(),
                      2 + (frequencyDependentPanning ?1:0) + (reverbConfig.empty() ? 0 : 1) )
 , mSubwooferMix( context, "SubwooferMixer", this )
 , mNullSource( context, "NullSource", this,
              (numberOfOutputs == loudspeakerConfiguration.getNumRegularSpeakers() + loudspeakerConfiguration.getNumSubwoofers()) ? 0 : 1 )
{
  std::size_t const numberOfLoudspeakers = loudspeakerConfiguration.getNumRegularSpeakers();
  std::size_t const numberOfSubwoofers = loudspeakerConfiguration.getNumSubwoofers();
  std::size_t const numberOfOutputSignals = numberOfLoudspeakers + numberOfSubwoofers;

  bool const trackingEnabled = not trackingConfiguration.empty( );

  parameterConnection( mObjectVectorInput, mGainCalculator.parameterPort( "objectVectorInput" ) );

  if( trackingEnabled )
  {
    // Instantiate the objects.
    mListenerCompensation.reset( new rcl::ListenerCompensation( context, "TrackingListenerCompensation", this, loudspeakerConfiguration ) );
    mListenerGainDelayCompensation.reset( new rcl::DelayVector( context, "ListenerGainDelayCompensation", this ) );

    // for the very moment, do not parse any options, but use hard-coded option values.
    SampleType const cMaxDelay = 1.0f; // maximum delay (in seconds)

    // We start with a initial gain of 0.0 to suppress transients on startup.
    mListenerGainDelayCompensation->setup( numberOfLoudspeakers, period(), cMaxDelay, "lagrangeOrder0",
                                           rcl::DelayVector::MethodDelayPolicy::Limit,
                                           rcl::DelayVector::ControlPortConfig::All,
                                           0.0f, 1.0f );
    mTrackingPositionInput.reset( new TrackingPositionInput( "trackingPositionInput", *this, pml::EmptyParameterConfig() ) );
    parameterConnection( *mTrackingPositionInput, mListenerCompensation->parameterPort("positionInput") );
    parameterConnection( *mTrackingPositionInput, mGainCalculator.parameterPort("listenerPosition") );
    parameterConnection( mListenerCompensation->parameterPort("delayOutput"), mListenerGainDelayCompensation->parameterPort("delayInput") );
    parameterConnection( mListenerCompensation->parameterPort("gainOutput"), mListenerGainDelayCompensation->parameterPort("gainInput") );
  }

  bool const outputEqSupport = loudspeakerConfiguration.outputEqualisationPresent();
  if( outputEqSupport )
  {
    std::size_t const outputEqSections = loudspeakerConfiguration.outputEqualisationNumberOfBiquads();
    rbbl::BiquadCoefficientMatrix<Afloat> const & eqConfig = loudspeakerConfiguration.outputEqualisationBiquads();
    if( numberOfOutputSignals != eqConfig.numberOfFilters() )
    {
      throw std::invalid_argument( "BaselineRenderer: Size of the output EQ configuration config differs from "
                                   "the number of output signals (regular loudspeakers + subwoofers).");
    }
    mOutputEqualisationFilter.reset( new rcl::BiquadIirFilter( context, "OutputEqualisationFilter", this ) );
    mOutputEqualisationFilter->setup( numberOfOutputSignals, outputEqSections, eqConfig );
  }

  parameterConnection(mObjectVectorInput, mObjectInputGainEqCalculator.parameterPort("objectIn") );
  mObjectGain.setup( numberOfInputs, interpolationPeriod, true /* controlInputs */ );
  audioConnection( mObjectSignalInput, mObjectGain.audioPort("in") );
  parameterConnection( mObjectInputGainEqCalculator.parameterPort("gainOut"), mObjectGain.parameterPort("gainInput"));

  mObjectEq.setup( numberOfInputs, numberOfObjectEqSections, true /* Enable control input */ );
  audioConnection( mObjectGain.audioPort("out"), mObjectEq.audioPort("in") );
  parameterConnection( mObjectInputGainEqCalculator.parameterPort("eqOut"), mObjectEq.parameterPort("eqInput"));

  mVbapMatrix.setup( numberOfInputs, numberOfLoudspeakers, interpolationPeriod, 0.0f );
  audioConnection( mVbapMatrix.audioPort("out"), mDirectDiffuseMix.audioPort("in0") );
  if( frequencyDependentPanning )
  {
    // Static crossover pair (2nd-order Linkwitz-Riley with cutoff 700 Hz @ fs=48 kHz)
    static rbbl::BiquadCoefficient<SampleType> const lowpass{ 0.001921697757295f, 0.003843395514590f, 0.001921697757295f,
        -1.824651307057289f, 0.832338098086468f };
    // Numerator coeffs are negated to account for the 180 degree phase shift of the original design.
    static rbbl::BiquadCoefficient<SampleType> const highpass{ -0.914247351285939f, 1.828494702571878f, -0.914247351285939f,
        -1.824651307057289f, 0.832338098086468f };

    rbbl::BiquadCoefficientMatrix<SampleType> coeffMatrix( 2*numberOfInputs, 1 );
    for( std::size_t chIdx(0); chIdx < numberOfInputs; ++chIdx )
    {
      coeffMatrix( chIdx, 0 ) = lowpass;
      coeffMatrix( chIdx + numberOfInputs, 0 ) = highpass;
    }

    mPanningFilterbank->setup( 2*numberOfInputs, 1, coeffMatrix );
    mVbipMatrix->setup( numberOfInputs, numberOfLoudspeakers, interpolationPeriod, 0.0f );
    parameterConnection( mGainCalculator.parameterPort("vbipGains"), mVbipMatrix->parameterPort( "gainInput") );

    audioConnection( mObjectEq.audioPort("out"), ChannelRange( 0, numberOfInputs ), mPanningFilterbank->audioPort("in"), ChannelRange( 0, numberOfInputs ) );
    audioConnection( mObjectEq.audioPort("out"), ChannelRange( 0, numberOfInputs ), mPanningFilterbank->audioPort("in"), ChannelRange( numberOfInputs, 2*numberOfInputs ) );
    audioConnection( mPanningFilterbank->audioPort("out"), ChannelRange( 0, numberOfInputs ), mVbapMatrix.audioPort("in"), ChannelRange( 0, numberOfInputs ) );
    audioConnection( mPanningFilterbank->audioPort("out"), ChannelRange( numberOfInputs, 2*numberOfInputs ), mVbipMatrix->audioPort("in"), ChannelRange( 0, numberOfInputs ) );

    audioConnection( mVbipMatrix->audioPort("out"), mDirectDiffuseMix.audioPort("in1") );
  }
  else
  {
    audioConnection( mObjectEq.audioPort("out"), mVbapMatrix.audioPort("in") );
  }

  //////////////////////////////////////////////////////////////////////////////////////
  // HOA decoding support
  // The decoder configuration is stored in two text files.
  // At the time being, these are compiled into the binaries.
  // To this end, ".cpp" files are created by adding quote pairs to each line and
  // replacing quotes by \". THis is done manually at the moment, but should be automated in the build system.

  // Workaround to prevent doxygen from searching for this include file (which it is not supposed to see)
  ///@cond NEVER
  static std::string const cRegularArrayConfigStr =
#include "libpanning/test/matlab/arrays/t-design_t8_P40.xml.cpp"
    ;
  ///@endcond NEVER
  panning::LoudspeakerArray allRadRegArray;
  std::stringstream cRegularArrayConfigStream( cRegularArrayConfigStr );
  allRadRegArray.loadXmlStream( cRegularArrayConfigStream );

  // Workaround to prevent doxygen from searching for this include file (which it is not supposed to see)
  ///@cond NEVER
  static std::string const allRadDecoderGainMatrixString =
#include "libpanning/test/matlab/arrays/decode_N8_P40_t-design_t8_P40.txt.cpp"
    ;
  ///@endcond NEVER
  pml::MatrixParameter<Afloat> const allRadDecoderGains
    = pml::MatrixParameter<Afloat>::fromString( allRadDecoderGainMatrixString );
  mAllradGainCalculator.reset( new rcl::HoaAllRadGainCalculator( context, "AllRadGainGalculator", this,
                               numberOfInputs, allRadRegArray, loudspeakerConfiguration, allRadDecoderGains,
                               pml::ListenerPosition(), trackingEnabled ) );

  parameterConnection( mObjectVectorInput, mAllradGainCalculator->parameterPort("objectInput") );
  parameterConnection( mGainCalculator.parameterPort( "vbapGains" ), mAllradGainCalculator->parameterPort( "gainInput" ) );
  parameterConnection( mAllradGainCalculator->parameterPort( "gainOutput" ), mVbapMatrix.parameterPort( "gainInput" ) );

  //////////////////////////////////////////////////////////////////////////////////////

  mDiffuseMatrix.setup( numberOfInputs, numberOfLoudspeakers, interpolationPeriod, 0.0f );
//  parameterConnection( mObjectVectorInput,  mDiffusionGainCalculator.parameterPort("objectInput") );
//  parameterConnection( "DiffusionCalculator", "gainOutput", "DiffusePartMatrix", "gainInput" );
  parameterConnection( mGainCalculator.parameterPort("diffuseGains"), mDiffuseMatrix.parameterPort("gainInput") );
  audioConnection( mObjectEq.audioPort("out"), mDiffuseMatrix.audioPort("in") );

  efl::BasicVector<SampleType> const & outputGains =loudspeakerConfiguration.getGainAdjustment();
  efl::BasicVector<SampleType> const & outputDelays = loudspeakerConfiguration.getDelayAdjustment();

  Afloat const * const maxEl = std::max_element( outputDelays.data(),
                                                 outputDelays.data()+outputDelays.size() );
  Afloat const maxDelay = std::ceil( *maxEl ); // Sufficient for nearestSample even if there is no particular compensation for the interpolation method's delay inside.

  mOutputAdjustment.setup( numberOfOutputSignals, period(), maxDelay,
                           "lagrangeOrder0",
                           rcl::DelayVector::MethodDelayPolicy::Limit,
                           rcl::DelayVector::ControlPortConfig::None,
                           outputDelays, outputGains );

  // Note: This assumes that the type 'Afloat' used in libpanning is
  // identical to SampleType (at the moment, both are floats).
  efl::BasicMatrix<SampleType> const & subwooferMixGains = loudspeakerConfiguration.getSubwooferGains();
  mSubwooferMix.setup( numberOfLoudspeakers, numberOfSubwoofers, 0/*interpolation steps*/, subwooferMixGains, false/*controlInput*/ );

  if( not reverbConfig.empty() )
  {
    mReverbRenderer.reset( new reverbobject::ReverbObjectRenderer( context, "ReverbObjectRenderer", this,
                                                                   reverbConfig, loudspeakerConfiguration,
                                                                   numberOfInputs ) );

    audioConnection( mObjectEq.audioPort("out"), mReverbRenderer->audioPort("in") );
    char const * diffuseInPort = frequencyDependentPanning ? "in3" : "in2";
    audioConnection( mReverbRenderer->audioPort("out"), mDirectDiffuseMix.audioPort( diffuseInPort) );

    parameterConnection( mObjectVectorInput, mReverbRenderer->parameterPort("objectIn") );
  }

  audioConnection( mDiffuseMatrix.audioPort("out"), mDecorrelator.audioPort("in") );
  audioConnection( mDecorrelator.audioPort("out"),
                   mDirectDiffuseMix.audioPort( frequencyDependentPanning ? "in2" : "in1" ) );

  if( trackingEnabled )
  {
    audioConnection( mDirectDiffuseMix.audioPort("out"), mListenerGainDelayCompensation->audioPort("in") );
    audioConnection( mListenerGainDelayCompensation->audioPort("out"), mSubwooferMix.audioPort("in") );
    if( outputEqSupport )
    {
      audioConnection( mListenerGainDelayCompensation->audioPort("out"), mOutputEqualisationFilter->audioPort("in") );
    }
    else
    {
      audioConnection( mListenerGainDelayCompensation->audioPort("out"), ChannelRange( 0, numberOfLoudspeakers ),
                       mOutputAdjustment.audioPort("in"), ChannelRange( 0, numberOfLoudspeakers ) );
    }
  }
  else
  {
    audioConnection( mDirectDiffuseMix.audioPort("out"), mSubwooferMix.audioPort("in") );
    if( outputEqSupport )
    {
      audioConnection( mDirectDiffuseMix.audioPort("out"),
                      ChannelRange( 0, numberOfLoudspeakers ),
                      mOutputEqualisationFilter->audioPort("in"),
                      ChannelRange( 0, numberOfLoudspeakers ) );
      audioConnection( mSubwooferMix.audioPort("out"),
                      ChannelRange( 0, numberOfSubwoofers ),
                      mOutputEqualisationFilter->audioPort("in"),
                      ChannelRange( numberOfLoudspeakers, numberOfLoudspeakers + numberOfSubwoofers ) );
      audioConnection( mOutputEqualisationFilter->audioPort("out"), mOutputAdjustment.audioPort("in") );
    }
    else
    {
      audioConnection( mDirectDiffuseMix.audioPort("out"), ChannelRange( 0, numberOfLoudspeakers ),
                       mOutputAdjustment.audioPort("in"), ChannelRange( 0, numberOfLoudspeakers ) );
      audioConnection( mSubwooferMix.audioPort("out"), ChannelRange( 0, numberOfSubwoofers ),
                       mOutputAdjustment.audioPort("in"), ChannelRange( numberOfLoudspeakers, numberOfLoudspeakers + numberOfSubwoofers ) );
    }
  }

  // Connect to the external playback channels, including the silencing of unused channels.
  if( numberOfLoudspeakers + numberOfSubwoofers > numberOfOutputs ) // Otherwise the computation below would cause an immense memory allocation.
  {
    throw std::invalid_argument( "The number of loudspeakers plus subwoofers exceeds the number of output channels." );
  }
  constexpr ChannelList::IndexType invalidIdx = std::numeric_limits<ChannelList::IndexType>::max();
  std::vector<ChannelList::IndexType> activePlaybackChannels( numberOfLoudspeakers + numberOfSubwoofers, invalidIdx );
  for( std::size_t idx( 0 ); idx < numberOfLoudspeakers; ++idx )
  {
    panning::LoudspeakerArray::ChannelIndex const chIdx = loudspeakerConfiguration.channelIndex( idx ) - 1;
    if( chIdx >= static_cast<panning::LoudspeakerArray::ChannelIndex>(numberOfOutputs) )
    {
      throw std::invalid_argument( "The loudspeakers channel index exceeds the admissible range." );
    }
    // This does not check whether an index is used multiple times.
    activePlaybackChannels[idx] = chIdx;
  }
  for( std::size_t idx( 0 ); idx < numberOfSubwoofers; ++idx )
  {
    panning::LoudspeakerArray::ChannelIndex const chIdx = loudspeakerConfiguration.getSubwooferChannels()[idx]-1;
    if( chIdx >= static_cast<panning::LoudspeakerArray::ChannelIndex>(numberOfOutputs) )
    {
      throw std::invalid_argument( "The subwoofer channel index exceeds the admissible range." );
    }
    // This does not check whether an index is used multiple times.
    activePlaybackChannels[numberOfLoudspeakers + idx] = chIdx;
  }
  if( std::find( activePlaybackChannels.begin(), activePlaybackChannels.end(), invalidIdx ) != activePlaybackChannels.end() )
  {
    throw std::invalid_argument( "Not all active output channels are assigned." );
  }
  std::vector<ChannelList::IndexType> sortedPlaybackChannels( activePlaybackChannels );
  std::sort( sortedPlaybackChannels.begin(), sortedPlaybackChannels.end() );
  if( std::unique( sortedPlaybackChannels.begin(), sortedPlaybackChannels.end() ) != sortedPlaybackChannels.end() )
  {
    throw std::invalid_argument( "The loudspeaker array contains a duplicated output channel index." );
  }
  audioConnection( mOutputAdjustment.audioPort("out"), ChannelRange(0, numberOfLoudspeakers + numberOfSubwoofers ),
                   mLoudspeakerOutput, ChannelList( activePlaybackChannels ) );

  std::size_t const numSilentOutputs = numberOfOutputs - (numberOfLoudspeakers + numberOfSubwoofers);
  if( numSilentOutputs > 0 )
  {
    std::vector<ChannelList::IndexType> nullOutput( numSilentOutputs, 0 );
    std::vector<ChannelList::IndexType> silencedPlaybackChannels( numSilentOutputs, invalidIdx );
    std::size_t silentIdx = 0;
    std::vector<ChannelList::IndexType>::const_iterator runIt{ sortedPlaybackChannels.begin() };
    for( std::size_t idx( 0 ); idx < numberOfOutputs; ++idx )
    {
      if( runIt == sortedPlaybackChannels.end() )
      {
        silencedPlaybackChannels[silentIdx++] = idx;
        continue;
      }
      if( idx < *runIt )
      {
        silencedPlaybackChannels[silentIdx++] = idx;
      }
      else
      {
        runIt++;
      }
    }
    if( silentIdx != numSilentOutputs )
    {
      throw std::logic_error( "Internal logic error: Computation of silent output channels failed." );
    }
    audioConnection( mNullSource.audioPort("out"), ChannelList( nullOutput ),
                     mLoudspeakerOutput, ChannelList( silencedPlaybackChannels ) );
  }
}

CoreRenderer::~CoreRenderer( ) = default;
} // namespace signalflows
} // namespace visr


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

#include "piecewise_polynomial_continuous_scalar_space.hpp"

#include "piecewise_polynomial_discontinuous_scalar_space.hpp"
#include "space_helper.hpp"

#include "../assembly/discrete_sparse_boundary_operator.hpp"
#include "../common/acc.hpp"
#include "../common/boost_make_shared_fwd.hpp"
#include "../common/bounding_box_helpers.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/lagrange_scalar_shapeset.hpp"
#include "../grid/entity.hpp"
#include "../grid/entity_iterator.hpp"
#include "../grid/geometry.hpp"
#include "../grid/grid.hpp"
#include "../grid/grid_segment.hpp"
#include "../grid/grid_view.hpp"
#include "../grid/mapper.hpp"
#include "../grid/vtk_writer.hpp"

#include <stdexcept>
#include <iostream>

#include <boost/array.hpp>

namespace Bempp {

template <typename BasisFunctionType>
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialContinuousScalarSpace(const shared_ptr<const Grid> &grid,
                                             int polynomialOrder)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_segment(GridSegment::wholeGrid(*grid)), m_strictlyOnSegment(false),
      m_flatLocalDofCount(0) {
  initialize();
}

template <typename BasisFunctionType>
bool
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::spaceIsCompatible(
    const Space<BasisFunctionType> &other) const {

  typedef PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>
  thisSpaceType;

  if (other.grid().get() != this->grid().get())
    return false;

  if (other.spaceIdentifier() == this->spaceIdentifier()) {
    // Try to typecast the other space down.
    const thisSpaceType &temp = dynamic_cast<const thisSpaceType &>(other);
    if (this->m_polynomialOrder == temp.m_polynomialOrder)
      return true;
    else
      return false;
  } else
    return false;
}

template <typename BasisFunctionType>
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialContinuousScalarSpace(const shared_ptr<const Grid> &grid,
                                             int polynomialOrder,
                                             const GridSegment &segment,
                                             bool strictlyOnSegment)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_segment(segment), m_strictlyOnSegment(strictlyOnSegment),
      m_flatLocalDofCount(0) {
  initialize();
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::initialize() {
  const int gridDim = this->grid()->dim();
  if (gridDim != 2)
    throw std::invalid_argument("PiecewisePolynomialContinuousScalarSpace::"
                                "PiecewisePolynomialContinuousScalarSpace(): "
                                "2-dimensional grids are supported");
  m_view = this->grid()->leafView();
  if (m_polynomialOrder == 1)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 1>());
  else if (m_polynomialOrder == 2)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 2>());
  else if (m_polynomialOrder == 3)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 3>());
  else if (m_polynomialOrder == 4)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 4>());
  else if (m_polynomialOrder == 5)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 5>());
  else if (m_polynomialOrder == 6)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 6>());
  else if (m_polynomialOrder == 7)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 7>());
  else if (m_polynomialOrder == 8)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 8>());
  else if (m_polynomialOrder == 9)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 9>());
  else if (m_polynomialOrder == 10)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarShapeset<3, BasisFunctionType, 10>());
  else
    throw std::invalid_argument("PiecewisePolynomialContinuousScalarSpace::"
                                "PiecewisePolynomialContinuousScalarSpace(): "
                                "polynomialOrder must be >= 1 and <= 10");
  assignDofsImpl();
}

template <typename BasisFunctionType>
PiecewisePolynomialContinuousScalarSpace<
    BasisFunctionType>::~PiecewisePolynomialContinuousScalarSpace() {}

template <typename BasisFunctionType>
int
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::domainDimension()
    const {
  return this->grid()->dim();
}

template <typename BasisFunctionType>
int
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::codomainDimension()
    const {
  return 1;
}

template <typename BasisFunctionType>
const Fiber::Shapeset<BasisFunctionType> &
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::shapeset(
    const Entity<0> &element) const {
  if (elementVariant(element) == 3)
    return *m_triangleShapeset;
  throw std::logic_error(
      "PiecewisePolynomialContinuousScalarSpace::shapeset(): "
      "invalid element variant, this shouldn't happen!");
}

template <typename BasisFunctionType>
ElementVariant
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::elementVariant(
    const Entity<0> &element) const {
  GeometryType type = element.type();
  if (type.isLine())
    return 2;
  else if (type.isTriangle())
    return 3;
  else if (type.isQuadrilateral())
    return 4;
  else
    throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                             "elementVariant(): invalid geometry type, "
                             "this shouldn't happen!");
}

template <typename BasisFunctionType>
void
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::setElementVariant(
    const Entity<0> &element, ElementVariant variant) {
  if (variant != elementVariant(element))
    // for this space, the element variants are unmodifiable,
    throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                             "setElementVariant(): invalid variant");
}

template <typename BasisFunctionType>
shared_ptr<const Space<BasisFunctionType>>
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::discontinuousSpace(
    const shared_ptr<const Space<BasisFunctionType>> &self) const {
  if (!m_discontinuousSpace) {
    tbb::mutex::scoped_lock lock(m_discontinuousSpaceMutex);
    typedef PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>
    DiscontinuousSpace;
    if (!m_discontinuousSpace)
      m_discontinuousSpace.reset(
          new DiscontinuousSpace(this->grid(), m_polynomialOrder, m_segment));
  }
  return m_discontinuousSpace;
}

template <typename BasisFunctionType>
bool
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::isDiscontinuous()
    const {
  return false;
}

template <typename BasisFunctionType>
void
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::assignDofsImpl() {
  // TODO: refactor this function, it's way too long!

  // In addition to DOF assignment, this function also precalculates bounding
  // boxes of global DOFs

  const int elementCount = m_view->entityCount(0);
  if (elementCount == 0)
    return;
  const int gridDim = this->domainDimension();
  if (gridDim != 2)
    throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                             "assignDofsImpl(): only 2-dimensional grids "
                             "are supported at present");
  const int vertexCodim = gridDim;
  const int edgeCodim = vertexCodim - 1;

  // const Mapper& elementMapper = m_view->elementMapper();
  const IndexSet &indexSet = m_view->indexSet();

  // Map vertices to global dofs
  const int vertexCount = m_view->entityCount(2);
  // At first, the elements of this vector will be set to the number of
  // DOFs corresponding to a given vertex or to -1 if that vertex is to be
  // ignored
  std::vector<GlobalDofIndex> vertexGlobalDofs(vertexCount);
  for (int i = 0; i < vertexCount; ++i)
    if (m_segment.contains(gridDim, i))
      acc(vertexGlobalDofs, (size_t)i) = 1;
    else
      acc(vertexGlobalDofs, (size_t)i) = -1;

  // Map edges to global dofs
  const int edgeCount = m_view->entityCount(1);
  const int internalDofCountPerEdge = m_polynomialOrder - 1;
  // At first, the elements of this vector will be set to the number of
  // DOFs corresponding to a given edge or to -1 if that edge is to be
  // ignored
  std::vector<GlobalDofIndex> edgeStartingGlobalDofs(edgeCount);
  for (int i = 0; i < edgeCount; ++i)
    if (m_segment.contains(gridDim - 1, i))
      acc(edgeStartingGlobalDofs, i) = internalDofCountPerEdge;
    else
      acc(edgeStartingGlobalDofs, i) = -1;

  // Map element interiors to global dofs
  // and, if striclyOnSegment is set, detect vertices and edges not belonging
  // to any element on segment
  const int bubbleDofCountPerTriangle =
      std::max(0, (m_polynomialOrder - 1) * (m_polynomialOrder - 2) / 2);
  const int bubbleDofCountPerQuad =
      std::max(0, (m_polynomialOrder - 1) * (m_polynomialOrder - 1));
  // At first, the elements of this vector will be set to the number of
  // DOFs corresponding to a given element or to -1 if that element is to be
  // ignored
  std::vector<GlobalDofIndex> bubbleStartingGlobalDofs(elementCount);
  std::vector<bool> noElementAdjacentToVertexIsOnSegment(vertexCount, true);
  std::vector<bool> noElementAdjacentToEdgeIsOnSegment(edgeCount, true);
  std::unique_ptr<EntityIterator<0>> it = m_view->entityIterator<0>();
  while (!it->finished()) {
    const Entity<0> &element = it->entity();
    EntityIndex elementIndex = indexSet.entityIndex(element);
    int vertexCount = element.template subEntityCount<2>();
    if (vertexCount != 3 && vertexCount != 4)
      throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                               "assignDofsImpl(): elements must be "
                               "triangular or quadrilateral");
    if (m_segment.contains(0, elementIndex)) {
      acc(bubbleStartingGlobalDofs, elementIndex) =
          vertexCount == 3 ? bubbleDofCountPerTriangle : bubbleDofCountPerQuad;
      if (m_strictlyOnSegment)
        for (int i = 0; i < vertexCount; ++i) {
          int index = indexSet.subEntityIndex(element, i, gridDim);
          acc(noElementAdjacentToVertexIsOnSegment, index) = false;
          index = indexSet.subEntityIndex(element, i, gridDim - 1);
          acc(noElementAdjacentToEdgeIsOnSegment, index) = false;
        }
    } else
      acc(bubbleStartingGlobalDofs, elementIndex) = -1;
    it->next();
  }

  // If strictlyOnSegment is set, deactivate vertices and edges not adjacent
  // to any element in segment
  if (m_strictlyOnSegment) {
    for (int i = 0; i < vertexCount; ++i)
      if (acc(noElementAdjacentToVertexIsOnSegment, i))
        acc(vertexGlobalDofs, i) = -1;

    for (int i = 0; i < edgeCount; ++i)
      if (acc(noElementAdjacentToEdgeIsOnSegment, i))
        acc(edgeStartingGlobalDofs, i) = -1;
  }

  // Assign global dofs to entities
  int globalDofCount_ = 0;
  for (int i = 0; i < vertexCount; ++i)
    if (acc(vertexGlobalDofs, i) == 1)
      acc(vertexGlobalDofs, i) = globalDofCount_++;
  for (int i = 0; i < edgeCount; ++i) {
    int dofCount = acc(edgeStartingGlobalDofs, i);
    if (dofCount > 0) {
      acc(edgeStartingGlobalDofs, i) = globalDofCount_;
      globalDofCount_ += dofCount;
    }
  }
  for (int i = 0; i < elementCount; ++i) {
    int dofCount = acc(bubbleStartingGlobalDofs, i);
    if (dofCount > 0) {
      acc(bubbleStartingGlobalDofs, i) = globalDofCount_;
      globalDofCount_ += dofCount;
    }
  }

  // Initialise DOF maps
  const int localDofCountPerTriangle =
      (m_polynomialOrder + 1) * (m_polynomialOrder + 2) / 2;
  m_local2globalDofs.clear();
  std::vector<GlobalDofIndex> prototypeGlobalDofs;
  prototypeGlobalDofs.reserve(localDofCountPerTriangle);
  m_local2globalDofs.resize(elementCount, prototypeGlobalDofs);
  m_global2localDofs.clear();
  // std::vector<LocalDof> prototypeLocalDofs;
  // prototypeLocalDofs.reserve(localDofCountPerTriangle);
  m_global2localDofs.resize(globalDofCount_ /*, prototypeLocalDofs*/);

  // Initialise bounding-box caches
  BoundingBox<CoordinateType> model;
  model.lbound.x = std::numeric_limits<CoordinateType>::max();
  model.lbound.y = std::numeric_limits<CoordinateType>::max();
  model.lbound.z = std::numeric_limits<CoordinateType>::max();
  model.ubound.x = -std::numeric_limits<CoordinateType>::max();
  model.ubound.y = -std::numeric_limits<CoordinateType>::max();
  model.ubound.z = -std::numeric_limits<CoordinateType>::max();
  m_globalDofBoundingBoxes.resize(globalDofCount_, model);

  // Iterate over elements
  it = m_view->entityIterator<0>();
  arma::Mat<CoordinateType> vertices;
  arma::Col<CoordinateType> dofPosition;
  m_flatLocalDofCount = 0;
  std::vector<int> gdofAccessCounts(globalDofCount_, 0);
  while (!it->finished()) {
    const Entity<0> &element = it->entity();
    const Geometry &geo = element.geometry();
    EntityIndex elementIndex = indexSet.entityIndex(element);
    bool elementContained =
        !m_strictlyOnSegment || m_segment.contains(0, elementIndex);

    geo.getCorners(vertices);
    int vertexCount = vertices.n_cols;

    // List of global DOF indices corresponding to the local DOFs of the
    // current element
    std::vector<GlobalDofIndex> &globalDofs =
        acc(m_local2globalDofs, elementIndex);
    if (vertexCount == 3) {
      std::vector<int> ldofAccessCounts(localDofCountPerTriangle, 0);
      boost::array<int, 3> vertexIndices;
      for (int i = 0; i < 3; ++i)
        acc(vertexIndices, i) =
            indexSet.subEntityIndex(element, i, vertexCodim);
      globalDofs.resize(localDofCountPerTriangle);
      // vertex dofs
      {
        int ldof, gdof;

        ldof = 0;
        if (elementContained)
          gdof = vertexGlobalDofs[acc(vertexIndices, 0)];
        else
          gdof = -1;
        if (gdof >= 0) {
          acc(globalDofs, ldof) = gdof;
          acc(m_global2localDofs, gdof).push_back(LocalDof(elementIndex, ldof));
          ++acc(gdofAccessCounts, gdof);
          extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, gdof), vertices.col(0));
          ++m_flatLocalDofCount;
        } else
          acc(globalDofs, ldof) = -1;
        ++acc(ldofAccessCounts, ldof);

        ldof = m_polynomialOrder;
        if (elementContained)
          gdof = vertexGlobalDofs[acc(vertexIndices, 1)];
        else
          gdof = -1;
        if (gdof >= 0) {
          acc(globalDofs, ldof) = gdof;
          acc(m_global2localDofs, gdof).push_back(LocalDof(elementIndex, ldof));
          ++acc(gdofAccessCounts, gdof);
          extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, gdof), vertices.col(1));
          ++m_flatLocalDofCount;
        } else
          acc(globalDofs, ldof) = -1;
        ++acc(ldofAccessCounts, ldof);

        ldof = localDofCountPerTriangle - 1;
        if (elementContained)
          gdof = vertexGlobalDofs[acc(vertexIndices, 2)];
        else
          gdof = -1;
        if (gdof >= 0) {
          acc(globalDofs, ldof) = gdof;
          acc(m_global2localDofs, gdof).push_back(LocalDof(elementIndex, ldof));
          ++acc(gdofAccessCounts, gdof);
          extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, gdof), vertices.col(2));
          ++m_flatLocalDofCount;
        } else
          acc(globalDofs, ldof) = -1;
        ++acc(ldofAccessCounts, ldof);
      }

      // edge dofs
      if (m_polynomialOrder >= 2) {
        int start, end, step;
        int edgeIndex;

        edgeIndex = indexSet.subEntityIndex(element, 0, edgeCodim);
        dofPosition = 0.5 * (vertices.col(0) + vertices.col(1));
        if (acc(edgeStartingGlobalDofs, edgeIndex) >= 0 && elementContained) {
          if (acc(vertexIndices, 0) < acc(vertexIndices, 1)) {
            start = acc(edgeStartingGlobalDofs, edgeIndex);
            end = start + internalDofCountPerEdge;
            step = 1;
          } else {
            end = acc(edgeStartingGlobalDofs, edgeIndex) - 1;
            start = end + internalDofCountPerEdge;
            step = -1;
          }
          for (int ldof = 1, gdof = start; gdof != end; ++ldof, gdof += step) {
            acc(globalDofs, ldof) = gdof;
            acc(m_global2localDofs, gdof)
                .push_back(LocalDof(elementIndex, ldof));
            ++acc(ldofAccessCounts, ldof);
            ++acc(gdofAccessCounts, gdof);
            extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
            setBoundingBoxReference<CoordinateType>(
                acc(m_globalDofBoundingBoxes, gdof), dofPosition);
            ++m_flatLocalDofCount;
          }
        } else
          for (int ldof = 1; ldof <= internalDofCountPerEdge; ++ldof) {
            acc(globalDofs, ldof) = -1;
            ++acc(ldofAccessCounts, ldof);
          }

        edgeIndex = indexSet.subEntityIndex(element, 1, edgeCodim);
        dofPosition = 0.5 * (vertices.col(0) + vertices.col(2));
        if (acc(edgeStartingGlobalDofs, edgeIndex) >= 0 && elementContained) {
          if (acc(vertexIndices, 0) < acc(vertexIndices, 2)) {
            start = acc(edgeStartingGlobalDofs, edgeIndex);
            end = start + internalDofCountPerEdge;
            step = 1;
          } else {
            end = acc(edgeStartingGlobalDofs, edgeIndex) - 1;
            start = end + internalDofCountPerEdge;
            step = -1;
          }
          for (int ldofy = 1, gdof = start; gdof != end;
               ++ldofy, gdof += step) {
            int ldof =
                ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
            acc(globalDofs, ldof) = gdof;
            acc(m_global2localDofs, gdof)
                .push_back(LocalDof(elementIndex, ldof));
            ++acc(ldofAccessCounts, ldof);
            ++acc(gdofAccessCounts, gdof);
            extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
            setBoundingBoxReference<CoordinateType>(
                acc(m_globalDofBoundingBoxes, gdof), dofPosition);
            ++m_flatLocalDofCount;
          }
        } else
          for (int ldofy = 1; ldofy <= internalDofCountPerEdge; ++ldofy) {
            int ldof =
                ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
            acc(globalDofs, ldof) = -1;
            ++acc(ldofAccessCounts, ldof);
          }

        edgeIndex = indexSet.subEntityIndex(element, 2, edgeCodim);
        dofPosition = 0.5 * (vertices.col(1) + vertices.col(2));
        if (acc(edgeStartingGlobalDofs, edgeIndex) >= 0 && elementContained) {
          if (acc(vertexIndices, 1) < acc(vertexIndices, 2)) {
            start = acc(edgeStartingGlobalDofs, edgeIndex);
            end = start + internalDofCountPerEdge;
            step = 1;
          } else {
            end = acc(edgeStartingGlobalDofs, edgeIndex) - 1;
            start = end + internalDofCountPerEdge;
            step = -1;
          }
          for (int ldofy = 1, gdof = start; gdof != end;
               ++ldofy, gdof += step) {
            int ldof = ldofy * (m_polynomialOrder + 1) -
                       ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
            acc(globalDofs, ldof) = gdof;
            acc(m_global2localDofs, gdof)
                .push_back(LocalDof(elementIndex, ldof));
            ++acc(ldofAccessCounts, ldof);
            ++acc(gdofAccessCounts, gdof);
            extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
            setBoundingBoxReference<CoordinateType>(
                acc(m_globalDofBoundingBoxes, gdof), dofPosition);
            ++m_flatLocalDofCount;
          }
        } else
          for (int ldofy = 1; ldofy <= internalDofCountPerEdge; ++ldofy) {
            int ldof = ldofy * (m_polynomialOrder + 1) -
                       ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
            acc(globalDofs, ldof) = -1;
            ++acc(ldofAccessCounts, ldof);
          }
      }

      // bubble dofs
      if (m_polynomialOrder >= 3) {
        dofPosition =
            (vertices.col(0) + vertices.col(1) + vertices.col(2)) / 3.;
        bool useDofs = acc(bubbleStartingGlobalDofs, elementIndex) >= 0;
        for (int ldofy = 1, gdof = acc(bubbleStartingGlobalDofs, elementIndex);
             ldofy < m_polynomialOrder; ++ldofy)
          for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder;
               ++ldofx, ++gdof) {
            int ldof = ldofy * (m_polynomialOrder + 1) -
                       ldofy * (ldofy - 1) / 2 + ldofx;
            if (useDofs) {
              acc(globalDofs, ldof) = gdof;
              acc(m_global2localDofs, gdof)
                  .push_back(LocalDof(elementIndex, ldof));
              ++acc(gdofAccessCounts, gdof);
              extendBoundingBox(acc(m_globalDofBoundingBoxes, gdof), vertices);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, gdof), dofPosition);
              ++m_flatLocalDofCount;
            } else
              acc(globalDofs, ldof) = -1;
            ++acc(ldofAccessCounts, ldof);
          }
      }
      for (size_t i = 0; i < ldofAccessCounts.size(); ++i)
        assert(acc(ldofAccessCounts, i) == 1);
    } else
      throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                               "assignDofsImpl(): quadrilateral elements "
                               "are not supported yet");

    it->next();
  }
// for (size_t i = 0; i < gdofAccessCounts.size(); ++i)
//     std::cout << i << " " << acc(gdofAccessCounts, i) << "\n";

#ifndef NDEBUG
  for (size_t i = 0; i < globalDofCount_; ++i) {
    const BoundingBox<CoordinateType> &bbox = acc(m_globalDofBoundingBoxes, i);

    assert(bbox.reference.x >= bbox.lbound.x);
    assert(bbox.reference.y >= bbox.lbound.y);
    assert(bbox.reference.z >= bbox.lbound.z);
    assert(bbox.reference.x <= bbox.ubound.x);
    assert(bbox.reference.y <= bbox.ubound.y);
    assert(bbox.reference.z <= bbox.ubound.z);
  }
#endif // NDEBUG

  // Initialize the container mapping the flat local dof indices to
  // local dof indices
  SpaceHelper<BasisFunctionType>::initializeLocal2FlatLocalDofMap(
      m_flatLocalDofCount, m_local2globalDofs, m_flatLocal2localDofs);
}

template <typename BasisFunctionType>
size_t
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::globalDofCount()
    const {
  return m_global2localDofs.size();
}

template <typename BasisFunctionType>
size_t
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::flatLocalDofCount()
    const {
  return m_flatLocalDofCount;
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::getGlobalDofs(
    const Entity<0> &element, std::vector<GlobalDofIndex> &dofs) const {
  const Mapper &mapper = m_view->elementMapper();
  EntityIndex index = mapper.entityIndex(element);
  dofs = m_local2globalDofs[index];
}

template <typename BasisFunctionType>
void
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::global2localDofs(
    const std::vector<GlobalDofIndex> &globalDofs,
    std::vector<std::vector<LocalDof>> &localDofs) const {
  localDofs.resize(globalDofs.size());
  for (size_t i = 0; i < globalDofs.size(); ++i)
    localDofs[i] = m_global2localDofs[globalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    flatLocal2localDofs(const std::vector<FlatLocalDofIndex> &flatLocalDofs,
                        std::vector<LocalDof> &localDofs) const {
  localDofs.resize(flatLocalDofs.size());
  for (size_t i = 0; i < flatLocalDofs.size(); ++i)
    localDofs[i] = m_flatLocal2localDofs[flatLocalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getGlobalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  positions.resize(m_globalDofBoundingBoxes.size());
  for (size_t i = 0; i < m_globalDofBoundingBoxes.size(); ++i)
    acc(positions, i) = acc(m_globalDofBoundingBoxes, i).reference;
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                           "getFlatLocalDofPositions(): not implemented yet");
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getGlobalDofBoundingBoxes(std::vector<BoundingBox<CoordinateType>> &bboxes)
    const {
  bboxes = m_globalDofBoundingBoxes;
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofBoundingBoxes(
        std::vector<BoundingBox<CoordinateType>> &bboxes) const {
  throw std::runtime_error(
      "PiecewisePolynomialContinuousScalarSpace::"
      "getFlatLocalDofBoundingBoxes(): not implemented yet");
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getGlobalDofNormals(std::vector<Point3D<CoordinateType>> &normals) const {
  SpaceHelper<BasisFunctionType>::getGlobalDofNormals_defaultImplementation(
      *m_view, m_global2localDofs, normals);
}

template <typename BasisFunctionType>
void PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofNormals(std::vector<Point3D<CoordinateType>> &normals)
    const {
  throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                           "getFlatLocalDofNormals(): not implemented yet");
}

template <typename BasisFunctionType>
void
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::dumpClusterIds(
    const char *fileName,
    const std::vector<unsigned int> &clusterIdsOfDofs) const {
  dumpClusterIdsEx(fileName, clusterIdsOfDofs, GLOBAL_DOFS);
}

template <typename BasisFunctionType>
void
PiecewisePolynomialContinuousScalarSpace<BasisFunctionType>::dumpClusterIdsEx(
    const char *fileName, const std::vector<unsigned int> &clusterIdsOfDofs,
    DofType dofType) const {
  throw std::runtime_error("PiecewisePolynomialContinuousScalarSpace::"
                           "dumpClusterIdsEx(): not implemented yet");
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS(
    PiecewisePolynomialContinuousScalarSpace);

} // namespace Bempp


#include <opencv2/highgui.hpp>
#include <boost/foreach.hpp>
#include <boost/math/special_functions/round.hpp>

#include "HistogramOverlay.h"
#include "Frame.h"
#include "FrameMetadata.h"
#include "FrameMetadataFactory.h"
#include "Utils.h"
#include "Logger.h"
#include "AIPExceptions.h"

class HistogramOverlay::Detail
{

public:
	Detail(): mSpacing(0) {}

	~Detail() {}

	void setImgMetadata(framemetadata_sp& metadata)
	{
		mOutputMetadata = metadata;
		mOutputImg = Utils::getMatHeader(FrameMetadataFactory::downcast<RawImageMetadata>(metadata));
	}

	void resetHistMetadata()
	{
		mHistMetadata.reset();
		mSpacing = 0;
		contours.clear();
	}

	void setHistMetadata(framemetadata_sp& metadata)
	{
		auto arrayMetadata = FrameMetadataFactory::downcast<ArrayMetadata>(metadata);
		mHist = Utils::getMatHeader(1, arrayMetadata->getLength(), arrayMetadata->getType());
		mHistMetadata = metadata;
	}

	void overlayHistogram(frame_sp& imgFrame, frame_sp& histFrame)
	{
		mHist.data = (uchar *)histFrame->data();
		mOutputImg.data = (uchar *)imgFrame->data();

		double maxVal = 0;
		cv::minMaxLoc(mHist, 0, &maxVal, 0, 0);

		auto height = mOutputImg.rows;
		auto maxHeight = mOutputImg.rows/2;
		if (mSpacing == 0)
		{
			// spacing not set
			setSpacing();
		}

		vector<cv::Point>& contour = contours[0];
		for (int h = 0; h < mHist.rows; h++)
		{
			float binVal = mHist.at<float>(h, 0);
			int ypos = height;
			if (maxVal != 0)
			{
				ypos = height - cvRound(binVal * maxHeight / maxVal);
			}
			contour[h+1].y = ypos;
		}

		cv::drawContours(mOutputImg, contours, 0, cv::Scalar(255, 0, 0), cv::LINE_4, 8);
	}

	void setSpacing(int spacing = 0)
	{
		auto bins = mHist.rows;
		mSpacing = spacing;
		if (mSpacing == 0)
		{
			mSpacing = 100; // initial value
		}

		if (mSpacing*(bins+2) > 0.75*mOutputImg.cols)
		{
			return setSpacing(boost::math::iround(mSpacing*0.75));
		}

		if (mSpacing < 20)
		{
			auto msg = "Not Enough width to overlay histogram of size<" + to_string(bins) + ">";
			throw AIPException(AIP_PARAM_OUTOFRANGE, msg);
		}

		auto maxHeight = mOutputImg.rows / 2;
		if (maxHeight < 30)
		{
			auto msg = "Not Enough height to overlay histogram.";
			throw AIPException(AIP_PARAM_OUTOFRANGE, string(msg));
		}

		vector<cv::Point> contour;
		contour.push_back(cv::Point(mSpacing*(2), mOutputImg.rows));
		for (int h = 0; h < mHist.rows; h++)
		{
			contour.push_back(cv::Point(mSpacing*(h + 2), 0));
		}
		contour.push_back(cv::Point(mSpacing*(mHist.rows+1), mOutputImg.rows));

		contours.push_back(contour);
	}

	framemetadata_sp getOutputMetadata()
	{
		return mOutputMetadata;
	}

	bool shouldTriggerSOS()
	{
		return !mHistMetadata.get() || !mOutputMetadata.get() || !mHist.rows;
	}

private:
	cv::Mat mHist;
	cv::Mat mOutputImg;
	framemetadata_sp mOutputMetadata;
	framemetadata_sp mHistMetadata;
	vector<vector<cv::Point> > contours;
	int mSpacing;
};

HistogramOverlay::HistogramOverlay(HistogramOverlayProps _props):Module(TRANSFORM, "HistogramOverlay", _props)
{
	mDetail.reset(new Detail());
}

bool HistogramOverlay::validateInputPins()
{
	if (getNumberOfInputPins() > 2)
	{
		LOG_ERROR << "<" << getId() << ">::validateInputPins size is expected to be 2. Actual<" << getNumberOfInputPins() << ">";
		return false;
	}

	pair<string, framemetadata_sp> me; // map element
	auto inputMetadataByPin = getInputMetadata();
	BOOST_FOREACH(me, inputMetadataByPin) {
		FrameMetadata::FrameType frameType = me.second->getFrameType();
		if (frameType != FrameMetadata::RAW_IMAGE && frameType != FrameMetadata::ARRAY)
		{
			LOG_ERROR << "<" << getId() << ">::validateInputPins input frameType is expected to be RAW_IMAGE OR ARRAY. Actual<" << frameType << ">";
			return false;
		}
	}

	return true;
}

bool HistogramOverlay::validateOutputPins()
{
	if (getNumberOfOutputPins() != 1)
	{
		LOG_ERROR << "<" << getId() << ">::validateOutputPins size is expected to be 1. Actual<" << getNumberOfOutputPins() << ">";
		return false;
	}

	framemetadata_sp metadata = getFirstOutputMetadata();
	FrameMetadata::FrameType frameType = metadata->getFrameType();
	if (frameType != FrameMetadata::RAW_IMAGE)
	{
		LOG_ERROR << "<" << getId() << ">::validateOutputPins input frameType is expected to be RAW_IMAGE. Actual<" << frameType << ">";
		return false;
	}

	return true;
}

bool HistogramOverlay::validateInputOutputPins()
{
	if (getNumberOfInputPins() != 2)
	{
		LOG_ERROR << "<" << getId() << ">::validateInputPins size is expected to be 2. Actual<" << getNumberOfInputPins() << ">";
		return false;
	}

	return Module::validateInputOutputPins();
}

bool HistogramOverlay::init()
{
	if (!Module::init())
	{
		return false;
	}

	return true;
}

bool HistogramOverlay::term()
{
	return Module::term();
}

bool HistogramOverlay::process(frame_container& frames)
{
	auto imgFrame = getFrameByType(frames, FrameMetadata::RAW_IMAGE);
	auto histFrame = getFrameByType(frames, FrameMetadata::ARRAY);

	if (!imgFrame.get() || !histFrame.get())
	{
		LOG_INFO << "Image and histogram both are required. So skipping.<" << getId() << ">";
		return true;
	}

	// makeframe
	auto metadata = mDetail->getOutputMetadata();
	auto outFrame = makeFrame(metadata->getDataSize());

	// copy buffer
	memcpy(outFrame->data(), imgFrame->data(), metadata->getDataSize());

	mDetail->overlayHistogram(outFrame, histFrame);

	frames.insert(make_pair(getOutputPinIdByType(FrameMetadata::RAW_IMAGE), outFrame));
	send(frames);

	return true;
}

bool HistogramOverlay::processSOS(frame_sp& frame)
{
	auto metadata = frame->getMetadata();
	if (metadata->getFrameType() == FrameMetadata::RAW_IMAGE)
	{
		mDetail->setImgMetadata(metadata);
	}
	else if (metadata->getFrameType() == FrameMetadata::ARRAY)
	{
		mDetail->setHistMetadata(metadata);
	}

	return true;
}

bool HistogramOverlay::shouldTriggerSOS()
{
	return mDetail->shouldTriggerSOS();
}

bool HistogramOverlay::processEOS(string& pinId)
{
	mDetail->resetHistMetadata();
	return true;
}


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_LOCALOPERATOR_CODIM1_HH
#define DUNE_GDT_LOCALOPERATOR_CODIM1_HH

#include <utility>
#include <vector>
#include <type_traits>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/densematrix.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/functions/interfaces.hh>

#include "../localevaluation/interface.hh"
#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalOperator {

// forwards
template< class QuaternaryEvaluationImp >
class Codim1CouplingIntegral;

template< class BinaryEvaluationImp >
class Codim1BoundaryIntegral;

namespace internal {

template< class QuaternaryEvaluationImp >
class Codim1CouplingIntegralTraits
{
  static_assert(std::is_base_of< LocalEvaluation::Codim1Interface< typename QuaternaryEvaluationImp::Traits, 4 >,
                                 QuaternaryEvaluationImp >::value,
                "QuaternaryEvaluationImp has to be derived from LocalEvaluation::Codim1Interface< ..., 4 >!");
public:
  typedef Codim1CouplingIntegral< QuaternaryEvaluationImp > derived_type;
};

template< class BinaryEvaluationImp >
class Codim1BoundaryIntegralTraits
{
  static_assert(std::is_base_of< LocalEvaluation::Codim1Interface< typename BinaryEvaluationImp::Traits, 2 >,
                                 BinaryEvaluationImp >::value,
                "BinaryEvaluationImp has to be derived from LocalEvaluation::Codim1Interface< ..., 2 >!");
public:
  typedef Codim1BoundaryIntegral< BinaryEvaluationImp > derived_type;
};

} // namespace internal

template< class QuaternaryEvaluationImp >
class Codim1CouplingIntegral
    : public LocalOperator::Codim1CouplingInterface< internal::Codim1CouplingIntegralTraits< QuaternaryEvaluationImp > >
{
public:
  typedef internal::Codim1CouplingIntegralTraits< QuaternaryEvaluationImp > Traits;
  typedef QuaternaryEvaluationImp                                           QuaternaryEvaluationType;

private:
  static const size_t numTmpObjectsRequired_ = 4;

public:
  template< class... Args >
  explicit Codim1CouplingIntegral(Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(0)
  {}

  template< class... Args >
  explicit Codim1CouplingIntegral(const int over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(over_integrate)
  {}

  template< class... Args >
  explicit Codim1CouplingIntegral(const size_t over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(boost::numeric_cast< size_t >(over_integrate))
  {}

  size_t numTmpObjectsRequired() const
  {
    return numTmpObjectsRequired_;
  }

  template< class E, class N, class IntersectionType, class D, size_t d, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void apply(const Stuff::LocalfunctionSetInterface< E, D, d, R, rT, rCT >& entityTestBase,
             const Stuff::LocalfunctionSetInterface< E, D, d, R, rA, rCA >& entityAnsatzBase,
             const Stuff::LocalfunctionSetInterface< N, D, d, R, rT, rCT >& neighborTestBase,
             const Stuff::LocalfunctionSetInterface< N, D, d, R, rA, rCA >& neighborAnsatzBase,
             const IntersectionType& intersection,
             Dune::DynamicMatrix< R >& entityEntityRet,
             Dune::DynamicMatrix< R >& neighborNeighborRet,
             Dune::DynamicMatrix< R >& entityNeighborRet,
             Dune::DynamicMatrix< R >& neighborEntityRet,
             std::vector< Dune::DynamicMatrix< R > >& tmpLocalMatrices) const
  {
    // local inducing function
    const auto& entity = entityTestBase.entity();
    const auto localFunctionsEn = evaluation_.localFunctions(entity);
    const auto& neighbor = neighborTestBase.entity();
    const auto localFunctionsNe = evaluation_.localFunctions(neighbor);
    // quadrature
    const size_t integrand_order = evaluation_.order(localFunctionsEn, localFunctionsNe,
                                                     entityTestBase, entityAnsatzBase,
                                                     neighborTestBase, neighborAnsatzBase) + over_integrate_;
    const auto& faceQuadrature = QuadratureRules< D, d - 1 >::rule(intersection.type(),
                                                                   boost::numeric_cast< int >(integrand_order));
    // check matrices
    entityEntityRet *= 0.0;
    neighborNeighborRet *= 0.0;
    entityNeighborRet *= 0.0;
    neighborEntityRet *= 0.0;
    const size_t rowsEn = entityTestBase.size();
    const size_t colsEn = entityAnsatzBase.size();
    const size_t rowsNe = neighborTestBase.size();
    const size_t colsNe = neighborAnsatzBase.size();
    assert(entityEntityRet.rows() >= rowsEn);
    assert(entityEntityRet.cols() >= colsEn);
    assert(neighborNeighborRet.rows() >= rowsNe);
    assert(neighborNeighborRet.cols() >= colsNe);
    assert(entityNeighborRet.rows() >= rowsEn);
    assert(entityNeighborRet.cols() >= colsNe);
    assert(neighborEntityRet.rows() >= rowsEn);
    assert(neighborEntityRet.cols() >= colsEn);
    assert(tmpLocalMatrices.size() >= numTmpObjectsRequired_);
    auto& entityEntityVals = tmpLocalMatrices[0];
    auto& neighborNeighborVals = tmpLocalMatrices[1];
    auto& entityNeighborVals = tmpLocalMatrices[2];
    auto& neighborEntityVals = tmpLocalMatrices[3];
    // loop over all quadrature points
    for (auto quadPoint = faceQuadrature.begin(); quadPoint != faceQuadrature.end(); ++quadPoint) {
      const Dune::FieldVector< D, d - 1 > localPoint = quadPoint->position();
      const auto integrationFactor = intersection.geometry().integrationElement(localPoint);
      const auto quadratureWeight = quadPoint->weight();
      // evaluate local
      evaluation_.evaluate(localFunctionsEn, localFunctionsNe,
                           entityTestBase, entityAnsatzBase,
                           neighborTestBase, neighborAnsatzBase,
                           intersection, localPoint,
                           entityEntityVals,
                           neighborNeighborVals,
                           entityNeighborVals,
                           neighborEntityVals);
      // compute integral
      assert(entityEntityVals.rows() >= rowsEn);
      assert(entityEntityVals.cols() >= colsEn);
      assert(neighborNeighborVals.rows() >= rowsNe);
      assert(neighborNeighborVals.cols() >= colsNe);
      assert(entityNeighborVals.rows() >= rowsEn);
      assert(entityNeighborVals.cols() >= colsNe);
      assert(neighborEntityVals.rows() >= rowsEn);
      assert(neighborEntityVals.cols() >= colsEn);
      // loop over all entity test basis functions
      for (size_t ii = 0; ii < rowsEn; ++ii) {
        auto& entityEntityRetRow = entityEntityRet[ii];
        const auto& entityEntityValsRow = entityEntityVals[ii];
        auto& entityNeighborRetRow = entityNeighborRet[ii];
        const auto& entityNeighborValsRow = entityNeighborVals[ii];
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < colsEn; ++jj) {
          entityEntityRetRow[jj] += entityEntityValsRow[jj] * integrationFactor * quadratureWeight;
        } // loop over all entity ansatz basis functions
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < colsNe; ++jj) {
          entityNeighborRetRow[jj] += entityNeighborValsRow[jj] * integrationFactor * quadratureWeight;
        } // loop over all neighbor ansatz basis functions
      } // loop over all entity test basis functions
      // loop over all neighbor test basis functions
      for (size_t ii = 0; ii < rowsNe; ++ii) {
        auto& neighborNeighborRetRow = neighborNeighborRet[ii];
        const auto& neighborNeighborValsRow = neighborNeighborVals[ii];
        auto& neighborEntityRetRow = neighborEntityRet[ii];
        const auto& neighborEntityValsRow = neighborEntityVals[ii];
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < colsNe; ++jj) {
          neighborNeighborRetRow[jj] += neighborNeighborValsRow[jj] * integrationFactor * quadratureWeight;
        } // loop over all neighbor ansatz basis functions
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < colsEn; ++jj) {
          neighborEntityRetRow[jj] += neighborEntityValsRow[jj] * integrationFactor * quadratureWeight;
        } // loop over all entity ansatz basis functions
      } // loop over all neighbor test basis functions
    } // loop over all quadrature points
  } // void apply(...) const

private:
  const QuaternaryEvaluationType evaluation_;
  const size_t over_integrate_;
}; // class Codim1CouplingIntegral

template< class BinaryEvaluationImp >
class Codim1BoundaryIntegral
  : public LocalOperator::Codim1BoundaryInterface< internal::Codim1BoundaryIntegralTraits< BinaryEvaluationImp > >
{
public:
  typedef internal::Codim1BoundaryIntegralTraits< BinaryEvaluationImp > Traits;
  typedef BinaryEvaluationImp                                           BinaryEvaluationType;

private:
  static const size_t numTmpObjectsRequired_ = 1;

public:
  template< class... Args >
  Codim1BoundaryIntegral(Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(0)
  {}

  template< class... Args >
  Codim1BoundaryIntegral(const int over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(over_integrate)
  {}

  template< class... Args >
  Codim1BoundaryIntegral(const size_t over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(boost::numeric_cast< size_t >(over_integrate))
  {}

  size_t numTmpObjectsRequired() const
  {
    return numTmpObjectsRequired_;
  }

  template< class E, class IntersectionType, class D, size_t d, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void apply(const Stuff::LocalfunctionSetInterface< E, D, d, R, rT, rCT >& testBase,
             const Stuff::LocalfunctionSetInterface< E, D, d, R, rA, rCA >& ansatzBase,
             const IntersectionType& intersection,
             Dune::DynamicMatrix< R >& ret,
             std::vector< Dune::DynamicMatrix< R > >& tmpLocalMatrices) const
  {
    // local inducing function
    const auto& entity = testBase.entity();
    const auto localFunctions = evaluation_.localFunctions(entity);
    // quadrature
    typedef Dune::QuadratureRules< D, d - 1 > FaceQuadratureRules;
    typedef Dune::QuadratureRule< D, d - 1 > FaceQuadratureType;
    const auto integrand_order = evaluation_.order(localFunctions, testBase, ansatzBase) + over_integrate_;
    const FaceQuadratureType& faceQuadrature = FaceQuadratureRules::rule(intersection.type(),
                                                                         boost::numeric_cast< int >(integrand_order));
    // check matrix and tmp storage
    ret *= 0.0;
    const size_t rows = testBase.size();
    const size_t cols = ansatzBase.size();
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    assert(tmpLocalMatrices.size() >= numTmpObjectsRequired_);
    Dune::DynamicMatrix< R >& localMatrix = tmpLocalMatrices[0];
    // loop over all quadrature points
    for (auto quadPoint = faceQuadrature.begin(); quadPoint != faceQuadrature.end(); ++quadPoint) {
      const Dune::FieldVector< D, d - 1 > localPoint = quadPoint->position();
      const R integrationFactor = intersection.geometry().integrationElement(localPoint);
      const R quadratureWeight = quadPoint->weight();
      // evaluate local
      evaluation_.evaluate(localFunctions, testBase, ansatzBase, intersection, localPoint, localMatrix);
      // compute integral
      assert(localMatrix.rows() >= rows);
      assert(localMatrix.cols() >= cols);
      // loop over all test basis functions
      for (size_t ii = 0; ii < rows; ++ii) {
        auto& retRow = ret[ii];
        const auto& localMatrixRow = localMatrix[ii];
        // loop over all ansatz basis functions
        for (size_t jj = 0; jj < cols; ++jj) {
          retRow[jj] += localMatrixRow[jj] * integrationFactor * quadratureWeight;
        } // loop over all ansatz basis functions
      } // loop over all test basis functions
    } // loop over all quadrature points
  } // void apply(...) const

private:
  const BinaryEvaluationType evaluation_;
  const size_t over_integrate_;
}; // class Codim1BoundaryIntegral

} // namespace LocalOperator
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCALOPERATOR_CODIM1_HH


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_FUNCTION_DERIVATIVE_DETAIL_DERIVATIVE_HPP__
#define __OPTIMISATION2_FUNCTION_DERIVATIVE_DETAIL_DERIVATIVE_HPP__

#include <cmath>
#include <limits>

#include "../../trait/size.hpp"
#include "../function.hpp"
#include "../../container/container.hpp"
#include <boost/mpl/push_back.hpp>
#include <boost/fusion/mpl/push_back.hpp>
#include <boost/fusion/include/at_c.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/include/deref.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <libv/lma/ttt/traits/to_ref.hpp>
#include <libv/lma/ttt/fusion/at.hpp>

namespace lma
{
  namespace detail
  {
    //! ToKey allow Sequence to be an associative container
    template<class Key> struct ToKey { typedef Key type; };
    template<template<class,class> class Pair, class Key, class Value> struct ToKey < Pair<Key,Value> > { typedef Key type; };

    template<class _1, class N, class Tag> struct Jac
    {
     typedef  boost::fusion::pair<
                            typename ToKey<_1>::type,
                            typename lma::ContainerOption<
                                                          Tag,
                                                          lma::Size<N>::value,
                                                          lma::Size<typename ToKey<_1>::type>::value
                                                         >::Matrix> type;
    };
    template<class Tag, class F, class Sequence> struct JacobReturnType :
      boost::mpl::transform<
			      Sequence,
			      Jac<
				  mpl::_1,
				  v::numeric_tag<lma::Size<typename F::ErreurType>::value>,
				  Tag
				  >
			    > {};
  }//! eon

}//! eon

#endif


#include "extractgeometryfeature.h"

#include "../viz/viz.h"

#include <cstring>
#include <opencv/highgui.h>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2//core/core.hpp>
#include <boost/graph/graph_concepts.hpp>
#include<ctime>
using namespace geomeasurer;
int main()
{

   sensor::RangesCorrespondingtoposes rcps;
   std::string logfiledir="../geomeasurer/data/mit-cscail.log";

   rcps=io::fromlogfile(logfiledir);

    std::ofstream file_w;
  // file_w.open("../geomeasurer/data/groundtruth_intel", std::ios_base::out);

  for(int iter=0;iter<60;iter++)
  {

  sensor::rangeData ranges,ranges1;
  ranges=rcps[iter].second;

  std::cout<<ranges.angle_increment<<" "<<ranges.angle_min<<" "<<ranges.angle_max<<" "<<ranges.maxRange<<std::endl;
  std::cout<<ranges.ranges.size()<<std::endl;
  for(auto range:ranges.ranges)
  {
    std::cout<<range<<" ";
  }

    std::cout<<std::endl;
    extractGeometryFeature gfs(ranges);

    gfs.setRegionGrowRadius(0.4);

    std::time_t begin_t,end_t;
    begin_t=std::clock();
    featurePointSet featurePoints=gfs.extractgfs("IFAKLO");
    end_t=std::clock();
    std::cout<<"DALKO run time:"<<double(end_t-begin_t)/CLOCKS_PER_SEC<<std::endl;

    viz::imshowPCDWithKeypoints("dalko",ranges,featurePoints);

/*
    if (file_w.good())
    {
      int x;
      std::cout<<"请输入预期的特征点个数："<<std::endl;
      std::cin>>x;
      file_w<<x<<" ";
    }
    else{
     exit(0);
    }
    */

    }

}


#ifndef FRINGE_SBAS_H
#define FRINGE_SBAS_H

#include <iostream>
#include <vector>
#include <armadillo>

struct Scene
{
    std::string date;
    std::string bperpName;

    Scene(){};
    ~Scene(){};

    void print()
    {
        std::cout<< "Date: " << date << "\n";
        std::cout<< "Bperp: " << bperpName << "\n";
    }
};

struct Pair
{
    //Master date
    std::string masterDate;

    //Slave date
    std::string slaveDate;

    //Filename for interferogram / offsets
    std::string ifgName;

    //Filename for coherence / quality
    std::string cohName;

    //Threshold to apply to coherence / quality
    float threshold;

    //Scale factor to multiply input data with
    float scale;

    //Values that will be determined in C++ code
    //Reference offset to take out of observations before scaling
    float referenceOffset;

    //Time difference in years
    float deltaT;

    //Constructors / destructors
    Pair():threshold(0.0f), scale(1.0f){};
    ~Pair(){};

    void print()
    {
        std::cout << "Pair: " << masterDate << " - " << slaveDate << std::endl;
        std::cout << "Observation: " << ifgName << std::endl;

        if (!cohName.empty())
        {
            std::cout << "Quality:  " << cohName << std::endl;
            std::cout << "Threshold: " << threshold << std::endl;
        }

    };

};

struct sbasOptions
{
    //Number of pairs
    int nPairs;

    //All the pairs
    std::vector<Pair> pairs;

    //All the scenes
    std::vector<Scene> scenes;

    //Reference box
    std::vector<int> refbox;

    //Valid region
    std::vector<int> bbox;

    //Output directory
    std::string outDir;

    //Reference date
    std::string refDate;

    //Values that will be determined in C++ code
    //Number of unique images
    int nSAR;

    //Dates
    std::vector<std::string> dates;

    //DEM error related stuff
    bool estimateDEMError;
    double startingRange;
    double rangeSpacing;
    double wavelength;
    std::string incAngleFile;

    //Constructors/destructors
    sbasOptions(): refbox(4), bbox(4), estimateDEMError(false) {};
    ~sbasOptions(){};

    void print()
    {
        std::cout << "Number of pairs : " << nPairs << std::endl;
        std::cout << "Reference box: " << refbox[0] << " " << refbox[1]
                  << " " << refbox[2] << " " << refbox[3] << std::endl;

        std::cout << "Valid region: " << bbox[0] << " " << bbox[1] << " "
                  << bbox[2] << " " << bbox[3] << std::endl;

        std::cout << "Reference date: " << refDate << std::endl;

        std::cout << "Output directory: " << outDir << std::endl;
    }
};

struct Solution
{
    arma::fvec tsest;
    double temporalCorrelation;
    double zerr;
    double velocity;
    bool rewrap;
};

struct Inverter
{
    int nPairs;
    int nSAR;
    int refIndex;
    bool fullRank;

    //Connecitivity matrix related
    arma::fmat Jmat;
    arma::fmat Jmatinv;
    arma::fvec epochs;

    //DEM error / model fit related
    arma::fmat Gmat;
    arma::fmat Gmatinv;
    arma::fmat GmatinvTrans;
    arma::fmat Pmat;

    void setup(int nPair, int nsar, int order)
    {
        Jmat.resize(nPair, nsar);
        Jmat.zeros();

        epochs.resize(nsar);
        Gmat.resize(nsar, order+1);

        nPairs = nPair;
        nSAR = nsar;
    }

    void prepare()
    {
        //This should only be called once in the code

        //Setting up the SBAS matrix
        Jmat.shed_col(refIndex);
        fullRank = (arma::rank(Jmat) == (nSAR-1));

        if (!fullRank)
        {
            std::cout << "Warning :: Network graph is not connected \n";
            throw std::invalid_argument("Network graph is not connected");
        }
        else
        {
            std::cout << "Network graph is connected. \n";
            arma::mat Jmatdbl  = arma::conv_to<arma::mat>::from(Jmat);
            arma::mat Jmatdblinv = arma::pinv(Jmatdbl);

            Jmatinv = arma::conv_to<arma::fmat>::from(Jmatdblinv);
        }

        //Setting up Bperp matrix for DEM error
        Gmat.col(0).fill(1);
        for (int ii=1; ii<Gmat.n_cols; ii++)
        {
            Gmat.col(ii) = Gmat.col(ii-1) % epochs;
        }

        Gmat.shed_row(refIndex);

        if (arma::rank(Gmat) != Gmat.n_cols)
        {
            std::cout << "Warning :: Design matrix is not full rank for dem error inversion. \n";
            throw std::invalid_argument("Poor design matrix for DEM error estimation");
        }
        {
            arma::mat Dmat = arma::conv_to<arma::mat>::from(Gmat);
            arma::mat Dmatinv = arma::pinv(Dmat);
            Gmatinv = arma::conv_to<arma::fmat>::from(Dmatinv);
            GmatinvTrans = arma::trans(Gmatinv);
            Pmat = arma::conv_to<arma::fmat>::from(Dmat * Dmatinv);
        }

    };

    void solve(const arma::fvec &inphs, Solution* worker) const
    {
        worker->tsest = Jmatinv * inphs;

        std::complex<float> unit(0., 1.);
        std::complex<float> tcorr = arma::sum(arma::exp(unit*(inphs - Jmat * worker->tsest)));

        worker->temporalCorrelation = std::abs(tcorr)/(1.0*nPairs);
        worker->rewrap=false;

    };

    void estimateDEMError(const arma::fvec &bperpfact, Solution* worker) const
    {
        //This is the pseudo-inverse update part
        //Computation of pseudo-inverse without repeated matrix operations.
        arma::fvec Ax = Gmatinv * bperpfact;
        arma::fvec Px = Pmat * bperpfact;
        arma::fvec b(Gmatinv.n_cols);

        float alpha = arma::dot(bperpfact, Px);
        if (alpha < 1.0e-9)
        {
            float eta = arma::dot(Ax, Ax);
            b = GmatinvTrans * (Ax / (1.0 + eta));
        }
        else
        {
            b = Px/alpha;
        }

        worker->zerr = arma::dot(b, worker->tsest);
        arma::fvec model = Gmatinv * worker->tsest -  Ax * worker->zerr;
        worker->velocity = model(1);

        //Return cleaned up time-series
        worker->tsest = worker->tsest - bperpfact * worker->zerr;
    };

    void estimateVelocity(Solution *worker) const
    {
        arma::fvec model =Gmatinv * worker->tsest;
        worker->velocity = model(1);
    };

    Inverter(){};
    ~Inverter(){};
};

#endif


#include "util.h"
#include "bitcoinrpc.h"
#include "distribution.h"
#include "json/json_spirit_value.h"
#include "json/json_spirit_writer_template.h"
#include "json/json_spirit_utils.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace json_spirit;

void DividendDistributor::Distribute(double dDistributedAmount, double dMinimumPayout)
{
    if (mapBalance.size() == 0)
        throw runtime_error("The balance map is empty. There's not address to distribute dividends to.");

    BalanceMap mapRetainedBalance(mapBalance);
    bool bMustRedistribute = true;

    while (bMustRedistribute)
    {
        bMustRedistribute = false;

        dTotalDistributed = 0;
        vDistribution.clear();

        BalanceMap::iterator it;
        double dTotalBalance = 0;

        it = mapRetainedBalance.begin();
        while (it != mapRetainedBalance.end())
        {
            //Exclude the ReservedShares
            if(sReservedShares != it->first.ToString())
                dTotalBalance += it->second;
            it++;
        }

        it = mapRetainedBalance.begin();
        while (it != mapRetainedBalance.end())
        {
            //Exclude the ReservedShares
            if(sReservedShares == it->first.ToString())
            {
                Distribution distribution(it->first, it->second, 0);
                vDistribution.push_back(distribution);
                it++;
                continue;
            }
            double dDistributed = it->second * dDistributedAmount / dTotalBalance;
            if (dDistributed < dMinimumPayout)
            {
                mapRetainedBalance.erase(it++);
                bMustRedistribute = true;
                continue;
            }
            Distribution distribution(it->first, it->second, dDistributed);
            vDistribution.push_back(distribution);

            dTotalDistributed += dDistributed;

            it++;
        }
    }
    if (dTotalDistributed == 0)
        throw runtime_error("No address received dividends.");

}

void DividendDistributor::GenerateOutputs(int nTransactions, vector<Object> &vTransactionOuts) const
{
    if (nTransactions <= 0)
        throw runtime_error("Invalid transaction count");

    if (vDistribution.size() == 0)
        throw runtime_error("No address to distribute to");

    if (nTransactions > vDistribution.size())
        throw runtime_error("Output split in too many transactions");

    vTransactionOuts.assign(nTransactions, Object());
    int nTransactionIndex = 0;

    BOOST_FOREACH(const Distribution &distribution, vDistribution)
    {
        double amount = distribution.GetDividendAmount();
        string address = distribution.GetPeercoinAddress().ToString();
        Object &out = vTransactionOuts[nTransactionIndex];

        out.push_back(Pair(address, (double)amount));
        nTransactionIndex = (nTransactionIndex + 1) % nTransactions;
    }
}

int DividendDistributor::GetTransactionCount(int nMaxDistributionPerTransaction) const
{
    return 1 + vDistribution.size() / nMaxDistributionPerTransaction;
}

double GetMinimumDividendPayout()
{
    return boost::lexical_cast<double>(GetArg("-distributionminpayout", "0.01"));
}

DividendDistributor GenerateDistribution(const BalanceMap &mapBalance, double dAmount)
{
    double dMinPayout = GetMinimumDividendPayout();

    printf("Distributing %f bitcoins to %d addresses with a minimum payout of %f\n", dAmount, mapBalance.size(), dMinPayout);

    try {
        DividendDistributor distributor(mapBalance);
        distributor.Distribute(dAmount, dMinPayout);
        return distributor;
    }
    catch (runtime_error &e)
    {
        printf("Distribution failed: %s\n", e.what());
        throw;
    }
}

int GetMaximumDistributionPerTransaction()
{
    // As of 2014-02-22, Peercoin won't generate transactions larger than 100,000 bytes (MAX_BLOCK_SIZE_GEN/5)
    // https://github.com/ppcoin/ppcoin/blob/master/src/wallet.cpp#L1181
    // Each (non compressed) input takes 180 bytes, each output 34, and max 50 extra bytes
    // http://bitcoin.stackexchange.com/a/3011/9199
    // So 1000 outputs leaves room for about 350 inputs
    return GetArg("-maxdistributionpertransaction", 1000);
}

Array SendDistribution(const DividendDistributor &distributor)
{
    try {
        double dTotalDistributed = distributor.TotalDistributed();
        int nDistributionCount = distributor.DistributionCount();
        double dBalance = GetDistributionBalance();

        if (dTotalDistributed > dBalance)
            throw runtime_error("Not enough bitcoins available in distribution account");

        int nMaxDistributionPerTransaction = GetMaximumDistributionPerTransaction();
        printf("Maximum output per transaction: %d\n", nMaxDistributionPerTransaction);

        int nTransactions = distributor.GetTransactionCount(nMaxDistributionPerTransaction);

        printf("Will send %f bitcoins to %d addresses in %d transactions\n", dTotalDistributed, nDistributionCount, nTransactions);

        vector<Object> vOutputs;
        distributor.GenerateOutputs(nTransactions, vOutputs);

        int i = 0;
        BOOST_FOREACH(Object &output, vOutputs)
        {
            printf("Output %d:\n", i);
            printf("%s\n", write_string(Value(output), true).c_str());
            i++;
        }

        string sAccount = GetArg("-distributionaccount", "");

        i = 0;
        Array results;
        BOOST_FOREACH(Object &output, vOutputs)
        {
            Array sendmanyParams;
            sendmanyParams.push_back(sAccount);
            sendmanyParams.push_back(output);
            std::string result;
            printf("Sending output %d from account \"%s\"\n", i, sAccount.c_str());
            result = CallPeercoinRPC("sendmany", sendmanyParams);
            printf("Successfully sent output %d: %s\n", i, result.c_str());
            results.push_back(result);
            i++;
        }

        return results;
    }
    catch (runtime_error &e)
    {
        printf("Distribution failed: %s\n", e.what());
        throw;
    }
}

double GetDistributionBalance()
{
    string sAccount = GetArg("-distributionaccount", "");

    Array params;
    params.push_back(sAccount);
    return boost::lexical_cast<double>(Value(CallPeercoinRPC("getbalance", params)).get_str());
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NONE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NONE_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/any.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( none_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::fundamental_<A0>, X>
                          )
  {
    using sA0 =  bd::scalar_of_t<A0>;
    BOOST_FORCEINLINE logical<sA0> operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return !bs::any(a0);
    }
  };
} } }

#endif


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/// \file
/// \brief vxl estimate fundamental matrix implementation

#include "estimate_fundamental_matrix.h"

#include <vital/util/enum_converter.h>
#include <vital/types/feature.h>
#include <vital/vital_config.h>

#include <arrows/vxl/camera.h>
#include <arrows/mvg/epipolar_geometry.h>

#include <vgl/vgl_point_2d.h>
#include <Eigen/LU>

#include <vpgl/algo/vpgl_fm_compute_8_point.h>
#include <vpgl/algo/vpgl_fm_compute_7_point.h>

using namespace kwiver::vital;

namespace kwiver {
namespace arrows {
namespace vxl {

namespace {
  enum method_t { EST_7_POINT, EST_8_POINT };
}

/// Private implementation class
class estimate_fundamental_matrix::priv
{
public:
  /// Constructor
  priv()
  : precondition(true),
    method(EST_8_POINT)
  {
  }

  bool precondition;
  method_t method;
};

// Define the enum converter
ENUM_CONVERTER( method_converter, method_t,
                { "EST_7_POINT",   EST_7_POINT },
                { "EST_8_POINT",   EST_8_POINT }
)

/// Constructor
estimate_fundamental_matrix
::estimate_fundamental_matrix()
: d_(new priv)
{
}

/// Destructor
estimate_fundamental_matrix
::~estimate_fundamental_matrix()
{
}

/// Get this algorithm's \link vital::config_block configuration block \endlink
vital::config_block_sptr
estimate_fundamental_matrix
::get_configuration() const
{
  // get base config from base class
  vital::config_block_sptr config =
      vital::algo::estimate_fundamental_matrix::get_configuration();

  config->set_value("precondition", d_->precondition,
                    "If true, precondition the data before estimating the "
                    "fundamental matrix");

  config->set_value("method", method_converter().to_string( d_->method ),
                    "Fundamental matrix estimation method to use. "
                    "(Note: does not include RANSAC).  Choices are: "
                    + method_converter().element_name_string() );

  return config;
}

/// Set this algorithm's properties via a config block
void
estimate_fundamental_matrix
::set_configuration(vital::config_block_sptr config)
{

  d_->precondition = config->get_value<bool>("precondition",
                                             d_->precondition);

  d_->method = config->get_enum_value< method_converter >( "method",
                                                           d_->method );
}

/// Check that the algorithm's current configuration is valid
bool
estimate_fundamental_matrix
::check_configuration( VITAL_UNUSED vital::config_block_sptr config) const
{
  return true;
}

/// Estimate an essential matrix from corresponding points
fundamental_matrix_sptr
estimate_fundamental_matrix
::estimate(const std::vector<vector_2d>& pts1,
           const std::vector<vector_2d>& pts2,
           std::vector<bool>& inliers,
           double inlier_scale) const
{
  std::vector<vgl_homg_point_2d<double> > right_points, left_points;
  for(const vector_2d& v : pts1)
  {
    right_points.push_back(vgl_homg_point_2d<double>(v.x(), v.y()));
  }
  for(const vector_2d& v : pts2)
  {
    left_points.push_back(vgl_homg_point_2d<double>(v.x(), v.y()));
  }

  vpgl_fundamental_matrix<double> vfm;
  if( d_->method == EST_8_POINT )
  {
    vpgl_fm_compute_8_point fm_compute(d_->precondition);
    fm_compute.compute(right_points, left_points, vfm);
  }
  else
  {
    std::vector< vpgl_fundamental_matrix<double>* > vfms;
    vpgl_fm_compute_7_point fm_compute(d_->precondition);
    fm_compute.compute(right_points, left_points, vfms);
    // TODO use the multiple solutions in a RANSAC framework
    // For now, only keep the first solution
    vfm = *vfms[0];
    for(auto v : vfms)
    {
      delete v;
    }
  }

  matrix_3x3d F(vfm.get_matrix().data_block());
  F.transposeInPlace();

  fundamental_matrix_sptr fm(new fundamental_matrix_d(F));
  inliers = mvg::mark_fm_inliers(*fm, pts1, pts2, inlier_scale);
  return fm;
}

} // end namespace vxl
} // end namespace arrows
} // end namespace kwiver