// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Clemens Groepl $
// $Authors: $
// --------------------------------------------------------------------------

#include <OpenMS/TRANSFORMATIONS/FEATUREFINDER/EmgFitter1D.h>
#include <OpenMS/TRANSFORMATIONS/FEATUREFINDER/InterpolationModel.h>
#include <OpenMS/MATH/STATISTICS/StatisticFunctions.h>
#include <OpenMS/CONCEPT/Constants.h>
#include <OpenMS/CONCEPT/Factory.h>

#include <boost/math/special_functions/fpclassify.hpp>

namespace OpenMS
{
  int EmgFitter1D::EgmFitterFunctor::operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
  {
    Size n = m_data->n;
    EmgFitter1D::RawDataArrayType set = m_data->set;

    EmgFitter1D::CoordinateType h = x(0);
    EmgFitter1D::CoordinateType w = x(1);
    EmgFitter1D::CoordinateType s = x(2);
    EmgFitter1D::CoordinateType z = x(3);

    EmgFitter1D::CoordinateType Yi = 0.0;

    // iterate over all points of the signal
    for (Size i = 0; i < n; i++)
    {
      double t = set[i].getPos();

      // Simplified EMG
      Yi = (h * w / s) * sqrt(2.0 * Constants::PI) * exp((pow(w, 2) / (2 * pow(s, 2))) - ((t - z) / s)) / (1 + exp((-2.4055 / sqrt(2.0)) * (((t - z) / w) - w / s)));

      fvec(i) = Yi - set[i].getIntensity();
    }
    return 0;
  }

  // compute Jacobian matrix for the different parameters
  int EmgFitter1D::EgmFitterFunctor::df(const Eigen::VectorXd& x, Eigen::MatrixXd& J)
  {
    Size n =  m_data->n;
    EmgFitter1D::RawDataArrayType set = m_data->set;

    EmgFitter1D::CoordinateType h = x(0);
    EmgFitter1D::CoordinateType w = x(1);
    EmgFitter1D::CoordinateType s = x(2);
    EmgFitter1D::CoordinateType z = x(3);

    const EmgFitter1D::CoordinateType emg_const = 2.4055;
    const EmgFitter1D::CoordinateType sqrt_2pi = sqrt(2 * Constants::PI);
    const EmgFitter1D::CoordinateType sqrt_2 = sqrt(2.0);

    EmgFitter1D::CoordinateType exp1, exp2, exp3 = 0.0;
    EmgFitter1D::CoordinateType derivative_height, derivative_width, derivative_symmetry, derivative_retention = 0.0;

    // iterate over all points of the signal
    for (Size i = 0; i < n; i++)
    {
      EmgFitter1D::CoordinateType t = set[i].getPos();

      exp1 = exp(((w * w) / (2 * s * s)) - ((t - z) / s));
      exp2 = (1 + exp((-emg_const / sqrt_2) * (((t - z) / w) - w / s)));
      exp3 = exp((-emg_const / sqrt_2) * (((t - z) / w) - w / s));

      // f'(h)
      derivative_height = w / s * sqrt_2pi * exp1 / exp2;

      // f'(h)
      derivative_width = h / s * sqrt_2pi * exp1 / exp2 + (h * w * w) / (s * s * s) * sqrt_2pi * exp1 / exp2 + (emg_const * h * w) / s * sqrt_2pi * exp1 * (-(t - z) / (w * w) - 1 / s) * exp3 / ((exp2 * exp2) * sqrt_2);

      // f'(s)
      derivative_symmetry = -h * w / (s * s) * sqrt_2pi * exp1 / exp2 + h * w / s * sqrt_2pi * (-(w * w) / (s * s * s) + (t - z) / (s * s)) * exp1 / exp2 + (emg_const * h * w * w) / (s * s * s) * sqrt_2pi * exp1 * exp3 / ((exp2 * exp2) * sqrt_2);

      // f'(z)
      derivative_retention = h * w / (s * s) * sqrt_2pi * exp1 / exp2 - (emg_const * h) / s * sqrt_2pi * exp1 * exp3 / ((exp2 * exp2) * sqrt_2);

      // set the jacobian matrix
      J(i, 0) = derivative_height;
      J(i, 1) = derivative_width;
      J(i, 2) = derivative_symmetry;
      J(i, 3) = derivative_retention;
    }
    return 0;
  }

  EmgFitter1D::EmgFitter1D() :
    LevMarqFitter1D()
  {
    setName(getProductName());
    defaults_.setValue("statistics:variance", 1.0, "Variance of the model.", ListUtils::create<String>("advanced"));
    defaultsToParam_();
  }

  EmgFitter1D::EmgFitter1D(const EmgFitter1D& source) :
    LevMarqFitter1D(source)
  {
    setParameters(source.getParameters());
    updateMembers_();
  }

  EmgFitter1D::~EmgFitter1D()
  {
  }

  EmgFitter1D& EmgFitter1D::operator=(const EmgFitter1D& source)
  {
    if (&source == this)
      return *this;

    LevMarqFitter1D::operator=(source);
    setParameters(source.getParameters());
    updateMembers_();

    return *this;
  }

  EmgFitter1D::QualityType EmgFitter1D::fit1d(const RawDataArrayType& set, InterpolationModel*& model)
  {
    // Calculate bounding box
    CoordinateType min_bb = set[0].getPos(), max_bb = set[0].getPos();
    for (Size pos = 1; pos < set.size(); ++pos)
    {
      CoordinateType tmp = set[pos].getPos();
      if (min_bb > tmp)
        min_bb = tmp;
      if (max_bb < tmp)
        max_bb = tmp;
    }

    // Enlarge the bounding box by a few multiples of the standard deviation
    const CoordinateType stdev = sqrt(statistics_.variance()) * tolerance_stdev_box_;
    min_bb -= stdev;
    max_bb += stdev;

    // Set advanced parameters for residual_  und jacobian_ method
    EmgFitter1D::Data d;
    d.n = set.size();
    d.set = set;

    // Compute start parameters
    setInitialParameters_(set);

    // Optimize parameter with Levenberg-Marquardt algorithm
//    CoordinateType x_init[4] = { height_, width_, symmetry_, retention_ };
    Eigen::VectorXd x_init(4);
    x_init(0) = height_;
    x_init(1) = width_;
    x_init(2) = symmetry_;
    x_init(3) = retention_;
    if (symmetric_ == false)
    {
      EgmFitterFunctor functor(4, &d);
      optimize_(x_init, functor);
    }

    // Set optimized parameters
    height_ = x_init[0];
    width_ = x_init[1];
    symmetry_ = x_init[2];
    retention_ = x_init[3];

#ifdef DEBUG_FEATUREFINDER
    if (getGslStatus_() != "success")
    {
      std::cout << "status: " << getGslStatus_() << std::endl;
    }
#endif

    // build model
    model = static_cast<InterpolationModel*>(Factory<BaseModel<1> >::create("EmgModel"));
    model->setInterpolationStep(interpolation_step_);

    Param tmp;
    tmp.setValue("bounding_box:min", min_bb);
    tmp.setValue("bounding_box:max", max_bb);
    tmp.setValue("statistics:variance", statistics_.variance());
    tmp.setValue("statistics:mean", statistics_.mean());
    tmp.setValue("emg:height", height_);
    tmp.setValue("emg:width", width_);
    tmp.setValue("emg:symmetry", symmetry_);
    tmp.setValue("emg:retention", retention_);
    model->setParameters(tmp);

    // calculate pearson correlation
    std::vector<float> real_data;
    real_data.reserve(set.size());
    std::vector<float> model_data;
    model_data.reserve(set.size());

    for (Size i = 0; i < set.size(); ++i)
    {
      real_data.push_back(set[i].getIntensity());
      model_data.push_back(model->getIntensity(DPosition<1>(set[i].getPosition())));
    }

    QualityType correlation = Math::pearsonCorrelationCoefficient(real_data.begin(), real_data.end(), model_data.begin(), model_data.end());
    if (boost::math::isnan(correlation))
      correlation = -1.0;

    return correlation;
  }

  void EmgFitter1D::setInitialParameters_(const RawDataArrayType& set)
  {
    // sum over all intensities
    CoordinateType sum = 0.0;
    for (Size i = 0; i < set.size(); ++i)
      sum += set[i].getIntensity();

    // calculate the median
    Size median = 0;
    float count = 0.0;
    for (Size i = 0; i < set.size(); ++i)
    {
      count += set[i].getIntensity();
      if (count <= sum / 2)
        median = i;
    }

    // calculate the height of the peak
    height_ = set[median].getIntensity();

    // calculate retention time
    retention_ = set[median].getPos();

    // default is an asymmetric peak
    symmetric_ = false;

    // calculate the symmetry (fronted peak: s<1 , tailed peak: s>1)
    symmetry_ = fabs(set[set.size() - 1].getPos() - set[median].getPos()) / fabs(set[median].getPos() - set[0].getPos());

    // check the symmetry
    if (boost::math::isinf(symmetry_) || boost::math::isnan(symmetry_))
    {
      symmetric_ = true;
      symmetry_ = 10;
    }

    // optimize the symmetry
    // The computations can lead to an overflow error at very low values of symmetry (s~0).
    // For s~5 the parameter can be approximated by the Levenberg-Marquardt algorithms.
    // (the other parameters are much greater than one)
    if (symmetry_ < 1)
      symmetry_ += 5;

    // calculate the width of the peak
    // rt-values with intensity zero are not allowed for calculation of the width
    // normally: width_ = fabs( set[set.size() - 1].getPos() - set[0].getPos() );
    // but its better for the emg function to proceed from narrow peaks
    width_ = symmetry_;
  }

  void EmgFitter1D::updateMembers_()
  {
    LevMarqFitter1D::updateMembers_();
    statistics_.setVariance(param_.getValue("statistics:variance"));
  }

}


#include <gamebase/engine/SimpleApplication.h>
#include <gamebase/engine/CanvasLayout.h>
#include <gamebase/engine/Button.h>
#include <gamebase/engine/StaticLabel.h>
#include <gamebase/engine/StaticTextureRect.h>
#include <gamebase/engine/Timer.h>
#include <gamebase/math/IntVector.h>
#include <gamebase/serial/JsonDeserializer.h>
#include <boost/lexical_cast.hpp>
#include <Windows.h>

using namespace gamebase;
using namespace std;

class Figure
{
public:
    Figure() {}

    Figure(int startYArg, const Color& colorArg, const vector<IntVec2>& geomArg)
    {
        startY = startYArg;
        color = colorArg;
        geom = geomArg;
    }

    void rotateLeft()
    {
        for (auto it = geom.begin(); it != geom.end(); ++it)
            *it = rotate90CCW(*it);
    }

    void rotateRight()
    {
        for (auto it = geom.begin(); it != geom.end(); ++it)
            *it = rotate90CW(*it);
    }

    int startY;
    Color color;
    vector<IntVec2> geom;
};

class MyApp : public SimpleApplication
{
public:
    void load()
    {
        srand(GetTickCount());

        design = deserialize<CanvasLayout>("tetris\\Design.json");
        m_view->addObject(design);

        fieldLayout = design->getChild<CanvasLayout>("#field");

        design->getChild<Button>("#restart")->setCallback(bind(&MyApp::restart, this));

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(0, 1));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(0, -1));
            geom.push_back(IntVec2(0, -2));
            figures.push_back(Figure(18, Color(1, 0, 0), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(1, 1));
            geom.push_back(IntVec2(0, 1));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(0, -1));
            figures.push_back(Figure(18, Color(0, 1, 0), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(-1, 1));
            geom.push_back(IntVec2(0, 1));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(0, -1));
            figures.push_back(Figure(18, Color(0, 0, 1), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(0, 1));
            geom.push_back(IntVec2(-1, 0));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(1, 0));
            figures.push_back(Figure(18, Color(1, 0, 1), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(1, 1));
            geom.push_back(IntVec2(0, 1));
            geom.push_back(IntVec2(1, 0));
            geom.push_back(IntVec2(0, 0));
            figures.push_back(Figure(18, Color(1, 1, 0), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(1, 0));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(0, -1));
            geom.push_back(IntVec2(-1, -1));
            figures.push_back(Figure(18, Color(0, 1, 1), geom));
        }

        {
            vector<IntVec2> geom;
            geom.push_back(IntVec2(-1, 0));
            geom.push_back(IntVec2(0, 0));
            geom.push_back(IntVec2(0, -1));
            geom.push_back(IntVec2(1, -1));
            figures.push_back(Figure(18, Color(1, 0.5, 0), geom));
        }

        record = 0;
        restart();
    }

    void restart()
    {
        gameover = false;
        design->getChild<StaticLabel>("#gameover")->setVisible(false);
        score = 0;
        design->getChild<StaticLabel>("#score")->setText("0");
        timer.start();
        fieldLayout->clear();

        for (int x = 0; x < 10; ++x)
            for (int y = 0; y < 20; ++y)
                field[x][y] = -1;

        newFigure();
    }

    void move()
    {
        if (gameover)
            return;

        if (timer.isPeriod(period))
        {
            if (!tryUpdatePos(0, -1))
            {
                for (int i = 0; i < curFigure.geom.size(); ++i)
                {
                    IntVec2 fullPos = curPos + curFigure.geom[i];
                    field[fullPos.x][fullPos.y] = textureIds[i];
                }

                vector<int> rawsToBurn;
                for (int y = 0; y < 20; ++y)
                {
                    bool hasHole = false;
                    for (int x = 0; x < 10; ++x)
                    {
                        if (field[x][y] == -1)
                            hasHole = true;
                    }
                    if (!hasHole)
                        rawsToBurn.push_back(y);
                }

                for (auto it = rawsToBurn.rbegin(); it != rawsToBurn.rend(); ++it)
                {
                    int rawY = *it;
                    for (int x = 0; x < 10; ++x)
                        fieldLayout->removeObject(field[x][rawY]);
                    for (int y = rawY; y < 19; ++y)
                    {
                        for (int x = 0; x < 10; ++x)
                        {
                            field[x][y] = field[x][y + 1];
                            if (field[x][y] != -1)
                                adjustTile(field[x][y], IntVec2(x, y));
                        }
                    }
                    for (int x = 0; x < 10; ++x)
                        field[x][19] = -1;
                }

                switch (rawsToBurn.size())
                {
                case 1: score += 100; break;
                case 2: score += 300; break;
                case 3: score += 700; break;
                case 4: score += 1500; break;
                }

                if (!rawsToBurn.empty())
                {
                    design->getChild<StaticLabel>("#score")->setText(
                        boost::lexical_cast<string>(score));
                }

                newFigure();
            }
        }

        if (m_inputRegister.keys.isJustPressed(13) || m_inputRegister.keys.isJustPressed('s'))
        {
            if (period != 50)
            {
                timer.start();
                period = 50;
            }
        }

        if (m_inputRegister.keys.isJustPressed('a'))
            tryUpdatePos(-1, 0);
        if (m_inputRegister.keys.isJustPressed('d'))
            tryUpdatePos(1, 0);

        if (m_inputRegister.keys.isJustPressed('q'))
        {
            Figure newFigure = curFigure;
            newFigure.rotateLeft();
            if (canPlace(curPos, newFigure))
            {
                curFigure = newFigure;
                adjustCurTiles();
            }
        }

        if (m_inputRegister.keys.isJustPressed('e'))
        {
            Figure newFigure = curFigure;
            newFigure.rotateRight();
            if (canPlace(curPos, newFigure))
            {
                curFigure = newFigure;
                adjustCurTiles();
            }
        }
    }

    bool tryUpdatePos(int xOffset, int yOffset)
    {
        auto newPos = curPos;
        newPos.x += xOffset;
        newPos.y += yOffset;
        if (canPlace(newPos, curFigure))
        {
            curPos = newPos;
            adjustCurTiles();
            return true;
        }
        return false;
    }

    void newFigure()
    {
        period = 500;
        curFigure = figures[rand() % figures.size()];
        curPos.y = curFigure.startY;
        if (canPlace(IntVec2(5, curPos.y), curFigure))
        {
            curPos.x = 5;
        }
        else
        {
            gameover = true;
            design->getChild<StaticLabel>("#gameover")->setVisible(true);
            if (score > record)
            {
                record = score;
                design->getChild<StaticLabel>("#record")->setText(
                    boost::lexical_cast<string>(record));
            }
            return;
        }

        textureIds.clear();
        for (int i = 0; i < curFigure.geom.size(); ++i)
        {
            auto tile = deserialize<StaticTextureRect>("tetris\\Tile.json");
            tile->setColor(curFigure.color);
            textureIds.push_back(fieldLayout->addObject(tile));
        }
        adjustCurTiles();
    }

    bool canPlace(const IntVec2& pos, const Figure& figure)
    {
        for (auto it = figure.geom.begin(); it != figure.geom.end(); ++it)
        {
            IntVec2 fullPos = pos + *it;
            if (fullPos.x < 0 || fullPos.x > 9)
                return false;
            if (fullPos.y < 0)
                return false;
            if (field[fullPos.x][fullPos.y] != -1)
                return false;
        }
        return true;
    }

    void adjustCurTiles()
    {
        for (int i = 0; i < textureIds.size(); ++i)
        {
            IntVec2 fullPos = curPos + curFigure.geom[i];
            adjustTile(textureIds[i], fullPos);
        }
    }

    void adjustTile(int id, const IntVec2& pos)
    {
        Vec2 vec(pos.x * 32 - 144, pos.y * 32 - 304);
        auto* tile = fieldLayout->getObject<StaticTextureRect>(id);
        auto* offset = tile->offset<FixedOffset>();
        offset->set(vec);
    }

    shared_ptr<CanvasLayout> design;
    CanvasLayout* fieldLayout;

    Figure curFigure;
    vector<int> textureIds;
    IntVec2 curPos;

    int score;
    int record;

    bool gameover;

    int field[10][20];
    Timer timer;
    int period;

    vector<Figure> figures;
};

int main(int argc, char** argv)
{
    MyApp app;
    if (!app.init(&argc, argv))
        return 1;
    app.run();
    return 0;
}


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

#include "Open3D/Geometry/PointCloud.h"

#include <Eigen/Dense>

#include "Open3D/Geometry/KDTreeFlann.h"
#include "Open3D/Utility/Console.h"

namespace open3d {
namespace geometry {

void PointCloud::Clear() {
    points_.clear();
    normals_.clear();
    colors_.clear();
}

bool PointCloud::IsEmpty() const { return !HasPoints(); }

Eigen::Vector3d PointCloud::GetMinBound() const {
    if (!HasPoints()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    auto itr_x = std::min_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(0) < b(0);
            });
    auto itr_y = std::min_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(1) < b(1);
            });
    auto itr_z = std::min_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(2) < b(2);
            });
    return Eigen::Vector3d((*itr_x)(0), (*itr_y)(1), (*itr_z)(2));
}

Eigen::Vector3d PointCloud::GetMaxBound() const {
    if (!HasPoints()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    auto itr_x = std::max_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(0) < b(0);
            });
    auto itr_y = std::max_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(1) < b(1);
            });
    auto itr_z = std::max_element(
            points_.begin(), points_.end(),
            [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) {
                return a(2) < b(2);
            });
    return Eigen::Vector3d((*itr_x)(0), (*itr_y)(1), (*itr_z)(2));
}

void PointCloud::Transform(const Eigen::Matrix4d &transformation) {
    for (auto &point : points_) {
        Eigen::Vector4d new_point =
                transformation *
                Eigen::Vector4d(point(0), point(1), point(2), 1.0);
        point = new_point.block<3, 1>(0, 0);
    }
    for (auto &normal : normals_) {
        Eigen::Vector4d new_normal =
                transformation *
                Eigen::Vector4d(normal(0), normal(1), normal(2), 0.0);
        normal = new_normal.block<3, 1>(0, 0);
    }
}

PointCloud &PointCloud::operator+=(const PointCloud &cloud) {
    // We do not use std::vector::insert to combine std::vector because it will
    // crash if the pointcloud is added to itself.
    if (cloud.IsEmpty()) return (*this);
    size_t old_vert_num = points_.size();
    size_t add_vert_num = cloud.points_.size();
    size_t new_vert_num = old_vert_num + add_vert_num;
    if ((!HasPoints() || HasNormals()) && cloud.HasNormals()) {
        normals_.resize(new_vert_num);
        for (size_t i = 0; i < add_vert_num; i++)
            normals_[old_vert_num + i] = cloud.normals_[i];
    } else {
        normals_.clear();
    }
    if ((!HasPoints() || HasColors()) && cloud.HasColors()) {
        colors_.resize(new_vert_num);
        for (size_t i = 0; i < add_vert_num; i++)
            colors_[old_vert_num + i] = cloud.colors_[i];
    } else {
        colors_.clear();
    }
    points_.resize(new_vert_num);
    for (size_t i = 0; i < add_vert_num; i++)
        points_[old_vert_num + i] = cloud.points_[i];
    return (*this);
}

PointCloud PointCloud::operator+(const PointCloud &cloud) const {
    return (PointCloud(*this) += cloud);
}

std::vector<double> ComputePointCloudToPointCloudDistance(
        const PointCloud &source, const PointCloud &target) {
    std::vector<double> distances(source.points_.size());
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)source.points_.size(); i++) {
        std::vector<int> indices(1);
        std::vector<double> dists(1);
        if (kdtree.SearchKNN(source.points_[i], 1, indices, dists) == 0) {
            utility::PrintDebug(
                    "[ComputePointCloudToPointCloudDistance] Found a point "
                    "without neighbors.\n");
            distances[i] = 0.0;
        } else {
            distances[i] = std::sqrt(dists[0]);
        }
    }
    return distances;
}

std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputePointCloudMeanAndCovariance(
        const PointCloud &input) {
    if (input.IsEmpty()) {
        return std::make_tuple(Eigen::Vector3d::Zero(),
                               Eigen::Matrix3d::Identity());
    }
    Eigen::Matrix<double, 9, 1> cumulants;
    cumulants.setZero();
    for (const auto &point : input.points_) {
        cumulants(0) += point(0);
        cumulants(1) += point(1);
        cumulants(2) += point(2);
        cumulants(3) += point(0) * point(0);
        cumulants(4) += point(0) * point(1);
        cumulants(5) += point(0) * point(2);
        cumulants(6) += point(1) * point(1);
        cumulants(7) += point(1) * point(2);
        cumulants(8) += point(2) * point(2);
    }
    cumulants /= (double)input.points_.size();
    Eigen::Vector3d mean;
    Eigen::Matrix3d covariance;
    mean(0) = cumulants(0);
    mean(1) = cumulants(1);
    mean(2) = cumulants(2);
    covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
    covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
    covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
    covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
    covariance(1, 0) = covariance(0, 1);
    covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
    covariance(2, 0) = covariance(0, 2);
    covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
    covariance(2, 1) = covariance(1, 2);
    return std::make_tuple(mean, covariance);
}

std::vector<double> ComputePointCloudMahalanobisDistance(
        const PointCloud &input) {
    std::vector<double> mahalanobis(input.points_.size());
    Eigen::Vector3d mean;
    Eigen::Matrix3d covariance;
    std::tie(mean, covariance) = ComputePointCloudMeanAndCovariance(input);
    Eigen::Matrix3d cov_inv = covariance.inverse();
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)input.points_.size(); i++) {
        Eigen::Vector3d p = input.points_[i] - mean;
        mahalanobis[i] = std::sqrt(p.transpose() * cov_inv * p);
    }
    return mahalanobis;
}

std::vector<double> ComputePointCloudNearestNeighborDistance(
        const PointCloud &input) {
    std::vector<double> nn_dis(input.points_.size());
    KDTreeFlann kdtree(input);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i = 0; i < (int)input.points_.size(); i++) {
        std::vector<int> indices(2);
        std::vector<double> dists(2);
        if (kdtree.SearchKNN(input.points_[i], 2, indices, dists) <= 1) {
            utility::PrintDebug(
                    "[ComputePointCloudNearestNeighborDistance] Found a point "
                    "without neighbors.\n");
            nn_dis[i] = 0.0;
        } else {
            nn_dis[i] = std::sqrt(dists[1]);
        }
    }
    return nn_dis;
}

}  // namespace geometry
}  // namespace open3d


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

#include "gtest/gtest.h"

#include <Eigen/Core>
#include "cpp/sensor_fusion/low_pass_filter.h"

namespace mahony_filter {

TEST(LowPassFilterTest, CreateFilter) {
  LowPassFilter filter(1.0);
  EXPECT_FALSE(filter.IsInitialized());
  EXPECT_FALSE(filter.GetIsStatic());
  EXPECT_EQ(filter.GetNStatic(), 0);
  EXPECT_EQ(filter.GetRunTime(), 0.0);
}

TEST(LowPassFilterTest, AddSampleDataToNewFilter) {
  LowPassFilter filter(1.0);
  Eigen::Vector3d sample(1.0, 0.0, 0.0);
  filter.AddSampleData(sample, 1.0);

  EXPECT_EQ(filter.GetFilteredData(), sample);
  EXPECT_EQ(filter.GetFilteredDataNorm(), 1.0);
  EXPECT_EQ(filter.GetFilteredDataDirection(), sample);
  EXPECT_NE(filter.GetLastData(), sample);
  EXPECT_EQ(filter.GetRunTime(), 0.0)
      << "The first sample shouldn't change run_time_s_.";

  // Adds another sample
  filter.AddSampleData(Eigen::Vector3d(3.0, 4.0, 0.0), 0.5);
  EXPECT_EQ(filter.GetRunTime(), 0.5);
  EXPECT_EQ(filter.GetLastData(), Eigen::Vector3d(3.0, 4.0, 0.0));
  EXPECT_EQ(filter.GetLastDataDirection(), Eigen::Vector3d(0.6, 0.8, 0.0));
}

TEST(LowPassFilterTest, CheckStatic) {
  LowPassFilter filter(1.0);
  filter.SetIsStatic(true);
  EXPECT_EQ(filter.GetNStatic(), 1);

  for (int i = 0; i < kContiguousStaticSamples - 2; i++) {
    filter.SetIsStatic(true);
    EXPECT_FALSE(filter.GetIsStatic());
  }
  filter.SetIsStatic(true);
  EXPECT_TRUE(filter.GetIsStatic());
  EXPECT_EQ(filter.GetNStatic(), kContiguousStaticSamples);
  EXPECT_FALSE(filter.GetIsStaticForN(kContiguousStaticSamples + 1));

  filter.SetIsStatic(false);

  for (int i = 0; i < kContiguousStaticSamples - 1; i++) {
    filter.SetIsStatic(true);
    EXPECT_FALSE(filter.GetIsStatic());
  }
  filter.SetIsStatic(true);
  EXPECT_TRUE(filter.GetIsStatic());
  EXPECT_EQ(filter.GetNStatic(), kContiguousStaticSamples);
}

TEST(LowPassFilterTest, AddSampleDataTillSettled) {
  // startup_time_s_ is 1.0.
  LowPassFilter filter(1.0);
  filter.AddSampleData(Eigen::Vector3d(1.0, 0.0, 0.0), 1.0);
  EXPECT_FALSE(filter.HasSettled());
  filter.AddSampleData(Eigen::Vector3d(1.0, 0.0, 0.0), 2.0);
  EXPECT_TRUE(filter.HasSettled());
}

TEST(LowPassFilterTest, ResetSettledFilter) {
  LowPassFilter filter(1.0);
  filter.AddSampleData(Eigen::Vector3d(1.0, 0.0, 0.0), 1.0);
  filter.AddSampleData(Eigen::Vector3d(1.0, 0.0, 0.0), 2.0);
  EXPECT_TRUE(filter.HasSettled());

  filter.Reset();

  EXPECT_FALSE(filter.HasSettled());
  EXPECT_FALSE(filter.IsInitialized());
  EXPECT_FALSE(filter.GetIsStatic());
  EXPECT_EQ(filter.GetNStatic(), 0);
  EXPECT_EQ(filter.GetRunTime(), 0);
}

}  // namespace mahony_filter


#ifndef STAN_MCMC_HMC_BASE_HMC_HPP
#define STAN_MCMC_HMC_BASE_HMC_HPP

#include <stan/services/callbacks/logger.hpp>
#include <stan/services/callbacks/writer.hpp>
#include "stan/algorithms/mcmc/base_mcmc.hpp"
#include "stan/algorithms/hmc/hamiltonians/ps_point.hpp"
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_01.hpp>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <string>
#include <vector>

namespace stan {
  namespace mcmc {

    template <class Model,
              template<class, class> class Hamiltonian,
              template<class> class Integrator,
              class BaseRNG>
    class base_hmc : public base_mcmc {
    public:
      base_hmc(const Model &model, BaseRNG& rng)
        : base_mcmc(),
          z_(model.num_params_r()),
          integrator_(),
          hamiltonian_(model),
          rand_int_(rng),
          rand_uniform_(rand_int_),
          nom_epsilon_(0.1),
          epsilon_(nom_epsilon_),
          epsilon_jitter_(0.0) {}

      /**
       * format and write stepsize
       */
      void
      write_sampler_stepsize(callbacks::writer& writer) {
        std::stringstream nominal_stepsize;
        nominal_stepsize << "Step size = " << get_nominal_stepsize();
        writer(nominal_stepsize.str());
      }

      /**
       * write elements of mass matrix
       */
      void
      write_sampler_metric(callbacks::writer& writer) {
        z_.write_metric(writer);
      }

      /**
       * write stepsize and elements of mass matrix
       */
      void
      write_sampler_state(callbacks::writer& writer) {
        write_sampler_stepsize(writer);
        write_sampler_metric(writer);
      }

      void get_sampler_diagnostic_names(std::vector<std::string>& model_names,
                                        std::vector<std::string>& names) {
        z_.get_param_names(model_names, names);
      }

      void get_sampler_diagnostics(std::vector<double>& values) {
        z_.get_params(values);
      }

      void seed(const Eigen::VectorXd& q) {
        z_.q = q;
      }

      void
      init_hamiltonian(callbacks::logger& logger) {
        this->hamiltonian_.init(this->z_, logger);
      }

      void
      init_stepsize(callbacks::logger& logger) {
        ps_point z_init(this->z_);

        // Skip initialization for extreme step sizes
        if (this->nom_epsilon_ == 0 || this->nom_epsilon_ > 1e7)
          return;

        this->hamiltonian_.sample_p(this->z_, this->rand_int_);
        this->hamiltonian_.init(this->z_, logger);

        // Guaranteed to be finite if randomly initialized
        double H0 = this->hamiltonian_.H(this->z_);

        this->integrator_.evolve(this->z_, this->hamiltonian_,
                                 this->nom_epsilon_,
                                 logger);

        double h = this->hamiltonian_.H(this->z_);
        if (boost::math::isnan(h))
          h = std::numeric_limits<double>::infinity();

        double delta_H = H0 - h;

        int direction = delta_H > std::log(0.8) ? 1 : -1;

        while (1) {
          this->z_.ps_point::operator=(z_init);

          this->hamiltonian_.sample_p(this->z_, this->rand_int_);
          this->hamiltonian_.init(this->z_, logger);

          double H0 = this->hamiltonian_.H(this->z_);

          this->integrator_.evolve(this->z_, this->hamiltonian_,
                                   this->nom_epsilon_,
                                   logger);

          double h = this->hamiltonian_.H(this->z_);
          if (boost::math::isnan(h))
            h = std::numeric_limits<double>::infinity();

          double delta_H = H0 - h;

          if ((direction == 1) && !(delta_H > std::log(0.8)))
            break;
          else if ((direction == -1) && !(delta_H < std::log(0.8)))
            break;
          else
            this->nom_epsilon_
              = direction == 1
              ? 2.0 * this->nom_epsilon_
              : 0.5 * this->nom_epsilon_;

          if (this->nom_epsilon_ > 1e7)
            throw std::runtime_error("Posterior is improper. "
                                     "Please check your model.");
          if (this->nom_epsilon_ == 0)
            throw std::runtime_error("No acceptably small step size could "
                                     "be found. Perhaps the posterior is "
                                     "not continuous?");
        }

        this->z_.ps_point::operator=(z_init);
      }

      typename Hamiltonian<Model, BaseRNG>::PointType& z() {
        return z_;
      }

      virtual void set_nominal_stepsize(double e) {
        if (e > 0)
          nom_epsilon_ = e;
      }

      double get_nominal_stepsize() {
        return this->nom_epsilon_;
      }

      double get_current_stepsize() {
        return this->epsilon_;
      }

      virtual void set_stepsize_jitter(double j) {
        if (j > 0 && j < 1)
          epsilon_jitter_ = j;
      }

      double get_stepsize_jitter() {
        return this->epsilon_jitter_;
      }

      void sample_stepsize() {
        this->epsilon_ = this->nom_epsilon_;
        if (this->epsilon_jitter_)
          this->epsilon_ *= 1.0
            + this->epsilon_jitter_ * (2.0 * this->rand_uniform_() - 1.0);
      }

    protected:
      typename Hamiltonian<Model, BaseRNG>::PointType z_;
      Integrator<Hamiltonian<Model, BaseRNG> > integrator_;
      Hamiltonian<Model, BaseRNG> hamiltonian_;

      BaseRNG& rand_int_;

      // Uniform(0, 1) RNG
      boost::uniform_01<BaseRNG&> rand_uniform_;

      double nom_epsilon_;
      double epsilon_;
      double epsilon_jitter_;
    };

  }  // mcmc
}  // stan
#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/translation_search/fast_terms.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace cctbx { namespace translation_search { namespace boost_python {

namespace {

  struct fast_terms_wrappers
  {
    typedef fast_terms<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_internal_reference<> rir;
      class_<w_t>("fast_terms", no_init)
        .def(init<af::int3 const&,
                  bool,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<std::complex<double> > >(
          (arg("gridding"),
           arg("anomalous_flag"),
           arg("miller_indices_p1_f_calc"),
           arg("p1_f_calc"))))
        .def("summation", &w_t::summation, rir(),
          (arg("space_group"),
           arg("miller_indices_f_obs"),
           arg("m"),
           arg("f_part"),
           arg("squared_flag")))
        .def("fft", &w_t::fft, rir())
        .def("accu_real_copy", &w_t::accu_real_copy)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_fast_terms()
  {
    fast_terms_wrappers::wrap();
  }

}}} // namespace cctbx::translation_search::boost_python