/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/render/bsdf.h>
#include <mitsuba/hw/basicshader.h>
#include <mitsuba/core/warp.h>
#include <boost/algorithm/string.hpp>

MTS_NAMESPACE_BEGIN

/*!\plugin{ward}{Anisotropic Ward BRDF}
 * \order{15}
 * \parameters{
 *     \parameter{variant}{\String}{
 *         Determines the variant of the Ward model to use:
 *         \begin{enumerate}[(i)]
 *             \item \code{ward}: The original model by Ward \cite{Ward1992Measuring}
 *             --- suffers from energy loss at grazing angles.
 *             \item \code{ward-duer}: Corrected Ward model with lower energy loss
 *             at grazing angles \cite{Dur2006Improved}.
 *             Does not always conserve energy.
 *             \item \code{balanced}: Improved version of the \code{ward-duer}
 *             model with energy balance at all angles \cite{Geisler2010New}.
 *         \end{enumerate}
 *         Default: \texttt{balanced}
 *     }
 *     \parameter{alphaU, alphaV}{\Float\Or\Texture}{
 *         Specifies the anisotropic roughness values along the tangent and
 *         bitangent directions.
 *         \default{0.1}.
 *     }
 *     \parameter{specular\showbreak Reflectance}{\Spectrum\Or\Texture}{
 *         Specifies the weight of the specular reflectance component.\default{0.2}}
 *     \parameter{diffuse\showbreak Reflectance}{\Spectrum\Or\Texture}{
 *         Specifies the weight of the diffuse reflectance component\default{0.5}}
 * }
 * \renderings{
 *     \rendering{$\alpha_u=0.1,\ \alpha_v=0.3$}{bsdf_ward_01_03}
 *     \rendering{$\alpha_u=0.3,\ \alpha_v=0.1$}{bsdf_ward_03_01}
 * }

 * This plugin implements the anisotropic Ward reflectance model and
 * several extensions. They are described in the papers
 * \begin{enumerate}[(i)]
 *    \item ``Measuring and Modeling Anisotropic Reflection''
 *      by Greg Ward \cite{Ward1992Measuring}
 *    \item ``Notes on the Ward BRDF'' by Bruce Walter \cite{Walter2005Notes}
 *    \item ``An Improved Normalization for the Ward Reflectance Model''
 *      by Arne D\"ur \cite{Dur2006Improved}
 *    \item ``A New Ward BRDF Model with Bounded Albedo'' by
 *      Geisler-Moroder et al. \cite{Geisler2010New}
 * \end{enumerate}
 *
 * Like the Phong BRDF, the Ward model does not take the Fresnel reflectance
 * of the material into account. In an experimental study by Ngan et al.
 * \cite{Ngan2005Experimental}, the Ward model performed noticeably worse than
 * models based on microfacet theory.
 *
 * For this reason, it is usually preferable to switch to a microfacet model
 * that incorporates knowledge about the material's index of refraction. In Mitsuba,
 * two such alternatives to \pluginref{ward} are given by the plugins
 * \pluginref{roughconductor} and \pluginref{roughplastic} (depending on the
 * material type).
 *
 * When using this plugin, note that the diffuse and specular reflectance
 * components should add up to a value less than or equal to one (for each
 * color channel). Otherwise, they will automatically be scaled appropriately
 * to ensure energy conservation.
 */
class Ward : public BSDF {
public:
	/// Supported model types
	enum EModelVariant {
		/// The original Ward model
		EWard = 0,
		/// Ward model with correction by Arne Duer
		EWardDuer = 1,
		/// Energy-balanced Ward model
		EBalanced = 2
	};

	Ward(const Properties &props)
		: BSDF(props) {
		m_diffuseReflectance = new ConstantSpectrumTexture(
			props.getSpectrum("diffuseReflectance", Spectrum(0.5f)));
		m_specularReflectance = new ConstantSpectrumTexture(
			props.getSpectrum("specularReflectance", Spectrum(0.2f)));

		std::string type =
			boost::to_lower_copy(props.getString("variant", "balanced"));
		if (type == "ward")
			m_modelVariant = EWard;
		else if (type == "ward-duer")
			m_modelVariant = EWardDuer;
		else if (type == "balanced")
			m_modelVariant = EBalanced;
		else
			Log(EError, "Specified an invalid model type \"%s\", must be "
				"\"ward\", \"ward-duer\", or \"balanced\"!", type.c_str());

		Float alpha = props.getFloat("alpha", 0.1f),
			  alphaU = props.getFloat("alphaU", alpha),
			  alphaV = props.getFloat("alphaV", alpha);

		m_alphaU = new ConstantFloatTexture(alphaU);
		if (alphaU == alphaV)
			m_alphaV = m_alphaU;
		else
			m_alphaV = new ConstantFloatTexture(alphaV);
		m_specularSamplingWeight = 0.0f;
	}

	Ward(Stream *stream, InstanceManager *manager)
	 : BSDF(stream, manager) {
		m_modelVariant = (EModelVariant) stream->readUInt();
		m_diffuseReflectance = static_cast<Texture *>(manager->getInstance(stream));
		m_specularReflectance = static_cast<Texture *>(manager->getInstance(stream));
		m_alphaU = static_cast<Texture *>(manager->getInstance(stream));
		m_alphaV = static_cast<Texture *>(manager->getInstance(stream));

		configure();
	}

	void configure() {
		unsigned int extraFlags = 0;
		if (m_alphaU != m_alphaV)
			extraFlags |= EAnisotropic;

		m_components.clear();
		m_components.push_back(EGlossyReflection | EFrontSide | extraFlags
			| ((!m_specularReflectance->isConstant() || !m_alphaU->isConstant()
			  || !m_alphaV->isConstant()) ? ESpatiallyVarying : 0));
		m_components.push_back(EDiffuseReflection | EFrontSide | extraFlags
			| (m_diffuseReflectance->isConstant() ? 0 : ESpatiallyVarying));

		/* Verify the input parameters and fix them if necessary */
		std::pair<Texture *, Texture *> result = ensureEnergyConservation(
			m_specularReflectance, m_diffuseReflectance,
			"specularReflectance", "diffuseReflectance", 1.0f);
		m_specularReflectance = result.first;
		m_diffuseReflectance = result.second;

		/* Compute weights that steer samples towards
		   the specular or diffuse components */
		Float dAvg = m_diffuseReflectance->getAverage().getLuminance(),
			  sAvg = m_specularReflectance->getAverage().getLuminance();
		m_specularSamplingWeight = sAvg / (dAvg + sAvg);

		m_usesRayDifferentials =
			m_diffuseReflectance->usesRayDifferentials() ||
			m_specularReflectance->usesRayDifferentials() ||
			m_alphaU->usesRayDifferentials() ||
			m_alphaV->usesRayDifferentials();

		BSDF::configure();
	}

	Spectrum getDiffuseReflectance(const Intersection &its) const {
		return m_diffuseReflectance->eval(its);
	}

	Spectrum eval(const BSDFSamplingRecord &bRec, EMeasure measure) const {
		if (Frame::cosTheta(bRec.wi) <= 0 ||
			Frame::cosTheta(bRec.wo) <= 0 || measure != ESolidAngle)
			return Spectrum(0.0f);

		bool hasSpecular = (bRec.typeMask & EGlossyReflection)
				&& (bRec.component == -1 || bRec.component == 0);
		bool hasDiffuse  = (bRec.typeMask & EDiffuseReflection)
				&& (bRec.component == -1 || bRec.component == 1);

		Spectrum result(0.0f);
		if (hasSpecular) {
			Vector H = bRec.wi+bRec.wo;
			Float alphaU = m_alphaU->eval(bRec.its).average();
			Float alphaV = m_alphaV->eval(bRec.its).average();

			Float factor1 = 0.0f;
			switch (m_modelVariant) {
				case EWard:
					factor1 = 1.0f / (4.0f * M_PI * alphaU * alphaV *
						std::sqrt(Frame::cosTheta(bRec.wi)*Frame::cosTheta(bRec.wo)));
					break;
				case EWardDuer:
					factor1 = 1.0f / (4.0f * M_PI * alphaU * alphaV *
						Frame::cosTheta(bRec.wi)*Frame::cosTheta(bRec.wo));
					break;
				case EBalanced:
					factor1 = dot(H,H) / (M_PI * alphaU * alphaV
						* std::pow(Frame::cosTheta(H),4));
					break;
				default:
					Log(EError, "Unknown model type!");
			}

			Float factor2 = H.x / alphaU, factor3 = H.y / alphaV;
			Float exponent = -(factor2*factor2+factor3*factor3)/(H.z*H.z);
			Float specRef = factor1 * math::fastexp(exponent);
			/* Important to prevent numeric issues when evaluating the
			   sampling density of the Ward model in places where it takes
			   on miniscule values (Veach-MLT does this for instance) */
			if (specRef > 1e-10f)
				result += m_specularReflectance->eval(bRec.its) * specRef;
		}

		if (hasDiffuse)
			result += m_diffuseReflectance->eval(bRec.its) * INV_PI;

		return result * Frame::cosTheta(bRec.wo);
	}

	Float pdf(const BSDFSamplingRecord &bRec, EMeasure measure) const {
		if (Frame::cosTheta(bRec.wi) <= 0 ||
			Frame::cosTheta(bRec.wo) <= 0 || measure != ESolidAngle)
			return 0.0f;

		bool hasSpecular = (bRec.typeMask & EGlossyReflection)
				&& (bRec.component == -1 || bRec.component == 0);
		bool hasDiffuse  = (bRec.typeMask & EDiffuseReflection)
				&& (bRec.component == -1 || bRec.component == 1);

		Float diffuseProb = 0.0f, specProb = 0.0f;

		if (hasSpecular) {
			Float alphaU = m_alphaU->eval(bRec.its).average();
			Float alphaV = m_alphaV->eval(bRec.its).average();
			Vector H = normalize(bRec.wi+bRec.wo);
			Float factor1 = 1.0f / (4.0f * M_PI * alphaU * alphaV *
				dot(H, bRec.wi) * std::pow(Frame::cosTheta(H), 3));
			Float factor2 = H.x / alphaU, factor3 = H.y / alphaV;

			Float exponent = -(factor2*factor2+factor3*factor3)/(H.z*H.z);
			specProb = factor1 * math::fastexp(exponent);
		}

		if (hasDiffuse)
			diffuseProb = warp::squareToCosineHemispherePdf(bRec.wo);

		if (hasDiffuse && hasSpecular)
			return m_specularSamplingWeight * specProb +
				   (1-m_specularSamplingWeight) * diffuseProb;
		else if (hasDiffuse)
			return diffuseProb;
		else if (hasSpecular)
			return specProb;
		else
			return 0.0f;
	}

	inline Spectrum sample(BSDFSamplingRecord &bRec, Float &_pdf, const Point2 &_sample) const {
		Point2 sample(_sample);

		bool hasSpecular = (bRec.typeMask & EGlossyReflection)
				&& (bRec.component == -1 || bRec.component == 0);
		bool hasDiffuse  = (bRec.typeMask & EDiffuseReflection)
				&& (bRec.component == -1 || bRec.component == 1);

		if (!hasSpecular && !hasDiffuse)
			return Spectrum(0.0f);

		bool choseSpecular = hasSpecular;

		if (hasDiffuse && hasSpecular) {
			if (sample.x <= m_specularSamplingWeight) {
				sample.x /= m_specularSamplingWeight;
			} else {
				sample.x = (sample.x - m_specularSamplingWeight)
					/ (1-m_specularSamplingWeight);
				choseSpecular = false;
			}
		}

		if (choseSpecular) {
			Float alphaU = m_alphaU->eval(bRec.its).average();
			Float alphaV = m_alphaV->eval(bRec.its).average();

			Float phiH = std::atan(alphaV/alphaU
				* std::tan(2.0f * M_PI * sample.y));
			if (sample.y > 0.5f)
				phiH += M_PI;
			Float cosPhiH = std::cos(phiH);
			Float sinPhiH = math::safe_sqrt(1.0f-cosPhiH*cosPhiH);

			Float thetaH = std::atan(math::safe_sqrt(
				-math::fastlog(sample.x) / (
					(cosPhiH*cosPhiH) / (alphaU*alphaU) +
					(sinPhiH*sinPhiH) / (alphaV*alphaV)
			)));
			Vector H = sphericalDirection(thetaH, phiH);
			bRec.wo = H * (2.0f * dot(bRec.wi, H)) - bRec.wi;

			bRec.sampledComponent = 1;
			bRec.sampledType = EGlossyReflection;

			if (Frame::cosTheta(bRec.wo) <= 0.0f)
				return Spectrum(0.0f);
		} else {
			bRec.wo = warp::squareToCosineHemisphere(sample);
			bRec.sampledComponent = 0;
			bRec.sampledType = EDiffuseReflection;
		}
		bRec.eta = 1.0f;

		_pdf = pdf(bRec, ESolidAngle);

		if (_pdf == 0)
			return Spectrum(0.0f);
		else
			return eval(bRec, ESolidAngle) / _pdf;
	}

	Spectrum sample(BSDFSamplingRecord &bRec, const Point2 &sample) const {
		Float pdf;
		return Ward::sample(bRec, pdf, sample);
	}

	void addChild(const std::string &name, ConfigurableObject *child) {
		if (child->getClass()->derivesFrom(MTS_CLASS(Texture))) {
			if (name == "alphaU")
				m_alphaU = static_cast<Texture *>(child);
			else if (name == "alphaV")
				m_alphaV = static_cast<Texture *>(child);
			else if (name == "diffuseReflectance")
				m_diffuseReflectance = static_cast<Texture *>(child);
			else if (name == "specularReflectance")
				m_specularReflectance = static_cast<Texture *>(child);
			else
				BSDF::addChild(name, child);
		} else {
			BSDF::addChild(name, child);
		}
	}

	void serialize(Stream *stream, InstanceManager *manager) const {
		BSDF::serialize(stream, manager);

		stream->writeUInt(m_modelVariant);
		manager->serialize(stream, m_diffuseReflectance.get());
		manager->serialize(stream, m_specularReflectance.get());
		manager->serialize(stream, m_alphaU.get());
		manager->serialize(stream, m_alphaV.get());
	}

	Float getRoughness(const Intersection &its, int component) const {
		Assert(component == 0 || component == 1);

		if (component == 0)
			return 0.5f * (m_alphaU->eval(its).average()
				+ m_alphaV->eval(its).average());
		else
			return std::numeric_limits<Float>::infinity();
	}

	Shader *createShader(Renderer *renderer) const;

	std::string toString() const {
		std::ostringstream oss;
		oss << "Ward[" << endl
			<< "  id = \"" << getID() << "\"," << endl
			<< "  variant = ";

		switch (m_modelVariant) {
			case EWard: oss << "ward," << endl; break;
			case EWardDuer: oss << "wardDuer," << endl; break;
			case EBalanced: oss << "balanced," << endl; break;
			default: Log(EError, "Unknown model type!");
		}

		oss << "  diffuseReflectance = " << indent(m_diffuseReflectance->toString()) << "," << endl
			<< "  specularReflectance = " << indent(m_specularReflectance->toString()) << "," << endl
			<< "  specularSamplingWeight = " << m_specularSamplingWeight << "," << endl
			<< "  alphaU = " << indent(m_alphaU->toString()) << "," << endl
			<< "  alphaV = " << indent(m_alphaV->toString()) << endl
			<< "]";
		return oss.str();
	}

	MTS_DECLARE_CLASS()
private:
	EModelVariant m_modelVariant;
	ref<Texture> m_diffuseReflectance;
	ref<Texture> m_specularReflectance;
	ref<Texture> m_alphaU;
	ref<Texture> m_alphaV;
	Float m_specularSamplingWeight;
};

// ================ Hardware shader implementation ================

/**
 * GLSL port of the Ward shader. This version only implements the variant
 * with energy balance. When the roughness is lower than
 * \alpha < 0.2, the shader clamps it to 0.2 so that it will still perform
 * reasonably well in a VPL-based preview.
 */
class WardShader : public Shader {
public:
	WardShader(Renderer *renderer,
			const Texture *diffuseColor,
			const Texture *specularColor,
			const Texture *alphaU,
			const Texture *alphaV) : Shader(renderer, EBSDFShader),
			m_diffuseReflectance(diffuseColor),
			m_specularReflectance(specularColor),
			m_alphaU(alphaU), m_alphaV(alphaV) {
		m_diffuseReflectanceShader = renderer->registerShaderForResource(m_diffuseReflectance.get());
		m_specularReflectanceShader = renderer->registerShaderForResource(m_specularReflectance.get());
		m_alphaUShader = renderer->registerShaderForResource(m_alphaU.get());
		m_alphaVShader = renderer->registerShaderForResource(m_alphaV.get());
	}

	bool isComplete() const {
		return m_diffuseReflectanceShader.get() != NULL &&
			   m_specularReflectanceShader.get() != NULL &&
			   m_alphaU.get() != NULL &&
			   m_alphaV.get() != NULL;
	}

	void putDependencies(std::vector<Shader *> &deps) {
		deps.push_back(m_diffuseReflectanceShader.get());
		deps.push_back(m_specularReflectanceShader.get());
		deps.push_back(m_alphaUShader.get());
		deps.push_back(m_alphaVShader.get());
	}

	void cleanup(Renderer *renderer) {
		renderer->unregisterShaderForResource(m_diffuseReflectance.get());
		renderer->unregisterShaderForResource(m_specularReflectance.get());
		renderer->unregisterShaderForResource(m_alphaU.get());
		renderer->unregisterShaderForResource(m_alphaV.get());
	}

	void generateCode(std::ostringstream &oss,
			const std::string &evalName,
			const std::vector<std::string> &depNames) const {
		oss << "vec3 " << evalName << "(vec2 uv, vec3 wi, vec3 wo) {" << endl
			<< "    if (wi.z <= 0.0 || wo.z <= 0.0)" << endl
			<< "    	return vec3(0.0);" << endl
			<< "    vec3 H = wi + wo;" << endl
			<< "    float cosSqr = H.z * H.z;" << endl
			<< "    float alphaU = max(0.3, " << depNames[2] << "(uv)[0]);" << endl
			<< "    float alphaV = max(0.3, " << depNames[3] << "(uv)[0]);" << endl
			<< "    float factor1 = dot(H, H)/(3.1415*alphaU*alphaV*cosSqr*cosSqr);"  << endl
			<< "    float factor2 = H.x / alphaU, factor3 = H.y / alphaV;" << endl
			<< "    float exponent = -(factor2*factor2 + factor3*factor3)/(H.z*H.z);" << endl
			<< "    float specRef = factor1 * exp(exponent);" << endl
			<< "    return (" << depNames[0] << "(uv) * inv_pi" << endl
			<< "           + " << depNames[1] << "(uv) * specRef) * cosTheta(wo);" << endl
			<< "}" << endl
			<< "vec3 " << evalName << "_diffuse(vec2 uv, vec3 wi, vec3 wo) {" << endl
			<< "    if (wi.z <= 0.0 || wo.z <= 0.0)" << endl
			<< "    	return vec3(0.0);" << endl
			<< "    return " << depNames[0] << "(uv) * (inv_pi * cosTheta(wo));" << endl
			<< "}" << endl;
	}

	MTS_DECLARE_CLASS()
private:
	ref<const Texture> m_diffuseReflectance;
	ref<const Texture> m_specularReflectance;
	ref<const Texture> m_alphaU;
	ref<const Texture> m_alphaV;
	ref<Shader> m_diffuseReflectanceShader;
	ref<Shader> m_specularReflectanceShader;
	ref<Shader> m_alphaUShader;
	ref<Shader> m_alphaVShader;
};

Shader *Ward::createShader(Renderer *renderer) const {
	return new WardShader(renderer, m_diffuseReflectance.get(),
		m_specularReflectance.get(), m_alphaU.get(), m_alphaV.get());
}

MTS_IMPLEMENT_CLASS(WardShader, false, Shader)
MTS_IMPLEMENT_CLASS_S(Ward, false, BSDF);
MTS_EXPORT_PLUGIN(Ward, "Anisotropic Ward BRDF");
MTS_NAMESPACE_END


//Tencent is pleased to support the open source community by making FeatherCNN available.

//Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.

//Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
//in compliance with the License. You may obtain a copy of the License at
//
//https://opensource.org/licenses/BSD-3-Clause
//
//Unless required by applicable law or agreed to in writing, software distributed
//under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
//CONDITIONS OF ANY KIND, either express or implied. See the License for the
//specific language governing permissions and limitations under the License.

#include <booster/sgemv.h>

#include <assert.h>
#include <arm_neon.h>
#include <string.h>

template <bool fuseBias, bool fuseRelu>
void fully_connected_inference_direct(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float* bias_arr)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];
        if (fuseBias)
            sum += bias_arr[i];
        if (fuseRelu)
            sum = (sum > 0.f) ? sum : 0.f;
        z[i] = sum;
    }
}

template <bool fuseBias, bool fuseRelu>
void fully_connected_transpose_inference(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float* bias_arr)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        float32x4_t vBias = vld1q_f32(bias_arr + k * 8);
        float32x4_t vBias1 = vld1q_f32(bias_arr + k * 8 + 4);
        float32x4_t vZero = vdupq_n_f32(0.f);
        const float *yPtr = y + k * 8 * input_size;
        float32x4_t res = {0.0, 0.0, 0.0, 0.0};
        float32x4_t res1 = {0.0, 0.0, 0.0, 0.0};
        float32x4_t va, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            //          float32x4_t v1, v2;
            va = vld1q_f32(x + i);

            vb0 = vld1q_f32(yPtr);
            vb1 = vld1q_f32(yPtr + 4);
            vb2 = vld1q_f32(yPtr + 8);
            vb3 = vld1q_f32(yPtr + 12);
            vb4 = vld1q_f32(yPtr + 16);
            vb5 = vld1q_f32(yPtr + 20);
            vb6 = vld1q_f32(yPtr + 24);
            vb7 = vld1q_f32(yPtr + 28);

#if __aarch64__
            res = vfmaq_laneq_f32(res, vb0, va, 0);
            res1 = vfmaq_laneq_f32(res1, vb1, va, 0);
            res = vfmaq_laneq_f32(res, vb2, va, 1);
            res1 = vfmaq_laneq_f32(res1, vb3, va, 1);
            res = vfmaq_laneq_f32(res, vb4, va, 2);
            res1 = vfmaq_laneq_f32(res1, vb5, va, 2);
            res = vfmaq_laneq_f32(res, vb6, va, 3);
            res1 = vfmaq_laneq_f32(res1, vb7, va, 3);
#else
            res = vmlaq_f32(res, vb0, vld1q_dup_f32(x + i + 0));
            res1 = vmlaq_f32(res1, vb1, vld1q_dup_f32(x + i + 0));
            res = vmlaq_f32(res, vb2, vld1q_dup_f32(x + i + 1));
            res1 = vmlaq_f32(res1, vb3, vld1q_dup_f32(x + i + 1));
            res = vmlaq_f32(res, vb4, vld1q_dup_f32(x + i + 2));
            res1 = vmlaq_f32(res1, vb5, vld1q_dup_f32(x + i + 2));
            res = vmlaq_f32(res, vb6, vld1q_dup_f32(x + i + 3));
            res1 = vmlaq_f32(res1, vb7, vld1q_dup_f32(x + i + 3));
#endif
            yPtr += 32;
        }

        if (fuseBias)
        {
            res = vaddq_f32(res, vBias);
            res1 = vaddq_f32(res1, vBias1);
        }
        if (fuseRelu)
        {
            res = vmaxq_f32(res, vZero);
            res1 = vmaxq_f32(res1, vZero);
        }
        vst1q_f32((float32_t *)(z + 8 * k), res);
        vst1q_f32((float32_t *)(z + 8 * k + 4), res1);
    }
}

template void fully_connected_inference_direct<false, false>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_inference_direct<false,  true>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_inference_direct<true,  false>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_inference_direct<true,   true>(const int, const int, const float *, const float *, float *, const int, float*);

template void fully_connected_transpose_inference<false, false>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_transpose_inference<false,  true>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_transpose_inference<true,  false>(const int, const int, const float *, const float *, float *, const int, float*);
template void fully_connected_transpose_inference<true,   true>(const int, const int, const float *, const float *, float *, const int, float*);

#if 0
void fully_connected_inference_direct_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0.f;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];

        sum += biasArr[i];
        if (sum < 0.f) sum = 0.f;
        z[i] = sum;
    }
}

void fully_connected_transpose_inference_neon8_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        float *yPtr = y + k * 8 * input_size;
        const float32x4_t vzero = vdupq_n_f32(0.f);

        float32x4_t res  = vld1q_f32(biasArr + k * 8);
        float32x4_t res1 = vld1q_f32(biasArr + k * 8 + 4);

        float32x4_t va, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            va = vld1q_f32(x + i);

            vb0 = vld1q_f32(yPtr);
            vb1 = vld1q_f32(yPtr + 4);
            vb2 = vld1q_f32(yPtr + 8);
            vb3 = vld1q_f32(yPtr + 12);
            vb4 = vld1q_f32(yPtr + 16);
            vb5 = vld1q_f32(yPtr + 20);
            vb6 = vld1q_f32(yPtr + 24);
            vb7 = vld1q_f32(yPtr + 28);

#if __aarch64__
            res = vfmaq_laneq_f32(res, vb0, va, 0);
            res1 = vfmaq_laneq_f32(res1, vb1, va, 0);
            res = vfmaq_laneq_f32(res, vb2, va, 1);
            res1 = vfmaq_laneq_f32(res1, vb3, va, 1);
            res = vfmaq_laneq_f32(res, vb4, va, 2);
            res1 = vfmaq_laneq_f32(res1, vb5, va, 2);
            res = vfmaq_laneq_f32(res, vb6, va, 3);
            res1 = vfmaq_laneq_f32(res1, vb7, va, 3);
#else
            res = vmlaq_f32(res, vb0, vld1q_dup_f32(x + i + 0));
            res1 = vmlaq_f32(res1, vb1, vld1q_dup_f32(x + i + 0));
            res = vmlaq_f32(res, vb2, vld1q_dup_f32(x + i + 1));
            res1 = vmlaq_f32(res1, vb3, vld1q_dup_f32(x + i + 1));
            res = vmlaq_f32(res, vb4, vld1q_dup_f32(x + i + 2));
            res1 = vmlaq_f32(res1, vb5, vld1q_dup_f32(x + i + 2));
            res = vmlaq_f32(res, vb6, vld1q_dup_f32(x + i + 3));
            res1 = vmlaq_f32(res1, vb7, vld1q_dup_f32(x + i + 3));
#endif
            yPtr += 32;
        }

        //res  = vaddq_f32(res, vBias);
        //res1 = vaddq_f32(res, vBias1);

        res  = vmaxq_f32(res, vzero);
        res1 = vmaxq_f32(res1, vzero);

        vst1q_f32((float32_t *)(z + 8 * k), res);
        vst1q_f32((float32_t *)(z + 8 * k + 4), res1);
    }
}
/*
void fully_connected_transpose_inference_neon(int input_size, int output_size, float *x, float *y, float *z)
{
    assert(input_size %4==0);
    assert(output_size%4==0);
//#pragma omp parallel for num_threads(32) schedule(static)
    for(int k=0; k<output_size/4; k++)
    {
        float *yPtr = y + k*4*input_size;
        float32x4_t res = {0.0,0.0,0.0,0.0};

        for(int i=0; i<input_size; i+=4)
        {
            float32x4_t v1, v2;
            v2 = vld1q_f32(x + i);

#if __aarch64__
            v1 = vld1q_f32(yPtr);
            res = vfmaq_laneq_f32(res, v1, v2, 0);

            v1 = vld1q_f32(yPtr + 4);
            res = vfmaq_laneq_f32(res, v1, v2, 1);

            v1 = vld1q_f32(yPtr + 8);
            res = vfmaq_laneq_f32(res, v1, v2, 2);

            v1 = vld1q_f32(yPtr + 12);
            res = vfmaq_laneq_f32(res, v1, v2, 3);
#else
            v1 = vld1q_f32(yPtr);
            res = vmlaq_f32(res, v1, vld1q_dup_f32(x + i + 0));

            v1 = vld1q_f32(yPtr + 4);
            res = vmlaq_f32(res, v1, vld1q_dup_f32(x + i + 1));

            v1 = vld1q_f32(yPtr + 8);
            res = vmlaq_f32(res, v1, vld1q_dup_f32(x + i + 2));

            v1 = vld1q_f32(yPtr + 12);
            res = vmlaq_f32(res, v1, vld1q_dup_f32(x + i + 3));
#endif
            yPtr += 16;
        }
        vst1q_f32((float32_t *) (z+4*k), res);
    }
}
*/
#endif
void matrixTranspose(float* array, size_t m, size_t n, float *buffer)//  A[m][n] -> A[n][m]
{
    for (int i = 0; i < m; i++)    for (int j = 0; j < n; j++)
            buffer[j * m + i] = array[i * n + j];
    memcpy(array, buffer, m * n * sizeof(float));
}


#pragma once

// #include <any>
// #include <boost/coroutine2/all.hpp>
#include <cassert>
#include <py2cpp/py2cpp.hpp>
// #include <range/v3/view/enumerate.hpp>
#include <type_traits>
#include <utility>
#include <vector>
#include <xnetwork/classes/coreviews.hpp>  // import AtlasView, AdjacencyView
#include <xnetwork/classes/graph.hpp>
#include <xnetwork/classes/reportviews.hpp>  // import NodeView, EdgeView, DegreeView

#if __cplusplus > 201703L
#    include <cppcoro/generator.hpp>
#endif

namespace xnetwork {

    /** Base class for directed graphs.

        A DiGraphS stores nodes and edges with optional data, or attributes.

        DiGraphSs hold directed edges.  Self loops are allowed but multiple
        (parallel) edges are not.

        Nodes can be arbitrary (hashable) C++ objects with optional
        key/value attributes. By convention `None` is not used as a node.

        Edges are represented as links between nodes with optional
        key/value attributes.

        Parameters
        ----------
        node_container : input graph (optional, default: None)
            Data to initialize graph. If None (default) an empty
            graph is created.  The data can be any format that is supported
            by the to_networkx_graph() function, currently including edge list,
            dict of dicts, dict of lists, NetworkX graph, NumPy matrix
            or 2d ndarray, SciPy sparse matrix, or PyGraphviz graph.

        See Also
        --------
        Graph
        DiGraph
        MultiGraph
        MultiDiGraph
        OrderedGraph

        Examples
        --------
        Create an empty graph structure (a "null graph") with 5 nodes and
        no edges.

            > auto v = std::vector{3, 4, 2, 8};
            > auto G = xnetwork::DiGraphS(v);

            > auto va = py::dict{{3, 0.1}, {4, 0.5}, {2, 0.2}};
            > auto G = xnetwork::DiGraphS(va);

            > auto r = py::range(100);
            > auto G = xnetwork::DiGraphS(r);

        G can be grown in several ways.

        **Nodes:**

        Add one node at a time:

            > G.add_node(1)

        Add the nodes from any container (a list, dict, set or
        even the lines from a file or the nodes from another graph).

            > G.add_nodes_from([2, 3])
            > G.add_nodes_from(range(100, 110))
            > H = xnetwork::path_graph(10)
            > G.add_nodes_from(H)

        In addition to strings and integers any hashable C++ object
        (except None) can represent a node, e.g. a customized node object,
        or even another DiGraphS.

            > G.add_node(H)

        **Edges:**

        G can also be grown by adding edges.

        Add one edge,

            > G.add_edge(1, 2);

        a list of edges,

            > G.add_edges_from([(1, 2), (1, 3)]);

        or a collection of edges,

            > G.add_edges_from(H.edges());

        If some edges connect nodes not yet in the graph, the nodes
        are added automatically.  There are no errors when adding
        nodes or edges that already exist.

        **Attributes:**

        Each graph can hold key/value attribute pairs
        in an associated attribute dictionary (the keys must be hashable).
        By default these are empty, but can be added or changed using
        direct manipulation of the attribute
        dictionaries named graph, node and edge respectively.

            > G.graph["day"] = std::any("Friday");
        {'day': 'Friday'}

        **Subclasses (Advanced):**

        The DiGraphS class uses a container-of-container-of-container data
       structure. The outer dict (node_dict) holds adjacency information keyed by
       node. The next dict (adjlist_dict) represents the adjacency information and
       holds edge data keyed by neighbor.  The inner dict (edge_attr_dict)
       represents the edge data and holds edge attribute values keyed by attribute
       names.

        Each of these three dicts can be replaced in a subclass by a user defined
        dict-like object. In general, the dict-like features should be
        maintained but extra features can be added. To replace one of the
        dicts create a new graph class by changing the class(!) variable
        holding the factory for that dict-like structure. The variable names are
        node_dict_factory, node_attr_dict_factory, adjlist_inner_dict_factory,
        adjlist_outer_dict_factory, edge_attr_dict_factory and
       graph_attr_dict_factory.

        node_dict_factory : function, (default: dict)
            Factory function to be used to create the dict containing node
            attributes, keyed by node id.
            It should require no arguments and return a dict-like object

        node_attr_dict_factory: function, (default: dict)
            Factory function to be used to create the node attribute
            dict which holds attribute values keyed by attribute name.
            It should require no arguments and return a dict-like object

        adjlist_outer_dict_factory : function, (default: dict)
            Factory function to be used to create the outer-most dict
            in the data structure that holds adjacency info keyed by node.
            It should require no arguments and return a dict-like object.

        adjlist_inner_dict_factory : function, (default: dict)
            Factory function to be used to create the adjacency list
            dict which holds edge data keyed by neighbor.
            It should require no arguments and return a dict-like object

        edge_attr_dict_factory : function, (default: dict)
            Factory function to be used to create the edge attribute
            dict which holds attribute values keyed by attribute name.
            It should require no arguments and return a dict-like object.

        graph_attr_dict_factory : function, (default: dict)
            Factory function to be used to create the graph attribute
            dict which holds attribute values keyed by attribute name.
            It should require no arguments and return a dict-like object.

        Typically, if your extension doesn't impact the data structure all
        methods will inherit without issue except: `to_directed/to_undirected`.
        By default these methods create a DiGraph/DiGraphS class and you probably
        want them to create your extension of a DiGraph/DiGraphS. To facilitate
        this we define two class variables that you can set in your subclass.

        to_directed_class : callable, (default: DiGraph or MultiDiGraph)
            Class to create a new graph structure in the `to_directed` method.
            If `None`, a NetworkX class (DiGraph or MultiDiGraph) is used.

        to_undirected_class : callable, (default: DiGraphS or MultiGraph)
            Class to create a new graph structure in the `to_undirected` method.
            If `None`, a NetworkX class (DiGraphS or MultiGraph) is used.

        Examples
        --------

        Create a low memory graph class that effectively disallows edge
        attributes by using a single attribute dict for all edges.
        This reduces the memory used, but you lose edge attributes.

            > class ThinGraph(xnetwork::DiGraphS):
        ...     all_edge_dict = {'weight': 1}
        ...     def single_edge_dict(self):
        ...         return self.all_edge_dict
        ...     edge_attr_dict_factory = single_edge_dict
            > G = ThinGraph()
            > G.add_edge(2, 1)
            > G[2][1]
        {'weight': 1}
            > G.add_edge(2, 2)
            > G[2][1] is G[2][2]
        True

        Please see :mod:`~networkx.classes.ordered` for more examples of
        creating graph subclasses by overwriting the base class `dict` with
        a dictionary-like object.
    */
    template <typename nodeview_t, typename adjlist_t = py::dict<Value_type<nodeview_t>, int>,
              typename adjlist_outer_dict_factory = py::dict<Value_type<nodeview_t>, adjlist_t>>
    class DiGraphS : public Graph<nodeview_t, adjlist_t, adjlist_outer_dict_factory> {
        using _Base = Graph<nodeview_t, adjlist_t, adjlist_outer_dict_factory>;

      public:
        using Node = typename _Base::Node;  // luk
        using edge_t = std::pair<Node, Node>;
        // using graph_attr_dict_factory = typename _Base::graph_attr_dict_factory;
        // using adjlist_outer_dict_factory =
        //     typename _Base::adjlist_outer_dict_factory;
        using key_type = typename _Base::key_type;
        using value_type = typename _Base::value_type;

      public:
        adjlist_outer_dict_factory& _succ;  // successor

        /** Initialize a graph with edges, name, or graph attributes.

            Parameters
            ----------
            node_container : input nodes

            Examples
            --------
                > v = std::vector{5, 3, 2};
                > G = xnetwork::DiGraphS(v);  // or DiGraph, MultiGraph, MultiDiGraph, etc

                > r = py::range(100);
                > G = xnetwork::DiGraphS(r, r);  // or DiGraph, MultiGraph, MultiDiGraph,
           etc
        */
        explicit DiGraphS(const nodeview_t& Nodes) : _Base{Nodes}, _succ{_Base::_adj} {}

        explicit DiGraphS(uint32_t num_nodes) : _Base{num_nodes}, _succ{_Base::_adj} {}

        /** DiGraphS adjacency object holding the neighbors of each node.

            This object is a read-only dict-like structure with node keys
            and neighbor-dict values.  The neighbor-dict is keyed by neighbor
            to the edge-data-dict.  So `G.adj[3][2]['color'] = 'blue'` sets
            the color of the edge `(3, 2)` to `"blue"`.

            Iterating over G.adj behaves like a dict. Useful idioms include
            `for nbr, datadict in G.adj[n].items():`.

            The neighbor information is also provided by subscripting the graph.
            So `for nbr, foovalue in G[node].data('foo', default=1):` works.

            For directed graphs, `G.adj` holds outgoing (successor) info.
        */
        auto adj() const {
            using T = decltype(this->_succ);
            return AdjacencyView<T>(this->_succ);
        }

        /** Graph adjacency object holding the successors of each node.

            This object is a read-only dict-like structure with node keys
            and neighbor-dict values.  The neighbor-dict is keyed by neighbor
            to the edge-data-dict.  So `G.succ[3][2]['color'] = 'blue'` sets
            the color of the edge `(3, 2)` to `"blue"`.

            Iterating over G.succ behaves like a dict. Useful idioms include
            `for nbr, datadict in G.succ[n].items():`.  A data-view not provided
            by dicts also exists: `for nbr, foovalue in G.succ[node].data('foo'):`
            and a default can be set via a `default` argument to the `data` method.

            The neighbor information is also provided by subscripting the graph.
            So `for nbr, foovalue in G[node].data('foo', default=1):` works.

            For directed graphs, `G.adj` is identical to `G.succ`.
        */
        auto succ() const {
            using T = decltype(this->_succ);
            return AdjacencyView<T>(this->_succ);
        }

        /** Add an edge between u and v.

            The nodes u and v will be automatically added if (they are
            not already : the graph.

            Edge attributes can be specified with keywords or by directly
            accessing the edge"s attribute dictionary. See examples below.

            Parameters
            ----------
            u, v : nodes
                Nodes can be, for example, strings or numbers.
                Nodes must be hashable (and not None) C++ objects.

            See Also
            --------
            add_edges_from : add a collection of edges

            Notes
            -----
            Adding an edge that already exists updates the edge data.

            Many XNetwork algorithms designed for weighted graphs use
            an edge attribute (by default `weight`) to hold a numerical value.

            Examples
            --------
            The following all add the edge e=(1, 2) to graph G) {

                > G = xnetwork::DiGraphS()   // or DiGraph, MultiGraph, MultiDiGraph, etc
                > e = (1, 2);
                > G.add_edge(1, 2)           // explicit two-node form
                > G.add_edges_from([(1, 2)]);  // add edges from iterable container

            Associate data to edges using keywords) {

                > G.add_edge(1, 2);

            For non-string attribute keys, use subscript notation.

                > G.add_edge(1, 2);
                > G[1][2].update({0: 5});
                > G.edges()[1, 2].update({0: 5});
         */
        template <typename U = key_type> auto add_edge(const Node& u, const Node& v) ->
            typename std::enable_if<std::is_same<U, value_type>::value>::type {
            // auto [u, v] = u_of_edge, v_of_edge;
            // add nodes
            // assert(this->s->_node.contains(u));
            // assert(this->s->_node.contains(v));
            // add the edge
            // datadict = this->_adj[u].get(v, this->edge_attr_dict_factory());
            // datadict.update(attr);
            this->_succ[u].insert(v);
            // this->_prev[v].insert(u);
            // this->_num_of_edges += 1;
        }

        template <typename U = key_type> auto add_edge(const Node& u, const Node& v) ->
            typename std::enable_if<!std::is_same<U, value_type>::value>::type {
            // auto [u, v] = u_of_edge, v_of_edge;
            // add nodes
            // assert(this->s->_node.contains(u));
            // assert(this->s->_node.contains(v));
            // add the edge
            // datadict = this->_adj[u].get(v, this->edge_attr_dict_factory());
            // datadict.update(attr);
            using T = typename adjlist_t::mapped_type;
            auto data = this->_adj[u].get(v, T{});
            this->_succ[u][v] = data;
            // this->_prev[v][u] = data;
            // this->_num_of_edges += 1;
        }

        template <typename T> auto add_edge(const Node& u, const Node& v, const T& data) {
            // assert(this->s->_node.contains(u));
            // assert(this->s->_node.contains(v));
            this->_succ[u][v] = data;
            // this->_num_of_edges += 1;
        }

        template <typename C1, typename C2> auto add_edges_from(const C1& edges, const C2& data) {
            auto N = edges.size();
            for (auto i = 0U; i != N; ++i) {
                const auto& e = edges[i];
                this->add_edge(e.first, e.second, data[i]);
            }
        }

        /** Returns True if node u has successor v.

            This is true if graph has the edge u->v.
        */
        auto has_successor(const Node& u, const Node& v) -> bool {
            return this->_node.contains(u) && this->_succ[u].contains(v);
        }

        /** Returns an iterator over successor nodes of n.

            A successor of n is a node m such that there exists a directed
            edge from n to m.

            Parameters
            ----------
            n : node
               A node in the graph

            Raises
            -------
            NetworkXError
               If n is not in the graph.

            See Also
            --------
            predecessors

            Notes
            -----
            neighbors() and successors() are the same.
        */
        auto successors(const Node& n) -> auto& { return this->_succ[n]; }

        auto successors(const Node& n) const -> const auto& { return this->_succ[n]; }

        /** An OutEdgeView of the DiGraph as G.edges().

            edges(self, nbunch=None, data=False, default=None)

            The OutEdgeView provides set-like operations on the edge-tuples
            as well as edge attribute lookup. When called, it also provides
            an EdgeDataView object which allows control of access to edge
            attributes (but does not provide set-like operations).
            Hence, `G.edges()[u, v]['color']` provides the value of the color
            attribute for edge `(u, v)` while
            `for (u, v, c) in G.edges().data('color', default='red'):`
            iterates through all the edges yielding the color attribute
            with default `'red'` if no color attribute exists.

            Parameters
            ----------
            nbunch : single node, container, or all nodes (default= all nodes)
                The view will only report edges incident to these nodes.
            data : string or bool, optional (default=False)
                The edge attribute returned in 3-tuple (u, v, ddict[data]).
                If True, return edge attribute dict in 3-tuple (u, v, ddict).
                If False, return 2-tuple (u, v).
            default : value, optional (default=None)
                Value used for edges that don't have the requested attribute.
                Only relevant if data is not True or False.

            Returns
            -------
            edges : OutEdgeView
                A view of edge attributes, usually it iterates over (u, v)
                or (u, v, d) tuples of edges, but can also be used for
                attribute lookup as `edges[u, v]['foo']`.

            See Also
            --------
            in_edges, out_edges

            Notes
            -----
            Nodes in nbunch that are not in the graph will be (quietly) ignored.
            For directed graphs this returns the out-edges.

            Examples
            --------
                > G = nx.DiGraph()   # or MultiDiGraph, etc
                > nx.add_path(G, [0, 1, 2])
                > G.add_edge(2, 3, weight=5)
                > [e for e in G.edges()]
            [(0, 1), (1, 2), (2, 3)]
                > G.edges().data()  # default data is {} (empty dict)
            OutEdgeDataView([(0, 1, {}), (1, 2, {}), (2, 3, {'weight': 5})])
                > G.edges().data('weight', default=1)
            OutEdgeDataView([(0, 1, 1), (1, 2, 1), (2, 3, 5)])
                > G.edges()([0, 2])  # only edges incident to these nodes
            OutEdgeDataView([(0, 1), (2, 3)])
                > G.edges()(0)  # only edges incident to a single node (use G.adj[0]?)
            OutEdgeDataView([(0, 1)])

        */
        // using coro_t = boost::coroutines2::coroutine<edge_t>;
        // using pull_t = typename coro_t::pull_type;

        // /// @TODO: sync with networkx
        // auto edges() const -> pull_t {
        //     auto func = [&](typename coro_t::push_type& yield) {
        //         if constexpr (std::is_same_v<nodeview_t, decltype(py::range<uint32_t>(
        //                                                      uint32_t{}))>) {  // this->_succ???
        //             for (auto&& [n, nbrs] : py::enumerate(this->_adj)) {
        //                 for (auto&& nbr : nbrs) {
        //                     yield(edge_t{Node(n), Node(nbr)});
        //                 }
        //             }
        //         } else {
        //             for (auto&& [n, nbrs] : this->_adj.items()) {
        //                 for (auto&& nbr : nbrs) {
        //                     yield(edge_t{n, nbr});
        //                 }
        //             }
        //         }
        //     };

        //     return pull_t(func);
        // }

#if __cplusplus > 201703L
        auto edges() const -> cppcoro::generator<edge_t> {
            if constexpr (std::is_same_v<nodeview_t, decltype(py::range<uint32_t>(
                                                         uint32_t{}))>) {  // this->_succ???
                for (auto&& [n, nbrs] : py::enumerate(this->_adj)) {
                    for (auto&& nbr : nbrs) {
                        co_yield edge_t{Node(n), Node(nbr)};
                    }
                }
            } else {
                for (auto&& [n, nbrs] : this->_adj.items()) {
                    for (auto&& nbr : nbrs) {
                        co_yield edge_t{n, nbr};
                    }
                }
            }
        }
#endif

        // cppcoro::generator<edge_t> edges() const
        // {
        //     for (auto&& [n, nbrs] : this->_nodes_nbrs())
        //     {
        //         for (auto&& nbr : nbrs)
        //         {
        //             co_yield edge_t{Node(n), Node(nbr)};
        //         }
        //     }
        // }

        // auto edges() {
        //     return OutEdgeView(*this);
        // }

        // auto in_edges() {
        //     return InEdgeView(*this);
        // }

        auto degree(const Node& n) const { return this->_succ[n].size(); }

        /** Remove all nodes and edges from the graph.

            This also removes the name, and all graph, node, and edge attributes.

            Examples
            --------
                > G = xnetwork::path_graph(4);  // or DiGraph, MultiGraph, MultiDiGraph, etc
                > G.clear();
                > list(G.nodes);
            [];
                > list(G.edges());
            [];

        */
        auto clear() {
            this->_succ.clear();
            // this->_pred.clear()
            // this->_node.clear();
            this->graph.clear();
        }

        /** Return true if (graph is a multigraph, false otherwise. */
        auto is_multigraph() { return false; }

        /** Return true if (graph is directed, false otherwise. */
        auto is_directed() { return true; }
    };

    using SimpleDiGraphS = DiGraphS<decltype(py::range<uint32_t>(uint32_t{})),
                                    py::dict<uint32_t, int>, std::vector<py::dict<uint32_t, int>>>;

    // template <typename nodeview_t,
    //           typename adjlist_t> DiGraphS(int )
    // -> DiGraphS<decltype(py::range<int>(1)), py::set<int>>;

}  // namespace xnetwork


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

/**
 * \file
 * \brief core essential matrix template implementations
 */

#include "essential_matrix.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/SVD>

namespace kwiver {
namespace vital {

/// Compute the twisted pair rotation from the rotation and translation
rotation_d
essential_matrix
::twisted_rotation() const
{
  // The quaternion representation of a 180 degree rotation about
  // unit vector [X,Y,Z] is simply [X, Y, Z, 0]
  vector_3d t = this->translation();
  return rotation_d(vector_4d(t.x(), t.y(), t.z(), 0.0)) * this->rotation();
}

/// Construct from a provided matrix
template <typename T>
essential_matrix_<T>
::essential_matrix_( Eigen::Matrix<T,3,3> const &mat )
{
  const matrix_t W = (matrix_t() << T(0), T(-1), T(0),
                                    T(1), T( 0), T(0),
                                    T(0), T( 0), T(1)).finished();
  Eigen::JacobiSVD<matrix_t> svd(mat, Eigen::ComputeFullU |
                                      Eigen::ComputeFullV);
  const matrix_t& U = svd.matrixU();
  const matrix_t& V = svd.matrixV();
  trans_ = U.col(2);
  matrix_t R = U*W*V.transpose();
  if( R.determinant() < T(0) )
  {
    R *= T(-1);
  }
  rot_ = rotation_<T>(R);
}

/// Construct from a rotation and translation
template <typename T>
essential_matrix_<T>
::essential_matrix_( rotation_<T> const &rot,
                     vector_t const &trans )
  : rot_( rot ),
    trans_( trans.normalized() )
{
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
essential_matrix_<float>
::essential_matrix_( essential_matrix_<float> const &other )
  : rot_( other.rot_ ),
    trans_( other.trans_ )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
essential_matrix_<double>
::essential_matrix_( essential_matrix_<double> const &other )
  : rot_( other.rot_ ),
    trans_( other.trans_ )
{
}

/// Construct from a generic essential_matrix
template <typename T>
essential_matrix_<T>
::essential_matrix_( essential_matrix const &base )
  : rot_( static_cast<rotation_<T> >(base.rotation()) ),
    trans_( base.translation().template cast<T>() )
{
}

/// Construct from a generic essential_matrix -- double specialization
template <>
essential_matrix_<double>
::essential_matrix_( essential_matrix const &base )
  : rot_( base.rotation() ),
    trans_( base.translation() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
essential_matrix_sptr
essential_matrix_<T>
::clone() const
{
  return essential_matrix_sptr( new essential_matrix_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix
template <typename T>
Eigen::Matrix<double,3,3>
essential_matrix_<T>
::matrix() const
{
  return this->compute_matrix().template cast<double>();
}

/// Specialization for matrices with native double type
template <>
Eigen::Matrix<double,3,3>
essential_matrix_<double>
::matrix() const
{
  return this->compute_matrix();
}

/// Return the one of two possible 3D rotations that can parameterize E
template <typename T>
rotation_d
essential_matrix_<T>
::rotation() const
{
  return static_cast<rotation_d>(this->rot_);
}

/// Return the second possible rotation that can parameterize E
template <typename T>
rotation_d
essential_matrix_<T>
::twisted_rotation() const
{
  return static_cast<rotation_d>(this->compute_twisted_rotation());
}

/// Return a unit translation vector (up to a sign) that parameterizes E
template <typename T>
vector_3d
essential_matrix_<T>
::translation() const
{
  return this->trans_.template cast<double>();
}

/// Get the underlying matrix
template <typename T>
typename essential_matrix_<T>::matrix_t
essential_matrix_<T>
::compute_matrix() const
{
  matrix_t t_cross;
  t_cross << T(0), -trans_[2], trans_[1],
             trans_[2], T(0), -trans_[0],
            -trans_[1], trans_[0], T(0);
  return t_cross * matrix_t(rot_.matrix());
}

/// Compute the twisted pair rotation from the rotation and translation
template <typename T>
rotation_<T>
essential_matrix_<T>
::compute_twisted_rotation() const
{
  typedef Eigen::Matrix<T,4,1> vector_4;
  // The quaternion representation of a 180 degree rotation about
  // unit vector [X,Y,Z] is simply [X, Y, Z, 0]
  const vector_t& t = trans_;
  return rotation_<T>(vector_4(t.x(), t.y(), t.z(), T(0))) * rot_;
}

/// Get a const reference to the underlying rotation
template <typename T>
rotation_<T> const&
essential_matrix_<T>
::get_rotation() const
{
  return rot_;
}

/// Get a const reference to the underlying translation
template <typename T>
typename essential_matrix_<T>::vector_t const&
essential_matrix_<T>
::get_translation() const
{
  return trans_;
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// essential_matrix_<T> output stream operator
template <typename T>
std::ostream&
operator<<( std::ostream &s, essential_matrix_<T> const &e )
{
  s << e.compute_matrix();
  return s;
}

/// Output stream operator for \p essential_matrix instances
std::ostream&
operator<<( std::ostream &s, essential_matrix const &e )
{
  s << e.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_ESSENTIAL_MATRIX(T)                                 \
  template class essential_matrix_<T>;                                  \
  template VITAL_EXPORT std::ostream& operator<<( std::ostream &,       \
                        essential_matrix_<T> const & )

INSTANTIATE_ESSENTIAL_MATRIX(float);
INSTANTIATE_ESSENTIAL_MATRIX(double);
#undef INSTANTIATE_ESSENTIAL_MATRIX
/// \endcond

} } // end vital namespace


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:       EffGlobalMinimizer
//- Description: Implementation code for the EffGlobalMinimizer class
//- Owner:       Barron J Bichon, Vanderbilt University

#include "EffGlobalMinimizer.hpp"
#include "dakota_system_defs.hpp"
#include "dakota_data_io.hpp"
#include "NonDLHSSampling.hpp"
#include "RecastModel.hpp"
#include "DataFitSurrModel.hpp"
#include "DakotaApproximation.hpp"
#include "ProblemDescDB.hpp"
#include "DakotaGraphics.hpp"
#ifdef HAVE_NCSU
#include "NCSUOptimizer.hpp"
#endif
#include "DakotaModel.hpp"
#include "DakotaResponse.hpp"
#include "pecos_stat_util.hpp"
#include <boost/lexical_cast.hpp>

//#define DEBUG
//#define DEBUG_PLOTS

namespace Dakota {

EffGlobalMinimizer* EffGlobalMinimizer::effGlobalInstance(NULL);

// This constructor accepts a Model
EffGlobalMinimizer::
EffGlobalMinimizer(ProblemDescDB& problem_db, Model& model):
  SurrBasedMinimizer(problem_db, model, std::shared_ptr<TraitsBase>(new EffGlobalTraits())),
  setUpType("model"), dataOrder(1)
{
  bestVariablesArray.push_back(iteratedModel.current_variables().copy());

  // initialize augmented Lagrange multipliers
  size_t num_multipliers = numNonlinearEqConstraints;
  for (size_t i=0; i<numNonlinearIneqConstraints; i++) {
    if (origNonlinIneqLowerBnds[i] > -bigRealBoundSize) // g has a lower bound
      ++num_multipliers;
    if (origNonlinIneqUpperBnds[i] <  bigRealBoundSize) // g has an upper bound
      ++num_multipliers;
  }
  augLagrangeMult.resize(num_multipliers);
  augLagrangeMult = 0.;

  truthFnStar.resize(numFunctions);

  // Always build a global Gaussian process model.  No correction is needed.
  String approx_type = "global_kriging";
  if (probDescDB.get_short("method.nond.emulator") == GP_EMULATOR)
    approx_type = "global_gaussian";

  String sample_reuse = "none";
  UShortArray approx_order; // empty
  short corr_order = -1, corr_type = NO_CORRECTION;
  if (probDescDB.get_bool("method.derivative_usage")) {
    if (approx_type == "global_gaussian") {
      Cerr << "\nError: efficient_global does not support gaussian_process "
	   << "when derivatives present; use kriging instead." << std::endl;
      abort_handler(METHOD_ERROR);
    }
    if (iteratedModel.gradient_type() != "none") dataOrder |= 2;
    if (iteratedModel.hessian_type()  != "none") dataOrder |= 4;
  }
  int db_samples = probDescDB.get_int("method.samples");
  int samples = (db_samples > 0) ? db_samples :
    (numContinuousVars+1)*(numContinuousVars+2)/2;
  int lhs_seed = probDescDB.get_int("method.random_seed");
  unsigned short sample_type = SUBMETHOD_DEFAULT;
  String rng; // empty string: use default
  //int symbols = samples; // symbols needed for DDACE
  bool vary_pattern = false;// for consistency across any outer loop invocations
  // get point samples file
  const String& import_pts_file
    = probDescDB.get_string("method.import_build_points_file");
  if (!import_pts_file.empty())
    { samples = 0; sample_reuse = "all"; }

  Iterator dace_iterator;
  // The following uses on the fly derived ctor:
  dace_iterator.assign_rep(new NonDLHSSampling(iteratedModel, sample_type,
    samples, lhs_seed, rng, vary_pattern, ACTIVE_UNIFORM), false);
  // only use derivatives if the user requested and they are available
  ActiveSet dace_set = dace_iterator.active_set(); // copy
  dace_set.request_values(dataOrder);
  dace_iterator.active_set(dace_set);

  // Construct f-hat using a GP approximation for each response function over
  // the active/design vars (same view as iteratedModel: not the typical All
  // view for DACE).
  //const Variables& curr_vars = iteratedModel.current_variables();
  ActiveSet gp_set = iteratedModel.current_response().active_set(); // copy
  gp_set.request_values(1); // no surr deriv evals, but GP may be grad-enhanced
  fHatModel.assign_rep(new DataFitSurrModel(dace_iterator, iteratedModel,
    gp_set, approx_type, approx_order, corr_type, corr_order, dataOrder,
    outputLevel, sample_reuse, import_pts_file,
    probDescDB.get_ushort("method.import_build_format"),
    probDescDB.get_bool("method.import_build_active_only"),
    probDescDB.get_string("method.export_approx_points_file"),
    probDescDB.get_ushort("method.export_approx_format")), false);

  // Following this ctor, IteratorScheduler::init_iterator() initializes the
  // parallel configuration for EffGlobalMinimizer + iteratedModel using
  // EffGlobalMinimizer's maxEvalConcurrency.  During fHatModel construction
  // above, DataFitSurrModel::derived_init_communicators() initializes the
  // parallel config for dace_iterator + iteratedModel using dace_iterator's
  // maxEvalConcurrency.  The only iteratedModel concurrency currently exercised
  // is that used by dace_iterator within the initial GP construction, but the
  // EffGlobalMinimizer maxEvalConcurrency must still be set so as to avoid
  // parallel config errors resulting from avail_procs > max_concurrency within
  // IteratorScheduler::init_iterator().  A max of the local derivative
  // concurrency and the DACE concurrency is used for this purpose.
  maxEvalConcurrency = std::max(maxEvalConcurrency,
				dace_iterator.maximum_evaluation_concurrency());

  // Configure a RecastModel with one objective and no constraints using the
  // alternate minimalist constructor: the recast fn pointers are reset for
  // each level within the run fn.
  SizetArray recast_vars_comps_total; // default: empty; no change in size
  BitArray all_relax_di, all_relax_dr; // default: empty; no discrete relaxation
  short recast_resp_order = 1; // nongradient-based optimizers
  eifModel.assign_rep(
    new RecastModel(fHatModel, recast_vars_comps_total, all_relax_di,
		    all_relax_dr, 1, 0, 0, recast_resp_order), false);

  // must use alternate NoDB ctor chain
  int max_iterations = 10000, max_fn_evals = 50000;
  double min_box_size = 1.e-15, vol_box_size = 1.e-15;
#ifdef HAVE_NCSU
  approxSubProbMinimizer.assign_rep(new NCSUOptimizer(eifModel, max_iterations,
    max_fn_evals, min_box_size, vol_box_size), false);
#else
  Cerr << "NCSU DIRECT is not available to optimize the GP subproblems. "
       << "Aborting process." << std::endl;
  abort_handler(METHOD_ERROR);
#endif //HAVE_NCSU
}

/* This is an alternate constructor for instantiations on the fly
    using a Model but no ProblemDescDB.
EffGlobalMinimizer::
EffGlobalMinimizer(Model& model, int max_iter, int max_eval) :
  SurrBasedMinimizer(EFFICIENT_GLOBAL, model), setUpType("model")
{ maxIterations = max_iter; maxFunctionEvals = max_eval; }
*/

EffGlobalMinimizer::~EffGlobalMinimizer()
{ }

/*
// SurrBasedMinimizer implementation of derived_{init,set,free} is sufficient

void EffGlobalMinimizer::derived_init_communicators(ParLevLIter pl_iter)
{
  // iteratedModel is evaluated to add truth data (single evaluate())
  iteratedModel.init_communicators(pl_iter, maxEvalConcurrency);

  // approxSubProbMinimizer.init_communicators() recursion is currently
  // sufficient for fHatModel.  An additional fHatModel.init_communicators()
  // call would be motivated by special parallel usage of fHatModel below that
  // is not otherwise covered by the recursion.
  //fHatMaxConcurrency = maxEvalConcurrency; // local derivative concurrency
  //fHatModel.init_communicators(pl_iter, fHatMaxConcurrency);

  // approxSubProbMinimizer uses NoDBBaseConstructor, so no need to
  // manage DB list nodes at this level
  approxSubProbMinimizer.init_communicators(pl_iter);
}

void EffGlobalMinimizer::derived_free_communicators(ParLevLIter pl_iter)
{
  // deallocate communicators for DIRECT on eifModel
  approxSubProbMinimizer.free_communicators(pl_iter);

  // approxSubProbMinimizer.free_communicators() recursion is currently
  // sufficient for fHatModel.  An additional fHatModel.free_communicators()
  // call would be motivated by special parallel usage of fHatModel below that
  // is not otherwise covered by the recursion.
  //fHatModel.free_communicators(pl_iter, fHatMaxConcurrency);

  // iteratedModel is evaluated to add truth data (single evaluate())
  iteratedModel.free_communicators(pl_iter, maxEvalConcurrency);
}
*/

void EffGlobalMinimizer::core_run()
{
  if (setUpType=="model")
    minimize_surrogates_on_model();
  else if (setUpType=="user_functions") {
    //minimize_surrogates_on_user_functions();
    Cerr << "Error: bad setUpType in EffGlobalMinimizer::core_run()."
	 << std::endl;
    abort_handler(METHOD_ERROR);
  }
  else {
    Cerr << "Error: bad setUpType in EffGlobalMinimizer::core_run()."
	 << std::endl;
    abort_handler(METHOD_ERROR);
  }
}

void EffGlobalMinimizer::minimize_surrogates_on_model()
{
  //------------------------------------------------------------------
  //     Solve the problem.
  //------------------------------------------------------------------

  // set the object instance pointers for use within the static member fns
  EffGlobalMinimizer* prev_instance = effGlobalInstance;
  effGlobalInstance = this;

  // now that variables/labels/bounds/targets have flowed down at run-time from
  // any higher level recursions, propagate them up the instantiate-on-the-fly
  // Model recursion so that they are correct when they propagate back down.
  eifModel.update_from_subordinate_model(); // depth = max

  // Build initial GP once for all response functions
  fHatModel.build_approximation();

  // Iterate until EGO converges
  unsigned short eif_convergence_cntr = 0, dist_convergence_cntr = 0,
    eif_convergence_limit = 2, dist_convergence_limit = 1;
  globalIterCount = 0;
  bool approx_converged = false;
  convergenceTol = 1.e-12; Real dist_tol = 1.e-8;
  // Decided for now (10-25-2013) to have EGO take the maxIterations
  // as the default from minimizer, so it will be initialized as 100
  //  maxIterations  = 25*numContinuousVars;
  RealVector prev_cv_star;
  while (!approx_converged) {
    ++globalIterCount;

    // initialize EIF recast model
    Sizet2DArray vars_map, primary_resp_map(1), secondary_resp_map;
    BoolDequeArray nonlinear_resp_map(1, BoolDeque(numFunctions, true));
    primary_resp_map[0].resize(numFunctions);
    for (size_t i=0; i<numFunctions; i++)
      primary_resp_map[0][i] = i;
    RecastModel* eif_model_rep = (RecastModel*)eifModel.model_rep();
    eif_model_rep->init_maps(vars_map, false, NULL, NULL,
      primary_resp_map, secondary_resp_map, nonlinear_resp_map,
      EIF_objective_eval, NULL);

    // determine fnStar from among sample data
    get_best_sample();

    // Execute GLOBAL search and retrieve results
    Cout << "\n>>>>> Initiating global optimization\n";
    ParLevLIter pl_iter = methodPCIter->mi_parallel_level_iterator(miPLIndex);
    approxSubProbMinimizer.run(pl_iter);
    const Variables&  vars_star = approxSubProbMinimizer.variables_results();
    const RealVector& c_vars    = vars_star.continuous_variables();
    const Response&   resp_star = approxSubProbMinimizer.response_results();
    const Real&       eif_star  = resp_star.function_value(0);

    // Get expected value for output
    fHatModel.continuous_variables(c_vars);
    fHatModel.evaluate();
    const RealVector& mean = fHatModel.current_response().function_values();
    Real aug_lag = augmented_lagrangian_merit(mean,
      iteratedModel.primary_response_fn_sense(),
      iteratedModel.primary_response_fn_weights(), origNonlinIneqLowerBnds,
      origNonlinIneqUpperBnds, origNonlinEqTargets);

    Cout << "\nResults of EGO iteration:\nFinal point =\n";
    write_data(Cout, c_vars);
    Cout << "Expected Improvement    =\n                     "
	 << std::setw(write_precision+7) << -eif_star
	 << "\n                     " << std::setw(write_precision+7)
	 << aug_lag << " [merit]\n";

#ifdef DEBUG
    RealVector variance = fHatModel.approximation_variances(vars_star);
    RealVector ev = expected_violation(mean,variance);
    RealVector stdv(numFunctions);
    for (size_t i=0; i<numFunctions; i++)
      stdv[i] = std::sqrt(variance[i]);
    Cout << "\nexpected values    =\n" << mean
	 << "\nstandard deviation =\n" << stdv
	 << "\nexpected violation =\n" << ev << std::endl;
#endif //DEBUG

    // Check for convergence based on max EIF
    if ( -eif_star < convergenceTol )
      ++eif_convergence_cntr;

    // Check for convergence based in distance between successive points.
    // If the dist between successive points is very small, then there is
    // little value in updating the GP since the new training point will
    // essentially be the previous optimal point.

    Real dist_cstar = (prev_cv_star.empty()) ? DBL_MAX :
      rel_change_L2(c_vars, prev_cv_star);
    // update prev_cv_star
    copy_data(c_vars, prev_cv_star);
    if (dist_cstar < dist_tol)
      ++dist_convergence_cntr;

    // If DIRECT failed to find a point with EIF>0, it returns the
    //   center point as the optimal solution. EGO may have converged,
    //   but DIRECT may have just failed to find a point with a good
    //   EIF value. Adding this midpoint can alter the GPs enough to
    //   to allow DIRECT to find something useful, so we force
    //   max(EIF)<tol twice to make sure. Note that we cannot make
    //   this check more than 2 because it would cause EGO to add
    //   the center point more than once, which will damage the GPs.
    //   Unfortunately, when it happens the second time, it may still
    //   be that DIRECT failed and not that EGO converged.

#ifdef DEBUG
    Cout << "EGO Iteration " << globalIterCount << "\neif_star " << eif_star
	 << "\ndist_cstar "  << dist_cstar      << "\ndist_convergence_cntr "
	 << dist_convergence_cntr << '\n';
#endif //DEBUG

    if ( dist_convergence_cntr >= dist_convergence_limit ||
	 eif_convergence_cntr  >= eif_convergence_limit ||
	 globalIterCount       >= maxIterations )
      approx_converged = true;
    else {
      // Evaluate response_star_truth
      fHatModel.component_parallel_mode(TRUTH_MODEL);
      iteratedModel.continuous_variables(c_vars);
      ActiveSet set = iteratedModel.current_response().active_set();
      set.request_values(dataOrder);
      iteratedModel.evaluate(set);
      IntResponsePair resp_star_truth(iteratedModel.evaluation_id(),
				      iteratedModel.current_response());

      if (numNonlinearConstraints) {
	// Update the merit function parameters
	// Logic follows Conn, Gould, and Toint, section 14.4:
	const RealVector& fns_star_truth
	  = resp_star_truth.second.function_values();
	Real norm_cv_star = std::sqrt(constraint_violation(fns_star_truth, 0.));
	if (norm_cv_star < etaSequence)
	  update_augmented_lagrange_multipliers(fns_star_truth);
	else
	  update_penalty();
      }

      // Update the GP approximation
      fHatModel.append_approximation(vars_star, resp_star_truth, true);
    }

  } // end approx convergence while loop

  // Set best variables and response for use by strategy level.
  // c_vars, fmin contain the optimal design
  get_best_sample(); // pull optimal result from sample data
  bestVariablesArray.front().continuous_variables(varStar);
  bestResponseArray.front().function_values(truthFnStar);

  // restore in case of recursion
  effGlobalInstance = prev_instance;

#ifdef DEBUG_PLOTS
  // DEBUG - output set of samples used to build the GP
  // If problem is 2d, output a grid of points on the GP
  //   and truth (if requested)
  for (size_t i=0; i<numFunctions; i++) {
    std::string samsfile("ego_sams");
    std::string tag = "_" + boost::lexical_cast<std::string>(i+1) + ".out";
    samsfile += tag;
    std::ofstream samsOut(samsfile.c_str(),std::ios::out);
    samsOut << std::scientific;
    const Pecos::SurrogateData& gp_data = fHatModel.approximation_data(i);
    size_t num_data_pts = gp_data.size(), num_vars = fHatModel.cv();
    for (size_t j=0; j<num_data_pts; ++j) {
      samsOut << '\n';
      const RealVector& sams = gp_data.continuous_variables(j);
      for (size_t k=0; k<num_vars; k++)
	samsOut << std::setw(13) << sams[k] << ' ';
      samsOut << std::setw(13) << gp_data.response_function(j);
    }
    samsOut << std::endl;

    // Plotting the GP over a grid is intended for visualization and
    //   is therefore only available for 2D problems
    if (num_vars==2) {
      std::string gpfile("ego_gp");
      std::string varfile("ego_var");
      gpfile  += tag;
      varfile += tag;
      std::ofstream  gpOut(gpfile.c_str(),  std::ios::out);
      std::ofstream varOut(varfile.c_str(), std::ios::out);
      std::ofstream eifOut("ego_eif.out",   std::ios::out);
      gpOut  << std::scientific;
      varOut << std::scientific;
      eifOut << std::scientific;
      RealVector test_pt(2);
      const RealVector& lbnd = fHatModel.continuous_lower_bounds();
      const RealVector& ubnd = fHatModel.continuous_upper_bounds();
      Real interval0 = (ubnd[0]-lbnd[0])/100.,
	   interval1 = (ubnd[1]-lbnd[1])/100.;
      for (size_t j=0; j<101; j++){
	test_pt[0] = lbnd[0] + float(j)*interval0;
	for (size_t k=0; k<101; k++){
	  test_pt[1] = lbnd[1] + float(k)*interval1;

	  fHatModel.continuous_variables(test_pt);
	  fHatModel.evaluate();
	  const Response& gp_resp = fHatModel.current_response();
	  const RealVector& gp_fn = gp_resp.function_values();

	  gpOut << '\n' << std::setw(13) << test_pt[0] << ' ' << std::setw(13)
		<< test_pt[1] << ' ' << std::setw(13) << gp_fn[i];

	  RealVector variances
	    = fHatModel.approximation_variances(fHatModel.current_variables());

	  varOut << '\n' << std::setw(13) << test_pt[0] << ' ' << std::setw(13)
		 << test_pt[1] << ' ' << std::setw(13) << variances[i];

	  if (i==numFunctions-1) {
	    Real m = augmented_lagrangian_merit(gp_fn,
	      iteratedModel.primary_response_fn_sense(),
	      iteratedModel.primary_response_fn_weights(),
	      origNonlinIneqLowerBnds, origNonlinIneqUpperBnds,
	      origNonlinEqTargets);
	    RealVector merit(1);
	    merit[0] = m;

	    Real ei = expected_improvement(merit, test_pt);

	    eifOut << '\n' << std::setw(13) << test_pt[0] << ' '
		   << std::setw(13) << test_pt[1] << ' ' << std::setw(13) << ei;
	  }
	}
	gpOut  << std::endl;
	varOut << std::endl;
	if (i==numFunctions-1)
	  eifOut << std::endl;
      }
    }
  }
#endif //DEBUG_PLOTS
}

void EffGlobalMinimizer::
EIF_objective_eval(const Variables& sub_model_vars,
		   const Variables& recast_vars,
		   const Response& sub_model_response,
		   Response& recast_response)
{
  // Means are passed in, but must retrieve variance from the GP
  const RealVector& means = sub_model_response.function_values();
  const RealVector& variances
    = effGlobalInstance->fHatModel.approximation_variances(recast_vars);

  const ShortArray& recast_asv = recast_response.active_set_request_vector();
  if (recast_asv[0] & 1) { // return -EI since we are maximizing
    Real neg_ei = -effGlobalInstance->expected_improvement(means, variances);
    recast_response.function_value(neg_ei, 0);
  }
}

Real EffGlobalMinimizer::
expected_improvement(const RealVector& means, const RealVector& variances)
{
  Real mean = objective(means, iteratedModel.primary_response_fn_sense(),
			iteratedModel.primary_response_fn_weights()), stdv;

  if ( numNonlinearConstraints ) {
    // mean_M = mean_f + lambda*EV + r_p*EV*EV
    // stdv_M = stdv_f
    const RealVector& ev = expected_violation(means, variances);
    for (size_t i=0; i<numNonlinearConstraints; ++i)
      mean += augLagrangeMult[i]*ev[i] + penaltyParameter*ev[i]*ev[i];
    stdv = std::sqrt(variances[0]); // ***
  }
  else { // extend for NLS/MOO ***
    // mean_M = M(mu_f)
    // stdv_M = sqrt(var_f)
    stdv = std::sqrt(variances[0]); // *** sqrt(sum(variances(1:nUsrPrimaryFns))
  }

  // Calculate the expected improvement
  Real cdf, pdf;
  Real snv = (meritFnStar - mean); // standard normal variate
  if(std::fabs(snv)>=std::fabs(stdv)*50.0) {
    //this will trap the denominator=0.0 case even if numerator=0.0
    pdf=0.0;
    cdf=(snv>0.0)?1.0:0.0;
  }
  else{
    snv/=stdv;
    cdf = Pecos::NormalRandomVariable::std_cdf(snv);
    pdf = Pecos::NormalRandomVariable::std_pdf(snv);
  }

  Real ei  = (meritFnStar - mean)*cdf + stdv*pdf;

  return ei;
}

RealVector EffGlobalMinimizer::
expected_violation(const RealVector& means, const RealVector& variances)
{
  RealVector ev(numNonlinearConstraints);

  size_t i, cntr=0;
  // inequality constraints
  for (i=0; i<numNonlinearIneqConstraints; i++) {
    const Real& mean = means[numUserPrimaryFns+i];
    const Real& stdv = std::sqrt(variances[numUserPrimaryFns+i]);
    const Real& lbnd = origNonlinIneqLowerBnds[i];
    const Real& ubnd = origNonlinIneqUpperBnds[i];
    if (lbnd > -bigRealBoundSize) {
      Real cdf, pdf;
      Real snv = (lbnd-mean);
      if(std::fabs(snv)>=std::fabs(stdv)*50.0){
	pdf=0.0;
	cdf=(snv>0.0)?1.0:0.0;
      }
      else{
	snv/=stdv; //now snv is the standard normal variate
	cdf = Pecos::NormalRandomVariable::std_cdf(snv);
	pdf = Pecos::NormalRandomVariable::std_pdf(snv);
      }
      ev[cntr++] = (lbnd-mean)*cdf + stdv*pdf;
    }
    if (ubnd < bigRealBoundSize) {
      Real cdf, pdf;
      Real snv = (ubnd-mean);
      if(std::fabs(snv)>=std::fabs(stdv)*50.0){
	pdf=0.0;
	cdf=(snv>0.0)?1.0:0.0;
      }
      else{
	snv/=stdv;
	cdf = Pecos::NormalRandomVariable::std_cdf(snv);
	pdf = Pecos::NormalRandomVariable::std_pdf(snv);
      }
      ev[cntr++] = (mean-ubnd)*(1.-cdf) + stdv*pdf;
    }
  }
  // equality constraints
  for (i=0; i<numNonlinearEqConstraints; i++) {
    const Real& mean = means[numUserPrimaryFns+numNonlinearIneqConstraints+i];
    const Real& stdv
      = std::sqrt(variances[numUserPrimaryFns+numNonlinearIneqConstraints+i]);
    const Real& zbar = origNonlinEqTargets[i];
    Real cdf, pdf;
    Real snv = (zbar-mean);
    if(std::fabs(snv)*50.0>=std::fabs(stdv)) {
      pdf=0.0;
      cdf=(snv>=0.0)?1.0:0.0;
    }
    else{
      snv/=stdv;
      cdf = Pecos::NormalRandomVariable::std_cdf(snv);
      pdf = Pecos::NormalRandomVariable::std_pdf(snv);
    }
    ev[cntr++] = (zbar-mean)*(2.*cdf-1.) + 2.*stdv*pdf;
  }

  return ev;
}

void EffGlobalMinimizer::get_best_sample()
{
  // Pull the samples and responses from data used to build latest GP
  // to determine fnStar for use in the expected improvement function

  const Pecos::SurrogateData& gp_data_0 = fHatModel.approximation_data(0);

  size_t i, sam_star_idx = 0, num_data_pts = gp_data_0.points();
  Real fn, fn_star = DBL_MAX;
  for (i=0; i<num_data_pts; ++i) {
    const RealVector& sams = gp_data_0.continuous_variables(i);

    fHatModel.continuous_variables(sams);
    fHatModel.evaluate();
    const RealVector& f_hat = fHatModel.current_response().function_values();
    fn = augmented_lagrangian_merit(f_hat,
      iteratedModel.primary_response_fn_sense(),
      iteratedModel.primary_response_fn_weights(), origNonlinIneqLowerBnds,
      origNonlinIneqUpperBnds, origNonlinEqTargets);

    if (fn < fn_star) {
      copy_data(sams, varStar);
      sam_star_idx   = i;
      fn_star        = fn;
      meritFnStar    = fn;
      truthFnStar[0] = gp_data_0.response_function(i);
    }
  }

  // update truthFnStar with all additional primary/secondary fns corresponding
  // to lowest merit function value
  for (i=1; i<numFunctions; ++i)
    truthFnStar[i]
      = fHatModel.approximation_data(i).response_function(sam_star_idx);
}

void EffGlobalMinimizer::update_penalty()
{
  // Logic follows Conn, Gould, and Toint, section 14.4, step 3
  //   CGT use mu *= tau with tau = 0.01 ->   r_p *= 50
  //   Rodriguez, Renaud, Watson:             r_p *= 10
  //   Robinson, Willcox, Eldred, and Haimes: r_p *= 5
  penaltyParameter *= 10.;
  //penaltyParameter = std::min(penaltyParameter, 1.e+20); // cap the max penalty?
  Real mu = 1./2./penaltyParameter; // conversion between r_p and mu penalties
  etaSequence = eta*std::pow(mu, alphaEta);

#ifdef DEBUG
  Cout << "Penalty updated: " << penaltyParameter << '\n'
       << "eta updated:     " << etaSequence      << '\n'
       << "Augmented Lagrange multipliers:\n" << augLagrangeMult;
#endif
}

} // namespace Dakota