/// @file DeconvolverBasisFunction.tcc
/// @brief Class for a deconvolver based on CLEANing with basis functions.
/// @details This concrete class defines a deconvolver used to estimate an
/// image from a residual image, psf optionally using a weights image.
/// @ingroup Deconvolver
///
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution is free software: you can redistribute it
/// and/or modify it under the terms of the GNU General Public License as
/// published by the Free Software Foundation; either version 2 of the License,
/// or (at your option) any later version.
///
/// This program 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, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
///

// ASKAPsoft includes
#include <askap/AskapLogging.h>
#include <casacore/casa/aips.h>
#include <boost/shared_ptr.hpp>
#include <casacore/casa/Arrays/Array.h>
#include <casacore/casa/Arrays/MaskArrMath.h>
#include <casacore/casa/Arrays/ArrayMath.h>
#include <casacore/casa/Arrays/MatrixMath.h>
#include <casacore/scimath/Mathematics/MatrixMathLA.h>

// Local package includes
#include <measurementequation/SynthesisParamsHelper.h>
#include <deconvolution/DeconvolverBasisFunction.h>
#include <deconvolution/MultiScaleBasisFunction.h>

ASKAP_LOGGER(decbflogger, ".deconvolution.basisfunction");

namespace askap {

    namespace synthesis {

        /// @brief Class for a deconvolver based on the BasisFunction Clean
        /// @details This base class defines a deconvolver used to estimate an
        /// image from a residual image, psf optionally using a weights image.
        /// The template argument T is the type, and FT is the transform
        /// e.g. DeconvolverBasisFunction<Double, DComplex>
        /// @ingroup Deconvolver

        template<class T, class FT>
        DeconvolverBasisFunction<T, FT>::DeconvolverBasisFunction(Vector<Array<T> >& dirty,
                                                                  Vector<Array<T> >& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf),
                itsUseCrossTerms(true), itsDecouple(true),
                itsDecouplingAlgorithm("diagonal")
        {
        };

        template<class T, class FT>
        DeconvolverBasisFunction<T, FT>::DeconvolverBasisFunction(Array<T>& dirty,
                                                                  Array<T>& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf),
                itsUseCrossTerms(true), itsDecouple(true),
                itsDecouplingAlgorithm("diagonal")
        {
        };

        template<class T, class FT>
        DeconvolverBasisFunction<T, FT>::~DeconvolverBasisFunction()
        {
        };

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::setBasisFunction(boost::shared_ptr<BasisFunction<T> > bf)
        {
            itsBasisFunction = bf;
        };

        template<class T, class FT>
        boost::shared_ptr<BasisFunction<T> > DeconvolverBasisFunction<T, FT>::basisFunction()
        {
            return itsBasisFunction;
        };

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::configure(const LOFAR::ParameterSet& parset)
        {
            DeconvolverBase<T, FT>::configure(parset);

            // Make the basis function
            {
                std::vector<float> defaultScales(3);
                defaultScales[0] = 0.0;
                defaultScales[1] = 10.0;
                defaultScales[2] = 30.0;
                const std::vector<float> scales = parset.getFloatVector("scales", defaultScales);

                ASKAPLOG_INFO_STR(decbflogger, "Constructing Multiscale basis function with scales " << scales);
                const Bool orthogonal = parset.getBool("orthogonal", false);

                if (orthogonal) {
                    ASKAPLOG_DEBUG_STR(decbflogger, "Multiscale basis functions will be orthogonalised");
                }

                itsBasisFunction = BasisFunction<Float>::ShPtr(new MultiScaleBasisFunction<Float>(scales,
                                   orthogonal));
            }
            itsUseCrossTerms = parset.getBool("usecrossterms", true);

            if (itsUseCrossTerms) {
                ASKAPLOG_DEBUG_STR(decbflogger, "Will use crossterms in subtraction");
            }

            itsDecouplingAlgorithm = parset.getString("decouplingalgorithm", "diagonal");
        }

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::finalise()
        {
            this->updateResiduals(this->itsModel);

            const Array<T> ones(this->itsL1image(0).shape(), 1.0);
            const T l0Norm(sum(ones(abs(this->itsL1image(0)) > T(0.0))));
            const T l1Norm(sum(abs(this->itsL1image(0))));
            ASKAPLOG_INFO_STR(decbflogger, "L0 norm = " << l0Norm << ", L1 norm   = " << l1Norm
                                  << ", Flux = " << sum(this->model()));

            for (uInt scale = 0; scale < itsScaleFlux.nelements(); scale++) {
                ASKAPLOG_INFO_STR(decbflogger, "   Scale " << scale << " Flux = " << itsScaleFlux(scale));
            }

        }

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::initialise()
        {
            DeconvolverBase<T, FT>::initialise();

            ASKAPLOG_INFO_STR(decbflogger, "Initialising Basis Function deconvolver");

            Int psfWidth = this->model().shape()(0);
            IPosition subPsfShape(2, 0, 0);

            // Only use the specified psfWidth if it makes sense
            if ((this->control()->psfWidth() > 0) && (this->control()->psfWidth() < psfWidth)) {
                psfWidth = this->control()->psfWidth();
                ASKAPLOG_INFO_STR(decbflogger, "Using subregion of Psf : size " << psfWidth
                                      << " pixels");
                subPsfShape = IPosition(2, psfWidth, psfWidth);
            } else {
                subPsfShape = IPosition(2, this->model().shape()(0), this->model().shape()(1));
            }

            this->itsBasisFunction->initialise(this->model().shape());
            initialiseResidual();
            this->itsBasisFunction->initialise(subPsfShape);
            initialisePSF();

            if (this->itsDecouplingAlgorithm == "basis") {
                // Decoupling using inverse coupling matrix generate orthogonal basis functions
                ASKAPLOG_INFO_STR(decbflogger, "Decoupling using inverse coupling matrix generate orthogonal basis functions");
                const Matrix<Double> inverseCouplingMatrix(this->itsInverseCouplingMatrix.copy());
                this->itsBasisFunction->initialise(this->model().shape());
                itsBasisFunction->multiplyArray(inverseCouplingMatrix);
                initialiseResidual();
                this->itsBasisFunction->initialise(subPsfShape);
                itsBasisFunction->multiplyArray(inverseCouplingMatrix);
                this->itsBasisFunction->multiplyArray(inverseCouplingMatrix);
                initialisePSF();
                //  SynthesisParamsHelper::saveAsCasaImage("BasisFunctionAfterInverseDecoupling.tab",
                //                         this->itsBasisFunction->basisFunction());
                //  SynthesisParamsHelper::saveAsCasaImage("ResidualsAfterInverseDecoupling.tab",
                //                         this->itsResidualBasisFunction);
            } else if (this->itsDecouplingAlgorithm == "residuals") {
                // Decoupling using inverse coupling matrix applied to basis and residuals
                ASKAPLOG_INFO_STR(decbflogger, "Decoupling using inverse coupling matrix applied to basis and residuals");
                const Array<T> invBF(applyInverse(this->itsInverseCouplingMatrix, this->itsBasisFunction->basisFunction()));
                this->itsBasisFunction->basisFunction() = invBF.copy();

                const Array<T> invRes(applyInverse(this->itsInverseCouplingMatrix, this->itsResidualBasisFunction));
                this->itsResidualBasisFunction = invRes.copy();

                if (itsUseCrossTerms) {
                    ASKAPLOG_DEBUG_STR(decbflogger, "Overriding usecrossterms since it makes no sense in this case");
                    this->itsUseCrossTerms = false;
                }

                //  SynthesisParamsHelper::saveAsCasaImage("BasisFunctionAfterResidualsDecoupling.tab",
                //                         this->itsBasisFunction->basisFunction());
                //  SynthesisParamsHelper::saveAsCasaImage("ResidualsAfterResidualsDecoupling.tab",
                //                         this->itsResidualBasisFunction);
            } else if (this->itsDecouplingAlgorithm == "inverse") {
                // Correcting coupling at subtraction phase with inverse coupling matrix
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse coupling matrix");
            } else if (this->itsDecouplingAlgorithm == "sqrtdiagonal") {
                // Correcting coupling at subtraction phase with inverse diag(coupling matrix)
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse sqrt(diag(coupling matrix))");
            } else if (this->itsDecouplingAlgorithm == "diagonal") {
                // Correcting coupling at subtraction phase with inverse diag(coupling matrix)
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse diag(coupling matrix)");
            } else if (this->itsDecouplingAlgorithm == "psfscales") {
                // Correcting coupling at subtraction phase with inverse psfscales
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse psfscales");
            } else if (this->itsDecouplingAlgorithm == "sqrtpsfscales") {
                // Correcting coupling at subtraction phase with inverse psfscales
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse sqrt(psfscales)");
            } else {
                // Correcting coupling at subtraction phase with inverse diag(coupling matrix)
                ASKAPLOG_DEBUG_STR(decbflogger, "Correcting coupling at subtraction phase with inverse diag(coupling matrix)");
            }

            const uInt nScales(this->itsBasisFunction->numberBases());
            const IPosition l1Shape(3, this->model().shape()(0), this->model().shape()(1), nScales);

            this->itsL1image.resize(this->itsNumberTerms);
            this->itsL1image(0).resize(l1Shape);
            this->itsL1image(0).set(0.0);
        }

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::initialiseResidual()
        {

            ASKAPCHECK(this->itsBasisFunction, "Basis function not initialised");

            this->state()->resetInitialObjectiveFunction();

            ASKAPLOG_DEBUG_STR(decbflogger, "Calculating cache of images");

            ASKAPLOG_DEBUG_STR(decbflogger, "Shape of basis functions "
                                   << this->itsBasisFunction->basisFunction().shape());

            const IPosition stackShape(this->itsBasisFunction->basisFunction().shape());

            itsResidualBasisFunction.resize(stackShape);

            Cube<FT> basisFunctionFFT(this->itsBasisFunction->basisFunction().shape());
            casa::setReal(basisFunctionFFT, this->itsBasisFunction->basisFunction());
            scimath::fft2d(basisFunctionFFT, true);

            Array<FT> residualFFT(this->dirty().shape().nonDegenerate());
            residualFFT.set(FT(0.0));
            casa::setReal(residualFFT, this->dirty().nonDegenerate());
            scimath::fft2d(residualFFT, true);

            Array<FT> work(this->model().nonDegenerate().shape());
            ASKAPLOG_DEBUG_STR(decbflogger,
                               "Calculating convolutions of residual image with basis functions");

            for (uInt term = 0; term < this->itsBasisFunction->numberBases(); term++) {

                ASKAPASSERT(basisFunctionFFT.xyPlane(term).nonDegenerate().shape().conform(residualFFT.nonDegenerate().shape()));
                work = conj(basisFunctionFFT.xyPlane(term).nonDegenerate()) * residualFFT.nonDegenerate();
                scimath::fft2d(work, false);

                // basis function * residual
                ASKAPLOG_DEBUG_STR(decbflogger, "Basis function(" << term
                                       << ") * Residual: max = " << max(real(work))
                                       << " min = " << min(real(work)));

                Cube<T>(itsResidualBasisFunction).xyPlane(term) = real(work);

            }
        }

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::initialisePSF()
        {
            // For the psf convolutions, we only need a small part of the
            // basis functions so we recalculate for that size
            Int psfWidth = this->model(0).shape()(0);

            // Only use the specified psfWidth if it makes sense
            if ((this->control()->psfWidth() > 0) && (this->control()->psfWidth() < psfWidth)) {
                psfWidth = this->control()->psfWidth();
                ASKAPLOG_DEBUG_STR(decbflogger, "Using subregion of Psf : size " << psfWidth
                                       << " pixels");
            }

            IPosition subPsfShape(2, psfWidth, psfWidth);

            Array<FT> work(subPsfShape);

            ASKAPLOG_DEBUG_STR(decbflogger, "Shape of basis functions "
                                   << this->itsBasisFunction->basisFunction().shape());

            const IPosition stackShape(this->itsBasisFunction->basisFunction().shape());

            // Now transform the basis functions
            Cube<FT> basisFunctionFFT(this->itsBasisFunction->basisFunction().shape());
            casa::setReal(basisFunctionFFT, this->itsBasisFunction->basisFunction());
            scimath::fft2d(basisFunctionFFT, true);

            this->itsPSFBasisFunction.resize(stackShape);

            this->itsScaleFlux.resize(stackShape(2));
            this->itsScaleFlux.set(T(0));

            // Calculate XFR for the subsection only
            Array<FT> subXFR(subPsfShape);

            const uInt nx(this->psf().shape()(0));
            const uInt ny(this->psf().shape()(1));

            const IPosition subPsfStart(2, nx / 2 - psfWidth / 2, ny / 2 - psfWidth / 2);
            const IPosition subPsfEnd(2, nx / 2 + psfWidth / 2 - 1, ny / 2 + psfWidth / 2 - 1);
            const IPosition subPsfStride(2, 1, 1);

            Slicer subPsfSlicer(subPsfStart, subPsfEnd, subPsfStride, Slicer::endIsLast);
            casa::IPosition minPos;
            casa::IPosition maxPos;
            T minVal, maxVal;
            casa::minMax(minVal, maxVal, minPos, maxPos, this->psf(0).nonDegenerate()(subPsfSlicer));
            ASKAPLOG_DEBUG_STR(decbflogger, "Maximum of PSF(0) = " << maxVal << " at " << maxPos);
            ASKAPLOG_DEBUG_STR(decbflogger, "Minimum of PSF(0) = " << minVal << " at " << minPos);
            this->itsPeakPSFVal = maxVal;
            this->itsPeakPSFPos(0) = maxPos(0);
            this->itsPeakPSFPos(1) = maxPos(1);

            const IPosition subPsfPeak(2, this->itsPeakPSFPos(0), this->itsPeakPSFPos(1));
            ASKAPLOG_DEBUG_STR(decbflogger, "Peak of PSF subsection at  " << subPsfPeak);
            ASKAPLOG_DEBUG_STR(decbflogger, "Shape of PSF subsection is " << subPsfShape);

            casa::setReal(subXFR, this->psf().nonDegenerate()(subPsfSlicer));
            scimath::fft2d(subXFR, true);

            // Now we have all the ingredients to calculate the convolutions
            // of basis function with psf's, etc.
            ASKAPLOG_DEBUG_STR(decbflogger, "Calculating convolutions of Psfs with basis functions");
            itsPSFScales.resize(this->itsBasisFunction->numberBases());

            for (uInt term = 0; term < this->itsBasisFunction->numberBases(); term++) {
                // basis function * psf
                ASKAPASSERT(basisFunctionFFT.xyPlane(term).nonDegenerate().shape().conform(subXFR.shape()));
                work = conj(basisFunctionFFT.xyPlane(term).nonDegenerate()) * subXFR;
                scimath::fft2d(work, false);
                Cube<T>(this->itsPSFBasisFunction).xyPlane(term) = real(work);

                ASKAPLOG_DEBUG_STR(decbflogger, "Basis function(" << term << ") * PSF: max = " << max(real(work)) << " min = " << min(real(work)));

                itsPSFScales(term) = max(real(work));
            }

            ASKAPLOG_DEBUG_STR(decbflogger, "Calculating double convolutions of PSF with basis functions");
            const IPosition crossTermsShape(4, psfWidth, psfWidth,
                                            this->itsBasisFunction->numberBases(),
                                            this->itsBasisFunction->numberBases());
            ASKAPLOG_DEBUG_STR(decbflogger, "Shape of cross terms " << crossTermsShape);
            itsPSFCrossTerms.resize(crossTermsShape);
            IPosition crossTermsStart(4, 0);
            IPosition crossTermsEnd(crossTermsShape - 1);
            IPosition crossTermsStride(4, 1);

            Array<FT> crossTermsPSFFFT(crossTermsShape);
            crossTermsPSFFFT.set(T(0));

            for (uInt term = 0; term < this->itsBasisFunction->numberBases(); term++) {
                crossTermsStart(2) = term;
                crossTermsEnd(2) = term;

                for (uInt term1 = 0; term1 < this->itsBasisFunction->numberBases(); term1++) {
                    crossTermsStart(3) = term1;
                    crossTermsEnd(3) = term1;
                    casa::Slicer crossTermsSlicer(crossTermsStart, crossTermsEnd, crossTermsStride, Slicer::endIsLast);
                    crossTermsPSFFFT(crossTermsSlicer).nonDegenerate() =
                        basisFunctionFFT.xyPlane(term).nonDegenerate() *
                        conj(basisFunctionFFT.xyPlane(term1)).nonDegenerate() * subXFR;
                }

            }

            this->itsCouplingMatrix.resize(itsBasisFunction->numberBases(), itsBasisFunction->numberBases());
            scimath::fft2d(crossTermsPSFFFT, true);
            this->itsPSFCrossTerms = real(crossTermsPSFFFT) / T(crossTermsShape(0) * crossTermsShape(1));

            for (uInt term = 0; term < this->itsBasisFunction->numberBases(); term++) {
                crossTermsStart(2) = term;
                crossTermsEnd(2) = term;

                for (uInt term1 = 0; term1 < this->itsBasisFunction->numberBases(); term1++) {
                    crossTermsStart(3) = term1;
                    crossTermsEnd(3) = term1;
                    casa::Slicer crossTermsSlicer(crossTermsStart, crossTermsEnd, crossTermsStride, Slicer::endIsLast);
                    casa::IPosition minPos;
                    casa::IPosition maxPos;
                    T minVal, maxVal;
                    casa::minMax(minVal, maxVal, minPos, maxPos, this->itsPSFCrossTerms(crossTermsSlicer));
                    this->itsCouplingMatrix(term, term1) = Double(maxVal);
                }

                this->itsCouplingMatrix(term, term) += Double(this->control()->lambda());
            }

            ASKAPLOG_DEBUG_STR(decbflogger, "Coupling matrix " << this->itsCouplingMatrix);
            this->itsInverseCouplingMatrix.resize(this->itsCouplingMatrix.shape());
            invertSymPosDef(this->itsInverseCouplingMatrix, this->itsDetCouplingMatrix, this->itsCouplingMatrix);
            ASKAPLOG_DEBUG_STR(decbflogger, "Coupling matrix determinant " << this->itsDetCouplingMatrix);
            ASKAPLOG_DEBUG_STR(decbflogger, "Inverse coupling matrix " << this->itsInverseCouplingMatrix);
            // Checked that the inverse really is an inverse.
            Matrix<T> identity(this->itsCouplingMatrix.shape(), 0.0);
            const uInt nRows(this->itsCouplingMatrix.nrow());
            const uInt nCols(this->itsCouplingMatrix.ncolumn());

            for (uInt row = 0; row < nRows; row++) {
                for (uInt col = 0; col < nCols; col++) {
                    identity(row, col) = sum(this->itsCouplingMatrix.row(row) * this->itsInverseCouplingMatrix.column(col));
                }
            }

            ASKAPLOG_DEBUG_STR(decbflogger, "Coupling matrix * inverse " << identity);

            // Now look at coupling between adjacent scales: this works well if the
            // scales are ordered.
            for (uInt term = 0; term < this->itsBasisFunction->numberBases() - 1; term++) {
                double det = this->itsCouplingMatrix(term, term) * this->itsCouplingMatrix(term + 1, term + 1) -
                             this->itsCouplingMatrix(term, term + 1) * this->itsCouplingMatrix(term + 1, term);
                ASKAPLOG_DEBUG_STR(decbflogger, "Independence between scales " << term << " and "
                                       << term + 1 << " = " << det);
            }
        }

        template<class T, class FT>
        bool DeconvolverBasisFunction<T, FT>::deconvolve()
        {
            this->initialise();

            ASKAPLOG_INFO_STR(decbflogger, "Performing BasisFunction CLEAN for "
                                  << this->control()->targetIter() << " iterations");

            do {
                this->oneIteration();
                this->monitor()->monitor(*(this->state()));
                this->state()->incIter();
            } while (!this->control()->terminate(*(this->state())));

            ASKAPLOG_INFO_STR(decbflogger, "Performed BasisFunction CLEAN for "
                                  << this->state()->currentIter() << " iterations");

            ASKAPLOG_INFO_STR(decbflogger, this->control()->terminationString());

            this->finalise();
            return True;
        }

        // This contains the heart of the BasisFunction Clean algorithm
        // The residual image and psfs are intrinsically two dimensional
        // but are expanded by projection onto the basis functions
        template<class T, class FT>
        bool DeconvolverBasisFunction<T, FT>::oneIteration()
        {
            // Find peak in residual image cube. This cube is full sized.
            casa::IPosition minPos;
            casa::IPosition maxPos;
            T minVal(0.0), maxVal(0.0);
            // Here the weights image is used as a weight in the determination
            // of the maximum i.e. it finds the max in weight . residual. The values
            // returned are without the weight
            minMaxMaskedScales(minVal, maxVal, minPos, maxPos, this->itsResidualBasisFunction,
                               this->weight(0));
            casa::IPosition absPeakPos;

            if (abs(minVal) < abs(maxVal)) {
                absPeakPos = maxPos;
            } else {
                absPeakPos = minPos;
            }

            // Find the peak values for each scale. Set the stopping criterion
            // to be the maximum of the maxima. Here we use the weighted
            // value since that's what we are interested in.
            const uInt nScales(this->itsBasisFunction->numberBases());
            Vector<T> peakValues(nScales);
            IPosition peakPos(absPeakPos);
            // If we are using residual decoupling, we need to
            // couple the peakvalues
            // Apply the inverse of the sqrt(diagonal values) to get the peak values
            Vector<T> coupledPeakValues(nScales);

            for (uInt scale = 0; scale < nScales; scale++) {
                peakPos(2) = scale;
                coupledPeakValues(scale) = this->itsResidualBasisFunction(peakPos);
            }

            if (itsDecouplingAlgorithm == "residuals") {
                // This is a special case - the residuals are already decoupled.
                peakValues = coupledPeakValues.copy();
            } else if (itsDecouplingAlgorithm == "inverse") {
                peakValues = apply(this->itsInverseCouplingMatrix, coupledPeakValues);
            } else if (itsDecouplingAlgorithm == "diagonal") {
                for (uInt scale = 0; scale < nScales; scale++) {
                    peakPos(2) = scale;
                    peakValues(scale) = coupledPeakValues(scale)
                                        / (this->itsCouplingMatrix(scale, scale));
                }
            } else if (itsDecouplingAlgorithm == "sqrtdiagonal") {
                for (uInt scale = 0; scale < nScales; scale++) {
                    peakPos(2) = scale;
                    peakValues(scale) = coupledPeakValues(scale)
                                        / sqrt(this->itsCouplingMatrix(scale, scale));
                }
            } else if (itsDecouplingAlgorithm == "psfscales") {
                for (uInt scale = 0; scale < nScales; scale++) {
                    peakPos(2) = scale;
                    peakValues(scale) = coupledPeakValues(scale)
                                        / this->itsPSFScales(scale);
                }
            } else if (itsDecouplingAlgorithm == "sqrtpsfscales") {
                for (uInt scale = 0; scale < nScales; scale++) {
                    peakPos(2) = scale;
                    peakValues(scale) = coupledPeakValues(scale)
                                        / sqrt(this->itsPSFScales(scale));
                }
            } else {
                ASKAPTHROW(AskapError, "Unknown decoupling algorithm " << itsDecouplingAlgorithm);
            }

            uInt optimumScale(0);
            T absPeakVal(0.0);

            for (uInt scale = 0; scale < nScales; scale++) {
                if (abs(peakValues(scale)) > abs(absPeakVal)) {
                    absPeakVal = peakValues(scale);
                    optimumScale = scale;
                }
            }

            // If we decoupled by residuals we need to recouple before
            // subtracting from the residuals
            if (this->itsDecouplingAlgorithm == "residuals") {
                peakValues = apply(this->itsCouplingMatrix, peakValues);
            } else {
                // Only the peak is useful
                for (uInt scale = 0; scale < nScales; scale++) {
                    if (scale != optimumScale) {
                        peakValues(scale) = T(0.0);
                    }
                }
            }

            if (this->state()->initialObjectiveFunction() == 0.0) {
                this->state()->setInitialObjectiveFunction(abs(absPeakVal));
            }

            this->state()->setPeakResidual(abs(absPeakVal));
            this->state()->setObjectiveFunction(abs(absPeakVal));
            this->state()->setTotalFlux(sum(this->model()));

            const casa::IPosition residualShape(this->itsResidualBasisFunction.shape());
            const casa::IPosition psfShape(this->itsPSFBasisFunction.shape());

            const casa::uInt ndim(this->itsResidualBasisFunction.shape().size());

            casa::IPosition residualStart(ndim, 0), residualEnd(ndim, 0), residualStride(ndim, 1);
            casa::IPosition psfStart(ndim, 0), psfEnd(ndim, 0), psfStride(ndim, 1);
            casa::IPosition psfCrossTermsStart(ndim + 1, 0), psfCrossTermsEnd(ndim + 1, 0), psfCrossTermsStride(ndim + 1, 1);

            const casa::IPosition modelShape(this->model().shape());
            const casa::uInt modelNdim(this->model().shape().size());
            casa::IPosition modelStart(modelNdim, 0), modelEnd(modelNdim, 0), modelStride(modelNdim, 1);

            // Wrangle the start, end, and shape into consistent form. It took me
            // quite a while to figure this out (slow brain day) so it may be
            // that there are some edge cases for which it fails.

            for (uInt dim = 0; dim < 2; dim++) {
                residualStart(dim) = max(0, Int(absPeakPos(dim) - psfShape(dim) / 2));
                residualEnd(dim) = min(Int(absPeakPos(dim) + psfShape(dim) / 2 - 1), Int(residualShape(dim) - 1));
                // Now we have to deal with the PSF. Here we want to use enough of the
                // PSF to clean the residual image.
                psfStart(dim) = max(0, Int(this->itsPeakPSFPos(dim) - (absPeakPos(dim) - residualStart(dim))));
                psfEnd(dim) = min(Int(this->itsPeakPSFPos(dim) - (absPeakPos(dim) - residualEnd(dim))),
                                  Int(psfShape(dim) - 1));

                psfCrossTermsStart(dim) = psfStart(dim);
                psfCrossTermsEnd(dim) = psfEnd(dim);

                modelStart(dim) = residualStart(dim);
                modelEnd(dim) = residualEnd(dim);
            }

            casa::Slicer modelSlicer(modelStart, modelEnd, modelStride, Slicer::endIsLast);

            // Add to model
            // Note that the model is only two dimensional. We could make it three dimensional
            // and keep the model layers separate
            // We loop over all terms and ignore those with no flux
            const casa::uInt nterms(this->itsResidualBasisFunction.shape()(2));

            for (uInt term = 0; term < nterms; term++) {
                if (abs(peakValues(term)) > 0.0) {
                    psfStart(2) = psfEnd(2) = term;
                    casa::Slicer psfSlicer(psfStart, psfEnd, psfStride, Slicer::endIsLast);
                    typename casa::Array<T> modelSlice = this->model()(modelSlicer).nonDegenerate();
                    modelSlice += this->control()->gain() * peakValues(term) *
                                  this->itsBasisFunction->basisFunction()(psfSlicer).nonDegenerate();
                }
            }

            // Keep track of strengths and locations of components
            for (uInt term = 0; term < nterms; term++) {
                if (abs(peakValues(term)) > 0.0) {
                    IPosition l1PeakPos(3, absPeakPos(0), absPeakPos(1), term);
                    casa::Slicer modelSlicer(modelStart, modelEnd, modelStride, Slicer::endIsLast);
                    this->itsL1image(0)(l1PeakPos) += this->control()->gain() * abs(peakValues(term));
                    this->itsScaleFlux(term) += this->control()->gain() * peakValues(term);
                }
            }

            // Subtract PSFs
            for (uInt term = 0; term < nterms; term++) {
                if (abs(peakValues(term)) > 0.0) {
                    psfStart(2) = psfEnd(2) = term;
                    casa::Slicer psfSlicer(psfStart, psfEnd, psfStride, Slicer::endIsLast);
                    residualStart(2) = residualEnd(2) = term;
                    casa::Slicer residualSlicer(residualStart, residualEnd, residualStride, Slicer::endIsLast);
                    typename casa::Array<T> residualBFSlice = this->itsResidualBasisFunction(residualSlicer).nonDegenerate();
                    residualBFSlice -= this->control()->gain() * peakValues(term) * this->itsPSFBasisFunction(psfSlicer).nonDegenerate();
                }
            }

            if (itsUseCrossTerms) {
                for (uInt term1 = 0; term1 < nterms; term1++) {
                    if (abs(peakValues(term1)) > 0.0) {
                        for (uInt term = 0; term < nterms; term++) {
                            if (term != term1) {
                                residualStart(2) = term;
                                residualEnd(2) = term;
                                casa::Slicer residualSlicer(residualStart, residualEnd, residualStride, Slicer::endIsLast);

                                psfCrossTermsStart(2) = term1;
                                psfCrossTermsEnd(2) = term1;
                                psfCrossTermsStart(3) = term;
                                psfCrossTermsEnd(3) = term;
                                casa::Slicer psfCrossTermsSlicer(psfCrossTermsStart, psfCrossTermsEnd, psfCrossTermsStride, Slicer::endIsLast);
                                typename casa::Array<T> residualBFSlice = this->itsResidualBasisFunction(residualSlicer).nonDegenerate();
                                residualBFSlice -= this->control()->gain() * peakValues(term1) *
                                                   this->itsPSFCrossTerms(psfCrossTermsSlicer).nonDegenerate();
                            }
                        }
                    }
                }
            }

            return True;
        }

        template<class T, class FT>
        void DeconvolverBasisFunction<T, FT>::minMaxMaskedScales(T& minVal, T& maxVal,
                IPosition& minPos, IPosition& maxPos,
                const Array<T>& dataArray,
                const Array<T>& weightArray)
        {
            const Cube<T> data(dataArray);
            bool isWeighted(weightArray.shape().nonDegenerate().conform(data.xyPlane(0).shape()));

            const uInt nScales = data.shape()(2);

            Vector<T> sMaxVal(nScales);
            Vector<T> sMinVal(nScales);
            Vector<IPosition> sMinPos(nScales);
            Vector<IPosition> sMaxPos(nScales);
            {
                if (isWeighted) {
                    for (uInt scale = 0; scale < nScales; scale++) {
                        casa::minMaxMasked(sMinVal(scale), sMaxVal(scale), sMinPos(scale), sMaxPos(scale),
                                           Cube<T>(dataArray).xyPlane(scale), weightArray.nonDegenerate());
                    }
                } else {
                    for (uInt scale = 0; scale < nScales; scale++) {
                        casa::minMax(sMinVal(scale), sMaxVal(scale), sMinPos(scale), sMaxPos(scale),
                                     Cube<T>(dataArray).xyPlane(scale));
                    }
                }
            }
            minPos = IPosition(3, sMinPos(0)(0), sMinPos(0)(1), 0);
            maxPos = IPosition(3, sMaxPos(0)(0), sMaxPos(0)(1), 0);
            minVal = sMinVal(0);
            maxVal = sMaxVal(0);

            for (uInt scale = 1; scale < nScales; scale++) {
                if (sMinVal(scale) <= minVal) {
                    minVal = sMinVal(scale);
                    minPos = IPosition(3, sMinPos(scale)(0), sMinPos(scale)(1), scale);
                }

                if (sMaxVal(scale) >= maxVal) {
                    maxVal = sMaxVal(scale);
                    maxPos = IPosition(3, sMaxPos(scale)(0), sMaxPos(scale)(1), scale);
                }
            }

            // If weighting (presumably with weights) was done we need to
            // look up the original values (without the weights).
            minVal = data.xyPlane(minPos(2))(minPos);
            maxVal = data.xyPlane(maxPos(2))(maxPos);
        }
        template<class T, class FT>
        Vector<T> DeconvolverBasisFunction<T, FT>::findCoefficients(const Matrix<Double>& invCoupling,
                const Vector<T>& peakValues)
        {
            const uInt nRows(invCoupling.nrow());
            const uInt nCols(invCoupling.ncolumn());
            Vector<T> coefficients(nRows);

            for (uInt row = 0; row < nRows; row++) {
                coefficients(row) = T(0.0);

                for (uInt col = 0; col < nCols; col++) {
                    coefficients(row) += T(invCoupling(row, col)) * peakValues(col);
                }
            }

            return coefficients;
        }

        template<class T, class FT>
        Array<T> DeconvolverBasisFunction<T, FT>::applyInverse(const Matrix<Double>& invCoupling,
                const Array<T> dataArray)
        {
            Array<T> invDataArray(dataArray.shape(), 0.0);

            const uInt nRows(invCoupling.nrow());
            const uInt nCols(invCoupling.ncolumn());
            const uInt nx = dataArray.shape()(0);
            const uInt ny = dataArray.shape()(1);

            for (uInt j = 0; j < ny; j++) {
                for (uInt i = 0; i < nx; i++) {

                    IPosition currentPosCol(3, i, j, 0);
                    IPosition currentPosRow(3, i, j, 0);

                    for (uInt row = 0; row < nRows; row++) {
                        currentPosRow(2) = row;

                        for (uInt col = 0; col < nCols; col++) {
                            currentPosCol(2) = col;
                            invDataArray(currentPosRow) += T(invCoupling(row, col)) * dataArray(currentPosCol);
                        }
                    }

                }
            }

            return invDataArray;
        }

        template<class T, class FT>
        Array<T> DeconvolverBasisFunction<T, FT>::apply(const Matrix<Double>& coupling,
                const Vector<T> dataVector)
        {
            Vector<T> vecDataVector(dataVector.shape(), 0.0);

            const uInt nRows(coupling.nrow());
            const uInt nCols(coupling.ncolumn());
            const uInt nx = dataVector.nelements();

            for (uInt i = 0; i < nx; i++) {
                for (uInt row = 0; row < nRows; row++) {
                    for (uInt col = 0; col < nCols; col++) {
                        vecDataVector(row) += T(coupling(row, col)) * dataVector(col);
                    }
                }
            }

            return vecDataVector;
        }

    }
}
// namespace synthesis
// namespace askap


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_COPY_INCLUDE
#define MTL_COPY_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/detail/index.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/flatcat.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/utility/updater_to_assigner.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/update.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/crop.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/utility/enable_if.hpp>
#include <iostream>
#include <limits>

namespace mtl {

    namespace detail {

	// Set Destination matrix to zero when source is sparse
	// (otherwise everything is overwritten anyway)
	template <typename MatrixDest>
	inline void zero_with_sparse_src(MatrixDest& dest, tag::flat<tag::sparse>)
	{
	    set_to_zero(dest);
	}

	template <typename MatrixDest>
	inline void zero_with_sparse_src(MatrixDest&, tag::universe) {}

	// Adapt inserter size to operation
	template <typename Updater> struct copy_inserter_size {};

	// Specialization for store
	template <typename Value>
	struct copy_inserter_size< operations::update_store<Value> >
	{
	    template <typename MatrixSrc, typename MatrixDest>
	    static inline int apply(const MatrixSrc& src, const MatrixDest& dest)
	    {
		// std::cout << "nnz = " << src.nnz() << ", dim1 = " << dest.dim1() << "\n";
		return int(src.nnz() * 1.2 / dest.dim1());
	    }
	};

	struct sum_of_sizes
	{
	    template <typename MatrixSrc, typename MatrixDest>
	    static inline int apply(const MatrixSrc& src, const MatrixDest& dest)
	    {	return int((src.nnz() + dest.nnz()) * 1.2 / dest.dim1()); }
	};

	// Specialization for plus and minus
	template <typename Value> struct copy_inserter_size< operations::update_plus<Value> > : sum_of_sizes {};
	template <typename Value> struct copy_inserter_size< operations::update_minus<Value> > : sum_of_sizes {};

    } // namespace detail

    template <typename Updater, typename MatrixSrc, typename MatrixDest>
    inline void gen_matrix_copy(const MatrixSrc& src, MatrixDest& dest, bool with_reset)
    {
	vampir_trace<3002> tracer;
	MTL_THROW_IF(num_rows(src) != num_rows(dest) || num_cols(src) != num_cols(dest), incompatible_size());

	if (with_reset)
	    detail::zero_with_sparse_src(dest, traits::sparsity_flatcat<MatrixSrc>());

	typename traits::row<MatrixSrc>::type             row(src);
	typename traits::col<MatrixSrc>::type             col(src);
	typename traits::const_value<MatrixSrc>::type     value(src);
	typedef typename traits::range_generator<tag::major, MatrixSrc>::type  cursor_type;

	// std::cout << "Slot size is " << detail::copy_inserter_size<Updater>::apply(src, dest) << "\n";
	mat::inserter<MatrixDest, Updater>   ins(dest, detail::copy_inserter_size<Updater>::apply(src, dest));
	for (cursor_type cursor = mtl::begin<tag::major>(src), cend = mtl::end<tag::major>(src);
	     cursor != cend; ++cursor) {
	    // std::cout << dest << '\n';

	    typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
	    for (icursor_type icursor = mtl::begin<tag::nz>(cursor), icend = mtl::end<tag::nz>(cursor);
		 icursor != icend; ++icursor) {
		//std::cout << "in " << row(*icursor) << ", " << col(*icursor) << " insert " << value(*icursor) << '\n';
		ins(row(*icursor), col(*icursor)) << value(*icursor); }
	}
    }

    // Specialization for multi_vector
    template <typename Updater, typename MatrixSrc, typename Vector>
    inline void gen_matrix_copy(const MatrixSrc& src, mtl::mat::multi_vector<Vector>& dest, bool)
    {
	MTL_THROW_IF(num_rows(src) != num_rows(dest) || num_cols(src) != num_cols(dest), incompatible_size());
	typedef typename mtl::traits::updater_to_assigner<Updater>::type Assigner;

	for (std::size_t i= 0, n= num_cols(src); i < n; ++i)
	    Assigner::first_update(dest.vector(i), src.vector(i));
    }

    namespace {
#    ifdef __clang__
#     pragma clang diagnostic ignored "-Wunneeded-internal-declaration"
#     pragma clang diagnostic ignored "-Wunused-function"
#    endif
		template <typename T>
		inline T inc_wo_over(T i)
		{ return i == std::numeric_limits<T>::max() ? i : i+1; }

		template <typename T>
		inline T negate_wo_over(T i)
		{ return i == std::numeric_limits<T>::min() ? std::numeric_limits<T>::max() : -i; }
    }

    template <typename Updater, typename ValueSrc, typename Para, typename ValueDest>
    typename boost::enable_if<boost::is_same<Updater, operations::update_store<ValueDest> > >::type
    inline gen_matrix_copy(const mat::banded_view<mtl::mat::compressed2D<ValueSrc, Para> >& src, mtl::mat::compressed2D<ValueDest, Para>& dest, bool)
    {
	vampir_trace<3061> tracer;
	typedef typename Para::size_type size_type;
	dest.change_dim(num_rows(src), num_cols(src)); // contains make_empty
	set_to_zero(dest);
	const mtl::mat::compressed2D<ValueSrc, Para>  &sref= src.ref;
	const std::vector<size_type>        &sstarts= sref.ref_major(), &sindices= sref.ref_minor();
	long first, last;
	if (traits::is_row_major<Para>::value) {
	    first= src.get_begin();
	    last= src.get_end();
	} else {
	    first= inc_wo_over(negate_wo_over(src.get_end()));
	    last=  inc_wo_over(negate_wo_over(src.get_begin()));
	}

	long jd= 0, j_end= long(sstarts[0]);
	for (long i= 0, i_end= long(src.dim1()), f= first, l= last; i < i_end; ++i) {
	    dest.ref_major()[i]= jd;
	    long j= j_end;
	    j_end= long(sstarts[i+1]);
	    while (j < j_end && long(sindices[j]) < f) j++;
	    while (j < j_end && long(sindices[j]) < l) jd++, j++;
	    f= inc_wo_over(f);
	    l= inc_wo_over(l);
	}
	dest.ref_major()[src.dim1()]= jd;
	dest.set_nnz(jd); // resizes indices and data

	for (long i= 0, i_end= long(src.dim1()), jd= 0, j_end= long(sstarts[0]); i < i_end; ++i) {
	    dest.ref_major()[i]= jd;
	    long j= j_end;
	    j_end= long(sstarts[i+1]);
	    while (j < j_end && long(sindices[j]) < first) j++;
	    while (j < j_end && long(sindices[j]) < last) {
		dest.ref_minor()[jd]= sindices[j];
		dest.data[jd++]= sref.data[j++];
	    }
	    first= inc_wo_over(first);
	    last= inc_wo_over(last);
	}
    }

    /// Copy matrix \p src into matrix \p dest
    template <typename MatrixSrc, typename MatrixDest>
    inline void matrix_copy(const MatrixSrc& src, MatrixDest& dest)
    {
	gen_matrix_copy< operations::update_store<typename MatrixDest::value_type> >(src, dest, true);
    }

    /// Add matrix \p src to matrix \p dest in copy-like style
    template <typename MatrixSrc, typename MatrixDest>
    inline void matrix_copy_plus(const MatrixSrc& src, MatrixDest& dest)
    {
	gen_matrix_copy< operations::update_plus<typename MatrixDest::value_type> >(src, dest, false);
    }

    /// Subtract matrix \p src from matrix \p dest in copy-like style
    template <typename MatrixSrc, typename MatrixDest>
    inline void matrix_copy_minus(const MatrixSrc& src, MatrixDest& dest)
    {
	gen_matrix_copy< operations::update_minus<typename MatrixDest::value_type> >(src, dest, false);
    }

    /// Multiply matrix \p src element-wise with matrix \p dest in copy-like style
    template <typename MatrixSrc, typename MatrixDest>
    inline void matrix_copy_ele_times(const MatrixSrc& src, MatrixDest& dest)
    {
	vampir_trace<3001> tracer;
	MTL_THROW_IF(num_rows(src) != num_rows(dest) || num_cols(src) != num_cols(dest), incompatible_size());

	typename traits::row<MatrixDest>::type             row(dest);
	typename traits::col<MatrixDest>::type             col(dest);
	typename traits::value<MatrixDest>::type           value(dest);
	typedef typename traits::range_generator<tag::major, MatrixDest>::type  cursor_type;
	typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;

	for (cursor_type cursor = begin<tag::major>(dest), cend = end<tag::major>(dest); cursor != cend; ++cursor)
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		value(*icursor, value(*icursor) * src[row(*icursor)][col(*icursor)]);
#if 0   // copy would result in a*0 = a and 0*b = b!!!!
	gen_matrix_copy< operations::update_times<typename MatrixDest::value_type> >(src, dest, false);
#endif
	crop(dest);
    }

    template <typename MatrixSrc, typename MatrixDest>
    inline void copy(const MatrixSrc& src, tag::flat<tag::matrix>, MatrixDest& dest, tag::flat<tag::matrix>)
	// inline void copy(const MatrixSrc& src, tag::matrix_expr, MatrixDest& dest, tag::matrix)
    {
	return matrix_copy(src, dest);
    }

    template <typename Updater, typename VectorSrc, typename VectorDest>
    inline void gen_vector_copy(const VectorSrc& src, VectorDest& dest, bool with_reset)
    {
	// Works only with dense vectors as dest !!!!! (source could be sparse)
	// Needs vector inserter
	vampir_trace<2001> tracer;

	MTL_STATIC_ASSERT((boost::is_same<typename ashape::ashape<VectorSrc>::type,
					  typename ashape::ashape<VectorDest>::type>::value), "Source and target must have the same algebraic shape.");

	MTL_THROW_IF(size(src) != size(dest), incompatible_size());

	if (with_reset)
	    detail::zero_with_sparse_src(dest, typename traits::category<VectorSrc>::type());

	typename traits::index<VectorSrc>::type           index(src);
	typename traits::const_value<VectorSrc>::type     value(src);

	typedef typename traits::range_generator<tag::nz, VectorSrc>::type  cursor_type;
	for (cursor_type cursor = begin<tag::nz>(src), cend = end<tag::nz>(src);
	     cursor != cend; ++cursor)
	    Updater()(dest[index(*cursor)], value(*cursor));
    }

    /// Copy vector \p src into vector \p dest
    template <typename VectorSrc, typename VectorDest>
    inline void vector_copy(const VectorSrc& src, VectorDest& dest)
    {
	gen_vector_copy< operations::update_store<typename VectorDest::value_type> >(src, dest, true);
    }

    /// Add vector \p src to vector \p dest in copy-like style
    template <typename VectorSrc, typename VectorDest>
    inline void vector_copy_plus(const VectorSrc& src, VectorDest& dest)
    {
	gen_vector_copy< operations::update_plus<typename VectorDest::value_type> >(src, dest, false);
    }

    /// Subtract vector \p src from vector \p dest in copy-like style
    template <typename VectorSrc, typename VectorDest>
    inline void vector_copy_minus(const VectorSrc& src, VectorDest& dest)
    {
	gen_vector_copy< operations::update_minus<typename VectorDest::value_type> >(src, dest, false);
    }

    template <typename VectorSrc, typename VectorDest>
    inline void copy(const VectorSrc& src, tag::flat<tag::vector>, VectorDest& dest, tag::flat<tag::vector>)
    {
	return vector_copy(src, dest);
    }

    template <typename CollSrc, typename CollDest>
    inline void copy(const CollSrc& src, CollDest& dest)
    {
	vampir_trace<3003> tracer;
	return copy(src, traits::flatcat2<CollSrc, tag::matrix, tag::vector>(),
		    dest, traits::flatcat2<CollDest, tag::matrix, tag::vector>());
    }

} // namespace mtl

#endif // MTL_COPY_INCLUDE


#ifndef STAN_IO_ARRAY_VAR_CONTEXT_HPP
#define STAN_IO_ARRAY_VAR_CONTEXT_HPP

#include <stan/util/io/var_context.hpp>
#include <boost/throw_exception.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <map>
#include <sstream>
#include <string>
#include <vector>
#include <utility>

namespace stan {

  namespace io {

    template<typename T>
    T product(std::vector<T> dims) {
      T y = 1;
      for (size_t i = 0; i < dims.size(); ++i)
        y *= dims[i];
      return y;
    }

    /**
     * An array_var_context object represents a named arrays
     * with dimensions constructed from an array, a vector
     * of names, and a vector of all dimensions for each element.
     */
    class array_var_context : public var_context {
    private:
      std::map<std::string,
               std::pair<std::vector<double>,
                         std::vector<size_t> > > vars_r_;
      std::map<std::string,
               std::pair<std::vector<int>,
                         std::vector<size_t> > > vars_i_;
      std::vector<double> const empty_vec_r_;
      std::vector<int> const empty_vec_i_;
      std::vector<size_t> const empty_vec_ui_;

      bool contains_r_only(const std::string& name) const {
        return vars_r_.find(name) != vars_r_.end();
      }

      /**
       * Check (1) if the vector size of dimensions is no smaller
       * than the name vector size; (2) if the size of the input
       * array is large enough for what is needed.
       */
      template <typename T>
      void validate(const std::vector<std::string>& names,
                    const T& array,
                    const std::vector<std::vector<size_t> >& dims) {
        size_t total = 0;
        size_t num_par = names.size();
        if (num_par > dims.size()) {
          std::stringstream msg;
          msg << "size of vector of dimensions (found " << dims.size() << ") "
              << "should be no smaller than number of parameters (found "
              << num_par << ").";
          BOOST_THROW_EXCEPTION(std::invalid_argument(msg.str()));
        }
        for (size_t i = 0; i < num_par; i++)
          total += stan::io::product(dims[i]);
        size_t array_len = array.size();
        if (total > array_len) {
          std::stringstream msg;
          msg << "array is not long enough for all elements: " << array_len
              << " is found, but "
              << total << " is needed.";
          BOOST_THROW_EXCEPTION(std::invalid_argument(msg.str()));
        }
      }

      void add_r(const std::vector<std::string>& names,
                 const std::vector<double>& values,
                 const std::vector<std::vector<size_t> >& dims) {
        validate(names, values, dims);
        size_t start = 0;
        size_t end = 0;
        for (size_t i = 0; i < names.size(); i++) {
          end += product(dims[i]);
          std::vector<double> v(values.begin() + start, values.begin() + end);
          vars_r_[names[i]]
            = std::pair<std::vector<double>,
                        std::vector<size_t> >(v, dims[i]);
          start = end;
        }
      }

      void add_r(const std::vector<std::string>& names,
                 const Eigen::VectorXd& values,
                 const std::vector<std::vector<size_t> >& dims) {
        validate(names, values, dims);
        size_t start = 0;
        size_t end = 0;
        for (size_t i = 0; i < names.size(); i++) {
          end += product(dims[i]);
          size_t v_len = end - start;
          std::vector<double> v(v_len);
          for (size_t i = 0; i < v_len; ++i)
            v[i] = values(start + i);
          vars_r_[names[i]]
            = std::pair<std::vector<double>,
                        std::vector<size_t> >(v, dims[i]);
          start = end;
        }
      }

      void add_i(const std::vector<std::string>& names,
                 const std::vector<int>& values,
                 const std::vector<std::vector<size_t> >& dims) {
        validate(names, values, dims);
        size_t start = 0;
        size_t end = 0;
        for (size_t i = 0; i < names.size(); i++) {
          end += product(dims[i]);
          std::vector<int> v(values.begin() + start, values.begin() + end);
          vars_i_[names[i]]
            = std::pair<std::vector<int>,
                        std::vector<size_t> >(v, dims[i]);
          start = end;
        }
      }

    public:
      /**
       * Construct an array_var_context from only real value arrays.
       *
       * @param names_r  names for each element
       * @param values_r a vector of double values for all elements
       * @param dim_r   a vector of dimensions
       */
      array_var_context(const std::vector<std::string>& names_r,
                        const std::vector<double>& values_r,
                        const std::vector<std::vector<size_t> >& dim_r) {
        add_r(names_r, values_r, dim_r);
      }

      /**
       * Construct an array_var_context from an Eigen::RowVectorXd.
       *
       * @param names_r  names for each element
       * @param values_r an Eigen RowVector double values for all elements
       * @param dim_r   a vector of dimensions
       */
      array_var_context(const std::vector<std::string>& names_r,
                        const Eigen::RowVectorXd& values_r,
                        const std::vector<std::vector<size_t> >& dim_r) {
        add_r(names_r, values_r, dim_r);
      }

      /**
       * Construct an array_var_context from only integer value arrays.
       *
       * @param names_i  names for each element
       * @param values_i a vector of integer values for all elements
       * @param dim_i   a vector of dimensions
       */
      array_var_context(const std::vector<std::string>& names_i,
                        const std::vector<int>& values_i,
                        const std::vector<std::vector<size_t> >& dim_i) {
        add_i(names_i, values_i, dim_i);
      }

      /**
       * Construct an array_var_context from arrays of both double
       * and integer separately
       *
       */
      array_var_context(const std::vector<std::string>& names_r,
                        const std::vector<double>& values_r,
                        const std::vector<std::vector<size_t> >& dim_r,
                        const std::vector<std::string>& names_i,
                        const std::vector<int>& values_i,
                        const std::vector<std::vector<size_t> >& dim_i) {
        add_i(names_i, values_i, dim_i);
        add_r(names_r, values_r, dim_r);
      }

      /**
       * Return true if this dump contains the specified
       * variable name is defined. This method returns true
       * even if the values are all integers.
       *
       * @param name Variable name to test.
       * @return true if the variable exists.
       */
      bool contains_r(const std::string& name) const {
        return contains_r_only(name) || contains_i(name);
      }

      /**
       * Return true if this dump contains an integer
       * valued array with the specified name.
       *
       * @param name Variable name to test.
       * @return true if the variable name has an integer
       * array value.
       */
      bool contains_i(const std::string& name) const {
        return vars_i_.find(name) != vars_i_.end();
      }

      /**
       * Return the double values for the variable with the specified
       * name or null.
       *
       * @param name Name of variable.
       * @return Values of variable.
       */
      std::vector<double> vals_r(const std::string& name) const {
        if (contains_r_only(name)) {
          return (vars_r_.find(name)->second).first;
        } else if (contains_i(name)) {
          std::vector<int> vec_int = (vars_i_.find(name)->second).first;
          std::vector<double> vec_r(vec_int.size());
          for (size_t ii = 0; ii < vec_int.size(); ii++) {
            vec_r[ii] = vec_int[ii];
          }
          return vec_r;
        }
        return empty_vec_r_;
      }

      /**
       * Return the dimensions for the double variable with the specified
       * name.
       *
       * @param name Name of variable.
       * @return Dimensions of variable.
       */
      std::vector<size_t> dims_r(const std::string& name) const {
        if (contains_r_only(name)) {
          return (vars_r_.find(name)->second).second;
        } else if (contains_i(name)) {
          return (vars_i_.find(name)->second).second;
        }
        return empty_vec_ui_;
      }

      /**
       * Return the integer values for the variable with the specified
       * name.
       *
       * @param name Name of variable.
       * @return Values.
       */
      std::vector<int> vals_i(const std::string& name) const {
        if (contains_i(name)) {
          return (vars_i_.find(name)->second).first;
        }
        return empty_vec_i_;
      }

      /**
       * Return the dimensions for the integer variable with the specified
       * name.
       *
       * @param name Name of variable.
       * @return Dimensions of variable.
       */
      std::vector<size_t> dims_i(const std::string& name) const {
        if (contains_i(name)) {
          return (vars_i_.find(name)->second).second;
        }
        return empty_vec_ui_;
      }

      /**
       * Return a list of the names of the floating point variables in
       * the dump.
       *
       * @param names Vector to store the list of names in.
       */
      virtual void names_r(std::vector<std::string>& names) const {
        names.resize(0);
        for (std::map<std::string,
                      std::pair<std::vector<double>,
                                std::vector<size_t> > >
                 ::const_iterator it = vars_r_.begin();
             it != vars_r_.end(); ++it)
          names.push_back((*it).first);
      }

      /**
       * Return a list of the names of the integer variables in
       * the dump.
       *
       * @param names Vector to store the list of names in.
       */
      virtual void names_i(std::vector<std::string>& names) const {
        names.resize(0);
        for (std::map<std::string,
                      std::pair<std::vector<int>,
                                std::vector<size_t> > >
                 ::const_iterator it = vars_i_.begin();
             it != vars_i_.end(); ++it)
          names.push_back((*it).first);
      }

      /**
       * Remove variable from the object.
       *
       * @param name Name of the variable to remove.
       * @return If variable is removed returns true, else
       *   returns false.
       */
      bool remove(const std::string& name) {
        return (vars_i_.erase(name) > 0)
          || (vars_r_.erase(name) > 0);
      }
    };
  }
}
#endif


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

// std_real_concept is an archetype for built-in Real types.

// The main purpose in providing this type is to verify
// that std lib functions are found via a using declaration
// bringing those functions into the current scope, and not
// just because they happen to be in global scope.
//
// If ::pow is found rather than std::pow say, then the code
// will silently compile, but truncation of long doubles to
// double will cause a significant loss of precision.
// A template instantiated with std_real_concept will *only*
// compile if it std::whatever is in scope.

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

#include <ostream>
#include <istream>
#include <boost/config/no_tr1/cmath.hpp>
#include <math.h> // fmodl

#ifndef BOOST_MATH_STD_REAL_CONCEPT_HPP
#define BOOST_MATH_STD_REAL_CONCEPT_HPP

namespace boost{ namespace math{

namespace concepts
{

#ifdef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   typedef double std_real_concept_base_type;
#else
   typedef long double std_real_concept_base_type;
#endif

class std_real_concept
{
public:
   // Constructors:
   std_real_concept() : m_value(0){}
   std_real_concept(char c) : m_value(c){}
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   std_real_concept(wchar_t c) : m_value(c){}
#endif
   std_real_concept(unsigned char c) : m_value(c){}
   std_real_concept(signed char c) : m_value(c){}
   std_real_concept(unsigned short c) : m_value(c){}
   std_real_concept(short c) : m_value(c){}
   std_real_concept(unsigned int c) : m_value(c){}
   std_real_concept(int c) : m_value(c){}
   std_real_concept(unsigned long c) : m_value(c){}
   std_real_concept(long c) : m_value(c){}
#if defined(__DECCXX) || defined(__SUNPRO_CC)
   std_real_concept(unsigned long long c) : m_value(static_cast<std_real_concept_base_type>(c)){}
   std_real_concept(long long c) : m_value(static_cast<std_real_concept_base_type>(c)){}
#elif defined(BOOST_HAS_LONG_LONG)
   std_real_concept(boost::ulong_long_type c) : m_value(static_cast<std_real_concept_base_type>(c)){}
   std_real_concept(boost::long_long_type c) : m_value(static_cast<std_real_concept_base_type>(c)){}
#endif
   std_real_concept(float c) : m_value(c){}
   std_real_concept(double c) : m_value(c){}
   std_real_concept(long double c) : m_value(c){}
#ifdef BOOST_MATH_USE_FLOAT128
   std_real_concept(BOOST_MATH_FLOAT128_TYPE c) : m_value(c){}
#endif

   // Assignment:
   std_real_concept& operator=(char c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned char c) { m_value = c; return *this; }
   std_real_concept& operator=(signed char c) { m_value = c; return *this; }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   std_real_concept& operator=(wchar_t c) { m_value = c; return *this; }
#endif
   std_real_concept& operator=(short c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned short c) { m_value = c; return *this; }
   std_real_concept& operator=(int c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned int c) { m_value = c; return *this; }
   std_real_concept& operator=(long c) { m_value = c; return *this; }
   std_real_concept& operator=(unsigned long c) { m_value = c; return *this; }
#if defined(__DECCXX) || defined(__SUNPRO_CC)
   std_real_concept& operator=(unsigned long long c) { m_value = static_cast<std_real_concept_base_type>(c); return *this; }
   std_real_concept& operator=(long long c) { m_value = static_cast<std_real_concept_base_type>(c); return *this; }
#elif defined(BOOST_HAS_LONG_LONG)
   std_real_concept& operator=(boost::long_long_type c) { m_value = static_cast<std_real_concept_base_type>(c); return *this; }
   std_real_concept& operator=(boost::ulong_long_type c) { m_value = static_cast<std_real_concept_base_type>(c); return *this; }
#endif
   std_real_concept& operator=(float c) { m_value = c; return *this; }
   std_real_concept& operator=(double c) { m_value = c; return *this; }
   std_real_concept& operator=(long double c) { m_value = c; return *this; }

   // Access:
   std_real_concept_base_type value()const{ return m_value; }

   // Member arithmetic:
   std_real_concept& operator+=(const std_real_concept& other)
   { m_value += other.value(); return *this; }
   std_real_concept& operator-=(const std_real_concept& other)
   { m_value -= other.value(); return *this; }
   std_real_concept& operator*=(const std_real_concept& other)
   { m_value *= other.value(); return *this; }
   std_real_concept& operator/=(const std_real_concept& other)
   { m_value /= other.value(); return *this; }
   std_real_concept operator-()const
   { return -m_value; }
   std_real_concept const& operator+()const
   { return *this; }

private:
   std_real_concept_base_type m_value;
};

// Non-member arithmetic:
inline std_real_concept operator+(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result += b;
   return result;
}
inline std_real_concept operator-(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result -= b;
   return result;
}
inline std_real_concept operator*(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result *= b;
   return result;
}
inline std_real_concept operator/(const std_real_concept& a, const std_real_concept& b)
{
   std_real_concept result(a);
   result /= b;
   return result;
}

// Comparison:
inline bool operator == (const std_real_concept& a, const std_real_concept& b)
{ return a.value() == b.value(); }
inline bool operator != (const std_real_concept& a, const std_real_concept& b)
{ return a.value() != b.value();}
inline bool operator < (const std_real_concept& a, const std_real_concept& b)
{ return a.value() < b.value(); }
inline bool operator <= (const std_real_concept& a, const std_real_concept& b)
{ return a.value() <= b.value(); }
inline bool operator > (const std_real_concept& a, const std_real_concept& b)
{ return a.value() > b.value(); }
inline bool operator >= (const std_real_concept& a, const std_real_concept& b)
{ return a.value() >= b.value(); }

} // namespace concepts
} // namespace math
} // namespace boost

namespace std{

// Non-member functions:
inline boost::math::concepts::std_real_concept acos(boost::math::concepts::std_real_concept a)
{ return std::acos(a.value()); }
inline boost::math::concepts::std_real_concept cos(boost::math::concepts::std_real_concept a)
{ return std::cos(a.value()); }
inline boost::math::concepts::std_real_concept asin(boost::math::concepts::std_real_concept a)
{ return std::asin(a.value()); }
inline boost::math::concepts::std_real_concept atan(boost::math::concepts::std_real_concept a)
{ return std::atan(a.value()); }
inline boost::math::concepts::std_real_concept atan2(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::atan2(a.value(), b.value()); }
inline boost::math::concepts::std_real_concept ceil(boost::math::concepts::std_real_concept a)
{ return std::ceil(a.value()); }
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline boost::math::concepts::std_real_concept fmod(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return fmodl(a.value(), b.value()); }
#else
inline boost::math::concepts::std_real_concept fmod(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::fmod(a.value(), b.value()); }
#endif
inline boost::math::concepts::std_real_concept cosh(boost::math::concepts::std_real_concept a)
{ return std::cosh(a.value()); }
inline boost::math::concepts::std_real_concept exp(boost::math::concepts::std_real_concept a)
{ return std::exp(a.value()); }
inline boost::math::concepts::std_real_concept fabs(boost::math::concepts::std_real_concept a)
{ return std::fabs(a.value()); }
inline boost::math::concepts::std_real_concept abs(boost::math::concepts::std_real_concept a)
{ return std::abs(a.value()); }
inline boost::math::concepts::std_real_concept floor(boost::math::concepts::std_real_concept a)
{ return std::floor(a.value()); }
inline boost::math::concepts::std_real_concept modf(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept* ipart)
{
   boost::math::concepts::std_real_concept_base_type ip;
   boost::math::concepts::std_real_concept_base_type result = std::modf(a.value(), &ip);
   *ipart = ip;
   return result;
}
inline boost::math::concepts::std_real_concept frexp(boost::math::concepts::std_real_concept a, int* expon)
{ return std::frexp(a.value(), expon); }
inline boost::math::concepts::std_real_concept ldexp(boost::math::concepts::std_real_concept a, int expon)
{ return std::ldexp(a.value(), expon); }
inline boost::math::concepts::std_real_concept log(boost::math::concepts::std_real_concept a)
{ return std::log(a.value()); }
inline boost::math::concepts::std_real_concept log10(boost::math::concepts::std_real_concept a)
{ return std::log10(a.value()); }
inline boost::math::concepts::std_real_concept tan(boost::math::concepts::std_real_concept a)
{ return std::tan(a.value()); }
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, boost::math::concepts::std_real_concept b)
{ return std::pow(a.value(), b.value()); }
#if !defined(__SUNPRO_CC)
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, int b)
{ return std::pow(a.value(), b); }
#else
inline boost::math::concepts::std_real_concept pow(boost::math::concepts::std_real_concept a, int b)
{ return std::pow(a.value(), static_cast<long double>(b)); }
#endif
inline boost::math::concepts::std_real_concept sin(boost::math::concepts::std_real_concept a)
{ return std::sin(a.value()); }
inline boost::math::concepts::std_real_concept sinh(boost::math::concepts::std_real_concept a)
{ return std::sinh(a.value()); }
inline boost::math::concepts::std_real_concept sqrt(boost::math::concepts::std_real_concept a)
{ return std::sqrt(a.value()); }
inline boost::math::concepts::std_real_concept tanh(boost::math::concepts::std_real_concept a)
{ return std::tanh(a.value()); }

} // namespace std

#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{ namespace math{ namespace concepts{

//
// Conversion and truncation routines:
//
template <class Policy>
inline int iround(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::iround(v.value(), pol);
}
inline int iround(const concepts::std_real_concept& v)
{
   return boost::math::iround(v.value(), policies::policy<>());
}

template <class Policy>
inline long lround(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::lround(v.value(), pol);
}
inline long lround(const concepts::std_real_concept& v)
{
   return boost::math::lround(v.value(), policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG

template <class Policy>
inline boost::long_long_type llround(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::llround(v.value(), pol);
}
inline boost::long_long_type llround(const concepts::std_real_concept& v)
{
   return boost::math::llround(v.value(), policies::policy<>());
}

#endif

template <class Policy>
inline int itrunc(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::itrunc(v.value(), pol);
}
inline int itrunc(const concepts::std_real_concept& v)
{
   return boost::math::itrunc(v.value(), policies::policy<>());
}

template <class Policy>
inline long ltrunc(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::ltrunc(v.value(), pol);
}
inline long ltrunc(const concepts::std_real_concept& v)
{
   return boost::math::ltrunc(v.value(), policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG

template <class Policy>
inline boost::long_long_type lltrunc(const concepts::std_real_concept& v, const Policy& pol)
{
   return boost::math::lltrunc(v.value(), pol);
}
inline boost::long_long_type lltrunc(const concepts::std_real_concept& v)
{
   return boost::math::lltrunc(v.value(), policies::policy<>());
}

#endif

// Streaming:
template <class charT, class traits>
inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const std_real_concept& a)
{
   return os << a.value();
}
template <class charT, class traits>
inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, std_real_concept& a)
{
#if defined(__SGI_STL_PORT) || defined(_RWSTD_VER) || defined(__LIBCOMO__) || defined(_LIBCPP_VERSION)
   std::string s;
   std_real_concept_base_type d;
   is >> s;
   std::sscanf(s.c_str(), "%Lf", &d);
   a = d;
   return is;
#else
   std_real_concept_base_type v;
   is >> v;
   a = v;
   return is;
#endif
}

} // namespace concepts
}}

#include <boost/math/tools/precision.hpp>
#include <boost/math/tools/big_constant.hpp>

namespace boost{ namespace math{
namespace tools
{

template <>
inline concepts::std_real_concept make_big_value<concepts::std_real_concept>(boost::floatmax_t val, const char*, mpl::false_ const&, mpl::false_ const&)
{
   return val;  // Can't use lexical_cast here, sometimes it fails....
}

template <>
inline concepts::std_real_concept max_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return max_value<concepts::std_real_concept_base_type>();
}

template <>
inline concepts::std_real_concept min_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return min_value<concepts::std_real_concept_base_type>();
}

template <>
inline concepts::std_real_concept log_max_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return log_max_value<concepts::std_real_concept_base_type>();
}

template <>
inline concepts::std_real_concept log_min_value<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return log_min_value<concepts::std_real_concept_base_type>();
}

template <>
inline concepts::std_real_concept epsilon(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{
   return tools::epsilon<concepts::std_real_concept_base_type>();
}

template <>
inline int digits<concepts::std_real_concept>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(concepts::std_real_concept))
{ // Assume number of significand bits is same as std_real_concept_base_type,
  // unless std::numeric_limits<T>::is_specialized to provide digits.
   return digits<concepts::std_real_concept_base_type>();
}

} // namespace tools

#if BOOST_WORKAROUND(BOOST_MSVC, <= 1310)
using concepts::itrunc;
using concepts::ltrunc;
using concepts::lltrunc;
using concepts::iround;
using concepts::lround;
using concepts::llround;
#endif

} // namespace math
} // namespace boost

#endif // BOOST_MATH_STD_REAL_CONCEPT_HPP


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120521    T. Secretin       File created.
 *      130318    K. Kumar          Updated allocation of Vector6d to fix problem with Eigen types.
 *
 *    References
 *      Eigen. http://eigen.tuxfamily.org/dox/TopicUnalignedArrayAssert.html, last accessed: 18th
 *          March, 2013.
 *
 *    Notes
 *
 */

#include <stdexcept>

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Ephemerides/cartesianStateExtractor.h"

namespace tudat
{
namespace ephemerides
{

boost::shared_ptr< basic_mathematics::Vector6d > CartesianStateExtractor::extract(
        ParsedDataLineMapPtr dataLineMap )
{
    // Short-hand notation.
    namespace parsed_data_vector_utilities = input_output::parsed_data_vector_utilities;
    using basic_mathematics::Vector6d;

    // Create a new CartesianElements object.
    boost::shared_ptr< Vector6d > cartesianElements
            = boost::allocate_shared< Vector6d >( Eigen::aligned_allocator< Vector6d >( ) );

    // Find and set Cartesian x coordinate.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianXCoordinate ) )
    {
        ( *cartesianElements )( orbital_element_conversions::xCartesianPositionIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianXCoordinate );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                 std::runtime_error(
                                        "No Cartesian x coordinate entry found." ) ) );
    }

    // Find and set Cartesian y coordinate.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianYCoordinate ) )
    {
        ( *cartesianElements )( orbital_element_conversions::yCartesianPositionIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianYCoordinate );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                 std::runtime_error( "No Cartesian y coordinate entry found." ) ) );
    }

    // Find and set Cartesian z coordinate.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianZCoordinate ) )
    {
        ( *cartesianElements )( orbital_element_conversions::zCartesianPositionIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianZCoordinate );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                 std::runtime_error(
                                        "No Cartesian z coordinate entry found." ) ) );
    }

    // Find and set Cartesian x velocity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianXVelocity ) )
    {
        ( *cartesianElements )( orbital_element_conversions::xCartesianVelocityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianXVelocity );
    }
    else
    {
        boost::throw_exception( boost::enable_error_info(
                                   std::runtime_error(
                                        "No Cartesian x velocity entry found." ) ) );
    }

    // Find and set Cartesian y velocity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianYVelocity ) )
    {
        ( *cartesianElements )( orbital_element_conversions::yCartesianVelocityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianYVelocity );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                   std::runtime_error(
                                        "No Cartesian y velocity entry found." ) ) );
    }

    // Find and set Cartesian z velocity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::cartesianZVelocity ) )
    {
        ( *cartesianElements )( orbital_element_conversions::zCartesianVelocityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::cartesianZVelocity );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                   std::runtime_error(
                                        "No Cartesian z velocity entry found." ) ) );
    }

    return cartesianElements;
}

} // namespace ephemerides
} // namespace tudat


// This is an advanced implementation of the algorithm described in the
// following paper:
//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms with
//    sound   state   representationsthrough  encapsulation  of  manifolds. CoRR,  vol.  abs/1107.1119,  2011.[Online].
//    Available: http://arxiv.org/abs/1107.1119

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

/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/startIdx.hpp
 * @brief Tools to access sub-elements of compound manifolds.
 */
#ifndef GET_START_INDEX_H_
#define GET_START_INDEX_H_

#include <Eigen/Core>

#include "src/SubManifold.hpp"
#include "src/vectview.hpp"

namespace MTK {

/**
 * \defgroup SubManifolds Accessing Submanifolds
 * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
 * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
 * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
 * @c &pose::orient and @c &pose::trans give all required information and are still
 * valid if the base type gets extended or the actual types of @a orient and @a trans
 * change (e.g. from 2D to 3D).
 *
 * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
 */
//@{

/**
 * Determine the index of a sub-variable within a compound variable.
 */
template <class Base, class T, int idx, int dim>
int getStartIdx(MTK::SubManifold<T, idx, dim> Base::*) {
    return idx;
}

template <class Base, class T, int idx, int dim>
int getStartIdx_(MTK::SubManifold<T, idx, dim> Base::*) {
    return dim;
}

/**
 * Determine the degrees of freedom of a sub-variable within a compound variable.
 */
template <class Base, class T, int idx, int dim>
int getDof(MTK::SubManifold<T, idx, dim> Base::*) {
    return T::DOF;
}
template <class Base, class T, int idx, int dim>
int getDim(MTK::SubManifold<T, idx, dim> Base::*) {
    return T::DIM;
}

/**
 * set the diagonal elements of a covariance matrix corresponding to a sub-variable
 */
template <class Base, class T, int idx, int dim>
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>& cov, MTK::SubManifold<T, idx, dim> Base::*,
                 const typename Base::scalar& val) {
    cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
}

template <class Base, class T, int idx, int dim>
void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>& cov,
                  MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar& val) {
    cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
}

/**
 * Get the subblock of corresponding to two members, i.e.
 * \code
 *  Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
 *  MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
 *  MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
 * \endcode
 * lets you modify mixed covariance entries in a bigger covariance matrix.
 */
template <class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock<Base, T1, T2>::Type subblock(
    Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>& cov, MTK::SubManifold<T1, idx1, dim1> Base::*,
    MTK::SubManifold<T2, idx2, dim2> Base::*) {
    return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}

template <class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock_<Base, T1, T2>::Type subblock_(
    Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>& cov, MTK::SubManifold<T1, idx1, dim1> Base::*,
    MTK::SubManifold<T2, idx2, dim2> Base::*) {
    return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}

template <typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type subblock(
    Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>& cov, MTK::SubManifold<T1, idx1, dim1> Base1::*,
    MTK::SubManifold<T2, idx2, dim2> Base2::*) {
    return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}

template <typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type subblock_(
    Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>& cov, MTK::SubManifold<T1, idx1, dim1> Base1::*,
    MTK::SubManifold<T2, idx2, dim2> Base2::*) {
    return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}
/**
 * Get the subblock of corresponding to a member, i.e.
 * \code
 *  Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
 *  MTK::subblock(m, &Pose::orient) = some_expression;
 * \endcode
 * lets you modify covariance entries in a bigger covariance matrix.
 */
template <class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock_<Base, T, T>::Type subblock_(
    Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>& cov, MTK::SubManifold<T, idx, dim> Base::*) {
    return cov.template block<T::DIM, T::DIM>(dim, dim);
}

template <class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock<Base, T, T>::Type subblock(
    Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>& cov, MTK::SubManifold<T, idx, dim> Base::*) {
    return cov.template block<T::DOF, T::DOF>(idx, idx);
}

template <typename Base>
class get_cov {
   public:
    typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
    typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
};

template <typename Base>
class get_cov_ {
   public:
    typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
    typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
};

template <typename Base1, typename Base2>
class get_cross_cov {
   public:
    typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
    typedef const type const_type;
};

template <typename Base1, typename Base2>
class get_cross_cov_ {
   public:
    typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
    typedef const type const_type;
};

template <class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DIM> subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec,
                                                        SubManifold<T, idx, dim> Base::*) {
    return vec.template segment<T::DIM>(dim);
}

template <class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DOF> subvector_impl(vectview<typename Base::scalar, Base::DOF> vec,
                                                       SubManifold<T, idx, dim> Base::*) {
    return vec.template segment<T::DOF>(idx);
}

/**
 * Get the subvector corresponding to a sub-manifold from a bigger vector.
 */
template <class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM> subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl_(vec, ptr);
}

template <class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF> subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl(vec, ptr);
}

/**
 * @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx>
 * Base::*)
 */
template <class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF> subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
}

template <class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM> subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
}

template <class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DIM> subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec,
                                          SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
}

template <class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF> subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec,
                                         SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
}

/**
 * const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
 */
template <class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DOF> subvector_impl(
    const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*) {
    return cvec.template segment<T::DOF>(idx);
}

template <class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DIM> subvector_impl_(
    const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*) {
    return cvec.template segment<T::DIM>(dim);
}

template <class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF> subvector(const vectview<const Scalar, BaseDOF> cvec,
                                         SubManifold<T, idx, dim> Base::*ptr) {
    return subvector_impl(cvec, ptr);
}

}  // namespace MTK

#endif  // GET_START_INDEX_H_