//
// Created by Lu WJ on 14/02/2017.
//
#include "core/PPE/EncMat.hpp"
#include "core/PPE/EncVec.hpp"
#include "core/PPE/PubKey.hpp"
#include "core/PPE/SecKey.hpp"
#include "core/algebra/util.hpp"

#include "HElib/EncryptedArray.h"

#include <memory>
#include <NTL/vec_ZZ.h>

typedef NTL::vec_ZZ Vector;
namespace ppe {
class EncMat::Imp {
public:
    Imp(const ppe::PubKey &pk) : colNum_(0), rowNum_(0), pk_(pk) {
        getPrimes(pk_);
        getEncryptedArrays(pk_);
        for (int i = 0; i < pk_.partsNum(); i++) {
            crtParts_.push_back(core::EncMat(pk_.get(i)));
        }
    }

    Imp(const Imp &oth) {
        operator=(oth);
    }

    std::shared_ptr<Imp> copyAsEmpty() const {
        auto dummy = std::make_shared<Imp>(pk_);
        dummy->colNum_ = colNum_;
        dummy->ea_ = ea_;
        dummy->pk_ = pk_;
        dummy->primes_ = primes_;
        dummy->colNum_ = colNum_;
        dummy->rowNum_ = rowNum_;
        return dummy;
    }

    Imp& operator=(const Imp &oth) {
        ea_ = oth.ea_;
        pk_ = oth.pk_;
        primes_ = oth.primes_;
        colNum_ = oth.colNum_;
        rowNum_ = oth.rowNum_;
        crtParts_ = oth.crtParts_;
        return *this;
    }

    bool pack(const Matrix &mat) {
        #pragma omp parallel for
        for (size_t i = 0; i < crtParts_.size(); i++)
            crtParts_.at(i).pack(mat);

        rowNum_ = mat.NumRows();
        colNum_ = mat.NumCols();
        return true;
    }

    bool unpack(Matrix &result, const SecKey &sk, const bool negate) const {
        if (crtParts_.empty())
            return false;
        std::vector<Matrix> alphas(crtParts_.size());
        #pragma omp parallel for
        for (size_t i = 0; i < sk.partsNum(); i++)
            crtParts_.at(i).unpack(alphas.at(i), sk.get(i), false);
        return apply_crt(result, alphas, negate);
    }

    bool add(const std::shared_ptr<Imp> &oth) {
        assert(pk_ == oth->pk_);
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = oth->colNums();
            rowNum_ = oth->rowNums();
        } else if(colNums() != oth->colNums() || rowNums() != oth->rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::add()" << std::endl;
            return false;
        }
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].add(oth->crtParts_.at(i));
        return true;
    }

    bool add(const Matrix &mat) {
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = mat.NumCols();
            rowNum_ = mat.NumRows();
        } else if (mat.NumRows() != rowNums() || mat.NumRows() != rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::add()" << std::endl;
            return false;
        }
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].add(mat);
        return true;
    }

    bool sub(const std::shared_ptr<Imp> &oth) {
        assert(pk_ == oth->pk_);
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = oth->colNums();
            rowNum_ = oth->rowNums();
        } else if(colNums() != oth->colNums() || rowNums() != oth->rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::sub()" << std::endl;
            return false;
        }
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].sub(oth->crtParts_.at(i));
        return true;
    }

    bool sub(const Matrix &mat) {
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = mat.NumCols();
            rowNum_ = mat.NumRows();
        } else if (mat.NumRows() != rowNums() || mat.NumRows() != rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::sub()" << std::endl;
            return false;
        }
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].sub(mat);
        return true;
    }

    bool mul(const Matrix &mat) {
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = mat.NumCols();
            rowNum_ = mat.NumRows();
        } else if (mat.NumRows() != rowNums() || mat.NumRows() != rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::mul()" << std::endl;
            return false;
        }
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].mul(mat);
        return true;
    }

    bool dot(const std::shared_ptr<Imp> &oth) {
        assert(pk_ == oth->pk_);
        if (colNums() == 0 && rowNums() == 0) {
            colNum_ = oth->colNums();
            rowNum_ = oth->rowNums();
        } else if(colNums() != oth->colNums() || rowNums() != oth->rowNums()) {
            std::cerr << "Mismatch matrix size in EncMat::dot()" << std::endl;
            return false;
        }
        #pragma omp parallel for
        for (size_t i = 0; i < this->crtParts_.size(); i++)
            this->crtParts_[i].dot(oth->crtParts_[i]);
        return true;
    }

    ppe::EncVec sym_dot(const ppe::EncVec &vec) {
        assert(colNums() == vec.length());
        ppe::EncVec ret(pk_);
        std::vector<core::EncVec> parts(crtParts_.size());
        #pragma omp parallel for
        for (size_t i = 0; i < parts.size(); i++)
            parts[i] = crtParts_[i].sym_dot(vec.getCRTPartAt(i));
        ret.directSetup(parts);
        return ret;
    }

    void negate() {
        for (size_t i = 0; i < crtParts_.size(); i++)
            crtParts_[i].negate();
    }

    bool dump(std::ostream &out) const {
        out << rowNum_ << " " << colNum_ << std::endl;
        for (const auto &part : crtParts_)
            part.dump(out);
        return true;
    }

    bool restore(std::istream &in) {
        in >> rowNum_ >> colNum_;
        for (auto &part : crtParts_)
            part.restore(in);
        return true;
    }

    long colNums() const { return colNum_; }

    long rowNums() const { return rowNum_; }

private:
    void getEncryptedArrays(const PubKey &pk) {
        ea_.resize(pk.partsNum());
        for (size_t i = 0; i < ea_.size(); i++) {
            ea_[i] = pk.get(i)->getContext().ea;
        }
    }

    void getPrimes(const PubKey &pk) {
        primes_.resize(pk.partsNum());
        for (size_t i = 0; i < primes_.size(); i++) {
            primes_[i] = NTL::to_ZZ(pk.get(i)->getContext().alMod.getPPowR());
        }
    }

    bool apply_crt(Matrix &result, const std::vector<Matrix> &plains, const bool negate) const {
        if (plains.empty() || plains.size() > primes_.size()) {
            std::cerr << "Invalid parameter for apply_crt\n";
            return false;
        }

        const long rowNum = plains.front().NumRows();
        const long colNum = plains.front().NumCols();
        for (const auto &m : plains) {
           if (m.NumRows() != rowNum || m.NumCols() != colNum)
               return false;
        }
        std::vector<NTL::ZZ> alphas(plains.size());
        result.SetDims(rowNum, colNum);

        for (long rr = 0; rr < rowNum; rr++) {
            for (long cc = 0; cc < colNum; cc++) {
                for (size_t i = 0; i < plains.size(); i++) {
                    alphas.at(i) = plains.at(i).get(rr, cc);
                }
                result[rr][cc] = algebra::apply_crt(alphas, primes_, negate);
            }
        }
        return true;
    }

private:
    long rowNum_, colNum_;
    ppe::PubKey pk_;
    std::vector<NTL::ZZ> primes_;
    std::vector<const EncryptedArray *> ea_;
    std::vector<core::EncMat> crtParts_;
};

EncMat::EncMat(const PubKey &pk) {
    imp_ = std::make_shared<Imp>(pk);
}

EncMat::EncMat(const EncMat &oth) {
    imp_ = std::make_shared<Imp>(*oth.imp_);
}

EncMat EncMat::copyAsEmpty() const {
    EncMat dummy;
    dummy.imp_ = imp_->copyAsEmpty();
    return dummy;
}

EncMat& EncMat::operator=(const EncMat &oth) {
    imp_ = std::make_shared<Imp>(*oth.imp_);
    return *this;
}

long EncMat::rowNums() const {
    return imp_->rowNums();
}

long EncMat::colNums() const {
    return imp_->colNums();
}

EncMat &EncMat::pack(const Matrix &mat) {
    imp_->pack(mat);
    return *this;
}

bool EncMat::unpack(Matrix &result, const SecKey &sk, bool negate) const {
    return imp_->unpack(result, sk, negate);
}

EncMat& EncMat::add(const EncMat &oth) {
    imp_->add(oth.imp_);
    return *this;
}

EncMat& EncMat::add(const Matrix &c) {
    imp_->add(c);
    return *this;
}

EncMat& EncMat::sub(const EncMat &oth) {
    imp_->sub(oth.imp_);
    return *this;
}

EncMat& EncMat::sub(const Matrix &c) {
    imp_->sub(c);
    return *this;
}

EncMat& EncMat::dot(const EncMat &oth) {
    imp_->dot(oth.imp_);
    return *this;
}

EncVec EncMat::sym_dot(const EncVec &oth) const {
    return imp_->sym_dot(oth);
}

EncMat& EncMat::mul(const Matrix &oth) {
    imp_->mul(oth);
    return *this;
}

EncMat& EncMat::negate() {
    imp_->negate();
    return *this;
}

bool EncMat::dump(std::ostream &out) const {
   return imp_->dump(out);
}

bool EncMat::restore(std::istream &in) {
   return imp_->restore(in);
}
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under 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)

#ifndef BOOST_UNITS_ABSTRACT_HPP
#define BOOST_UNITS_ABSTRACT_HPP

#include <string>

#include <boost/units/conversion.hpp>
#include <boost/units/unit.hpp>

#include <boost/units/make_system.hpp>
#include <boost/units/base_unit.hpp>

#include <boost/units/physical_dimensions/amount.hpp>
#include <boost/units/physical_dimensions/current.hpp>
#include <boost/units/physical_dimensions/length.hpp>
#include <boost/units/physical_dimensions/luminous_intensity.hpp>
#include <boost/units/physical_dimensions/mass.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/units/physical_dimensions/solid_angle.hpp>
#include <boost/units/physical_dimensions/temperature.hpp>
#include <boost/units/physical_dimensions/time.hpp>

namespace boost {

namespace units {

namespace abstract {

struct length_unit_tag : base_unit<length_unit_tag, length_dimension, -30> { };
struct mass_unit_tag : base_unit<mass_unit_tag, mass_dimension, -29> { };
struct time_unit_tag : base_unit<time_unit_tag, time_dimension, -28> { };
struct current_unit_tag : base_unit<current_unit_tag, current_dimension, -27> { };
struct temperature_unit_tag : base_unit<temperature_unit_tag, temperature_dimension, -26> { };
struct amount_unit_tag : base_unit<amount_unit_tag, amount_dimension, -25> { };
struct luminous_intensity_unit_tag : base_unit<luminous_intensity_unit_tag, luminous_intensity_dimension, -24> { };
struct plane_angle_unit_tag : base_unit<plane_angle_unit_tag, plane_angle_dimension, -23> { };
struct solid_angle_unit_tag : base_unit<solid_angle_unit_tag, solid_angle_dimension, -22> { };

typedef make_system<
    length_unit_tag,
    mass_unit_tag,
    time_unit_tag,
    current_unit_tag,
    temperature_unit_tag,
    amount_unit_tag,
    luminous_intensity_unit_tag,
    plane_angle_unit_tag,
    solid_angle_unit_tag
>::type system;

typedef unit<length_dimension,system>                length;                 ///< abstract unit of length
typedef unit<mass_dimension,system>                  mass;                   ///< abstract unit of mass
typedef unit<time_dimension,system>                  time;                   ///< abstract unit of time
typedef unit<current_dimension,system>               current;                ///< abstract unit of current
typedef unit<temperature_dimension,system>           temperature;            ///< abstract unit of temperature
typedef unit<amount_dimension,system>                amount;                 ///< abstract unit of amount
typedef unit<luminous_intensity_dimension,system>    luminous_intensity;     ///< abstract unit of luminous intensity
typedef unit<plane_angle_dimension,system>           plane_angle;            ///< abstract unit of plane angle
typedef unit<solid_angle_dimension,system>           solid_angle;            ///< abstract unit of solid angle

} // namespace abstract

template<>
struct base_unit_info<abstract::length_unit_tag>
{
    static std::string name()       { return "[Length]"; }
    static std::string symbol()     { return "[L]"; }
};

template<>
struct base_unit_info<abstract::mass_unit_tag>
{
    static std::string name()       { return "[Mass]"; }
    static std::string symbol()     { return "[M]"; }
};

template<>
struct base_unit_info<abstract::time_unit_tag>
{
    static std::string name()       { return "[Time]"; }
    static std::string symbol()     { return "[T]"; }
};

template<>
struct base_unit_info<abstract::current_unit_tag>
{
    static std::string name()       { return "[Electric Current]"; }
    static std::string symbol()     { return "[I]"; }
};

template<>
struct base_unit_info<abstract::temperature_unit_tag>
{
    static std::string name()       { return "[Temperature]"; }
    static std::string symbol()     { return "[Theta]"; }
};

template<>
struct base_unit_info<abstract::amount_unit_tag>
{
    static std::string name()       { return "[Amount]"; }
    static std::string symbol()     { return "[N]"; }
};

template<>
struct base_unit_info<abstract::luminous_intensity_unit_tag>
{
    static std::string name()       { return "[Luminous Intensity]"; }
    static std::string symbol()     { return "[J]"; }
};

template<>
struct base_unit_info<abstract::plane_angle_unit_tag>
{
    static std::string name()       { return "[Plane Angle]"; }
    static std::string symbol()     { return "[QP]"; }
};

template<>
struct base_unit_info<abstract::solid_angle_unit_tag>
{
    static std::string name()       { return "[Solid Angle]"; }
    static std::string symbol()     { return "[QS]"; }
};

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_ABSTRACT_HPP


/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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 "modules/localization/msf/local_gnss/gnss_positioning.h"
#include <Eigen/Eigen>
#include <vector>
#include "modules/localization/msf/local_gnss/atmosphere.h"
#include "modules/localization/msf/local_gnss/gnss_utility.hpp"

namespace apollo {
namespace localization {
namespace local_gnss {

// this file contains lots of quality control strategies learned from field
// datas to be conservative for a safety-critical vehicle.

bool GnssPntSolver::IsSafeStandardPointPos(const double std_rover,
                                            const double pdop) {
  if (fabs(std_rover) >= 10.0 || std_rover < 0.0) {
    return false;
  }
  if (fabs(pdop) > 10.0) {
    return false;
  }
  return true;
}

bool GnssPntSolver::IsSafeCodeDiffPos(const double dis, const double std_dif,
                                       const bool valid_delta_pos) {
  if (fabs(std_dif) > 2.0 || fabs(std_dif) < 0.2) {
    return false;
  }
  if (dis > 10.0) {
    return false;
  }
  if (valid_delta_pos) {
    return false;
  }
  return true;
}

bool GnssPntSolver::IsSafeFixedSolution(const unsigned int gnss_type_size,
                                        const unsigned int num_fixed_phase) {
  if (!b_rtkfixed_) {
    return false;
  }
  if (num_fixed_phase <= 5 * gnss_type_size) {
    std::string excute_info = "";
    if (num_fixed_phase <= 4 * gnss_type_size) {
      ResetFixedRtk();
      excute_info = "reset";
    }
    EncodeDetails(
        "fixed_phase_num not enough for safe solution = %3d with num_sys = %3d "
        "%s",
        num_fixed_phase, gnss_type_size, excute_info.c_str());
    return false;
  }
  double leve_dop = sqrt(rtk_dop_phase_.x * rtk_dop_phase_.x +
                         rtk_dop_phase_.y * rtk_dop_phase_.y);
  std::string save_temp = gnss_utility::FormatString(
      " Safe_fixed_phase(3d_dop, level_dop, num, new) = %6.3f%6.3f%3d%3d",
      rtk_dop_phase_.Norm3D(), leve_dop, num_fixed_phase,
      new_fixed_phase_num_);

  if (rtk_dop_phase_.Norm3D() < 4.0 || leve_dop < 2.5) {
    invalid_rtk_sub_infor_ = invalid_rtk_sub_infor_ + save_temp;
    return true;
  }

  std::string rist_temp = gnss_utility::FormatString(
      " Risky_fixed_phase(3d_dop, level_dop, num, new) = %6.3f%6.3f%3d%3d",
      rtk_dop_phase_.Norm3D(), leve_dop, num_fixed_phase,
      new_fixed_phase_num_);

  invalid_rtk_sub_infor_ = invalid_rtk_sub_infor_ + rist_temp;
  if (++_continuous_risky_num >= 5) {
    _continuous_risky_num = 0;
    ResetFixedRtk();
  }

  return false;
}

void GnssPntSolver::WeightOnPostResidual(
    const double std_v,
    const std::vector<unsigned int>& range_type,
    const Eigen::MatrixXd& post_res,
    Eigen::MatrixXd* px_temp) {
  Eigen::MatrixXd px = *px_temp;
  for (unsigned int i = 0; i < range_type.size(); ++i) {
    if (range_type[i] == 0) {
      if (fabs(post_res(i, 0)) > 20 * std_v) {
        px(i, i) *= 0.01;
      } else if (fabs(post_res(i, 0)) > 15 * std_v) {
        px(i, i) *= 0.3;
      } else if (fabs(post_res(i, 0)) > 10 * std_v) {
        px(i, i) *= 0.7;
      }
    } else {
      if (fabs(post_res(i, 0)) > 4 * std_v) {
        px(i, i) *= 0.1;
      } else if (fabs(post_res(i, 0)) > 3 * std_v) {
        px(i, i) *= 0.3;
      } else if (fabs(post_res(i, 0)) > 2 * std_v) {
        px(i, i) *= 0.7;
      }
    }
  }
  *px_temp = px;
}

void GnssPntSolver::RemoveAmbWithAbnormalRes(
    const std::vector<ObsKey>& phase_recorder,
    const double& std_v,
    const std::vector<unsigned int>& range_type,
    const Eigen::MatrixXd& post_res,
    const double band_res_sum[apollo::drivers::gnss::GLO_G3]) {
  bool phase_slip_exist = false;
  unsigned int ind_phase = 0;
  for (unsigned int i = 0; i < range_type.size(); ++i) {
    if (range_type[i] == 0) {
      continue;
    }
    if (fabs(post_res(i, 0)) > 5 * std_v || fabs(post_res(i, 0)) >= 0.10) {
      phase_slip_exist = true;
      amb_tracker_.DeleteSlipAmb(phase_recorder[ind_phase].band_id,
                                 phase_recorder[ind_phase].sat_prn);
    }
    ++ind_phase;
  }
  if (!phase_slip_exist) {
    amb_tracker_.ReupdateReferenceAmb(band_res_sum);
  }
}

void GnssPntSolver::BoundRangePrecision(const double std_dif) {
  if (std_dif > range_precision_) {
    range_precision_ = std_dif;
  }
}

PointThreeDim GnssPntSolver::BoundedRtkStd() {
  PointThreeDim phase_std = std_rtk_.SquareRoot();
  if (std_rtk_thres_of_phase_dop_.Norm3D() > std_rtk_.Norm3D()) {
    phase_std = std_rtk_thres_of_phase_dop_.SquareRoot();
  }
  return phase_std;
}

bool GnssPntSolver::CheckSatObsForSPP(const SatelliteObservation& sat_obs) {
  if (sat_obs.band_obs_num() <= 0) {
    return false;
  }
  // 20804163.233 = 69 ms
  // 36804163.233 = 122.7 ms
  double time_duration = sat_obs.band_obs(0).pseudo_range() / C_MPS;
  if (time_duration < 0.030) {
    return false;
  }
  if (time_duration > 0.200) {
    return false;
  }
  return true;
}

int GnssPntSolver::IsValidBandObs(const BandObservation& band_obs1,
                                  const BandObservation& band_obs2) {
  if (band_obs1.band_id() != band_obs2.band_id()) {
    return -1;
  }
  if (gnss_utility::GetIndexInVector(band_obs1.band_id(), band_to_solve_) ==
      -1) {
    return -1;
  }
  return 0;
}

bool GnssPntSolver::CheckBaserObs(double std, double distance) {
  if (fabs(std) > 30.0 || std < 0.0) {
    return false;
  }
  if (fabs(distance) > 100.0) {
    return false;
  }
  return true;
}

int GnssPntSolver::EnableGlonassIfbEstimated() {
  // return 1 && b_rtkfixed_;
  return 0;
}

double GnssPntSolver::WeightScaleOnGnssSystem(
    const GnssType& type) {
  double scale = 25.0;
  switch (type) {
    case apollo::drivers::gnss::GPS_SYS:
      scale = 1.0;
      break;
    case apollo::drivers::gnss::BDS_SYS:
      scale = 1.0;
      break;
    case apollo::drivers::gnss::GLO_SYS:
      scale = 2.25;
      break;
    default:
      scale = 25.0;
  }
  return scale;
}

void GnssPntSolver::CheckConsecutiveTimeOffset(unsigned int week_num,
                                                  double week_sec) {
  double sec_per_week = SECOND_PER_WEEK;
  double t1 = week_num * sec_per_week + week_sec;
  double t2 = gnss_weeknum_ * sec_per_week + gnss_week_second_s_;
  if (fabs(t1 - t2) > 5.0) {
    ResetFixedRtk();
  }
  gnss_week_second_s_ = week_sec;
  gnss_weeknum_ = week_num;
}

bool GnssPntSolver::IsEnabledCodeDiff(const double& data_outage,
                                     const double& dis_rover2baser) {
  if (fabs(data_outage) > 60.0) {
    return false;
  }
  const double dis_tolerance = 50000;
  if (dis_rover2baser > dis_tolerance) {
    return false;
  }
  return true;
}

bool GnssPntSolver::IsEnabledPhaseDiff(const double& data_outage,
                                      const double& dis_rover2baser) {
  if (fabs(data_outage) > 15.0) {
    return false;
  }
  const double dis_tolerance = 25000;
  if (dis_rover2baser > dis_tolerance) {
    return false;
  }
  return true;
}

bool GnssPntSolver::CheckLambda(const double ratio, const double adop) {
  if (ratio < 0.0) {
    return false;
  }
  if (adop > 30.0) {
    return false;
  }
  return true;
}

bool GnssPntSolver::IsAbnormalFixedSolutionWithNewPhase(
    const bool& is_fixed, const double std_fixed,
    const unsigned int unknown_phase_num) {
  if (unknown_phase_num <= 0) {
    return false;
  }
  if (!is_fixed) {
    return false;
  }
  if (fabs(std_fixed) < 0.3) {
    return false;
  }
  return true;
}

bool GnssPntSolver::IsNeedRecursion(const bool& is_fixed,
                                    const double std_fixed,
                                    const unsigned int unknown_phase_num,
                                    const unsigned int common_phase_num) {
  if (unknown_phase_num > common_phase_num) {
    EncodeDetails("More unknown phase: %d > %d", unknown_phase_num,
                   common_phase_num);
    return true;
  }
  if (IsAbnormalFixedSolutionWithNewPhase(is_fixed, std_fixed,
                                             unknown_phase_num)) {
    EncodeDetails("abnormal_fixed_solution %6.3f", _std_v);
    SeedToughSatGroup(1.0);
    return true;
  }
  return false;
}

double GnssPntSolver::BoundRatio(const double& ratio) {
  if (ratio > 999.9) {
    return 1000.0;
  }
  return ratio;
}

double GnssPntSolver::GetStdThresForNewSat(const double new_ratio,
                                                const double std_v) {
  double thres_hold = 0.05;
  if (new_ratio < 3) {
    thres_hold = 0.002;
  } else if (new_ratio < 5) {
    thres_hold = 0.010;
  } else if (new_ratio < 8) {
    thres_hold = 0.020;
  } else if (new_ratio < 15) {
    thres_hold = 0.025;
  }
  if (std_v < 0.03) {
    thres_hold *= 2.0;
  }
  return thres_hold;
}

bool GnssPntSolver::CheckNewsatSolution(const PointThreeDim& denu,
                                          const double& thres,
                                          const double& dis_time_diff) {
  if (fabs(denu.z) > thres * 2) {
    return false;
  }
  if (fabs(denu.y) > thres) {
    return false;
  }
  if (fabs(denu.x) > thres) {
    return false;
  }
  if (fabs(dis_time_diff) > 0.05) {
    return false;
  }
  return true;
}

}  // namespace local_gnss
}  // namespace localization
}  // namespace apollo


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_CONSTANT_MAXINIT_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_MAXINIT_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/constant/definition/valmin.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate the constant maxinit which is identical to minf for
    arithmetic real types but that must be specialized for complex values ordering.

    maxinit is the value to be returned when a maximum of the elements function is used on an
    empty container.

    @return The Maxinit constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct maxinit_ : boost::dispatch::constant_value_<maxinit_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,maxinit_,boost::dispatch::constant_value_<maxinit_>);

      struct value_map
      {
        template<typename X>
        static auto value(X const& x) -> decltype(valmin_::value_map::value(x));

        template<typename X>
        static brigand::single_<0xFF800000U> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static brigand::double_<0xFFF0000000000000ULL> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,maxinit_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::maxinit_,maxinit);
  }

  template<typename T> BOOST_FORCEINLINE auto Maxinit()
  BOOST_NOEXCEPT_DECLTYPE(detail::maxinit( boost::dispatch::as_<T>{}))
  {
    return detail::maxinit( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_TABLE_DETAILS_IS_VECTORIZABLE_INDEXER_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_TABLE_DETAILS_IS_VECTORIZABLE_INDEXER_HPP_INCLUDED

#include <nt2/core/container/dsl/forward.hpp>
#include <nt2/core/functions/scalar/colon.hpp>
#include <nt2/sdk/memory/forward/container.hpp>
#include <nt2/core/utility/of_size.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/sdk/meta/as.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/find_if.hpp>
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>

namespace nt2 { namespace ext
{
  template<class It, class End>
  struct mpl_safe_next
       : boost::mpl::next<It>
  {
  };

  template<class It>
  struct mpl_safe_next<It, It>
  {
    typedef It type;
  };

  template<class T>
  struct is_vectorizable_scalar
       : boost::mpl::bool_< T::extent_type::static_size == 0u >
  {
  };

  template<>
  struct is_vectorizable_scalar< nt2::container::
                                 expression< boost::proto::
                                             basic_expr< boost::proto::tag::terminal
                                                       , boost::proto::term<nt2::container::colon_>
                                                       , 0l
                                                       >
                                           , nt2::container::colon_
                                           >
                               >
       : boost::mpl::false_
  {
  };

  template<class Id, class Cardinal, class Enable = void>
  struct is_vectorizable_indexer_impl
       : boost::mpl::false_
  {
  };

  template<class Id, class Cardinal>
  struct is_vectorizable_indexer
       : is_vectorizable_indexer_impl<Id, Cardinal>
  {
  };

  // scalar
  template<class Id>
  struct is_vectorizable_indexer_impl< Id, boost::mpl::size_t<1u>, typename boost::enable_if_c< Id::extent_type::static_size == 0u >::type >
       : boost::mpl::true_
  {
  };

  // _(a, b)
  template<std::size_t N, std::size_t Cardinal>
  struct is_multiple_of
    : boost::mpl::bool_< !(N % Cardinal) >
  {
  };

  template<std::size_t N>
  struct is_multiple_of<N, 0>
       : boost::mpl::false_
  {
  };

  template<class T, std::ptrdiff_t N, class Cardinal>
  struct is_vectorizable_indexer<
    nt2::container::expression<
      boost::proto::basic_expr<
          nt2::tag::colon_
        , boost::proto::list3<
              nt2::box<
                  nt2::of_size_<
                      1l
                    , N
                    , 1l
                    , 1l
                  >
              >
            , nt2::box<
                  nt2::meta::constant_<nt2::tag::unity_colon_, T>
              >
            , nt2::container::expression<
                  boost::proto::basic_expr<
                      boost::proto::tag::terminal
                    , boost::proto::term<
                          boost::dispatch::meta::as_<T>
                      >
                    , 0l
                  >
                , boost::dispatch::meta::as_<T>
              >
          >
        , 3l
      >
    , nt2::memory::container<
          T
        , nt2::settings(
            nt2::of_size_<
                1l
              , N
              , 1l
              , 1l
            >
          )
      >
    >
  , Cardinal
  >
    : is_multiple_of<std::size_t(N < 0 ? -N : N), Cardinal::value>
  {
  };

  // _
  template<class Cardinal>
  struct is_vectorizable_indexer< nt2::container::
                                  expression< boost::proto::
                                              basic_expr< boost::proto::tag::terminal
                                                        , boost::proto::term<nt2::container::colon_>
                                                        , 0l
                                                        >
                                            , nt2::container::colon_
                                            >
                                , Cardinal
                                >
       : boost::mpl::true_
  {
  };

  template<class Children, class Cardinal>
  struct is_vectorizable_indexers_impl
  {
    typedef typename boost::mpl::end<Children>::type seq_end;

    typedef typename boost::mpl::
            find_if< Children
                   , boost::mpl::not_< is_vectorizable_indexer< boost::mpl::_1, Cardinal > >
                   >::type it;

    typedef typename boost::mpl::
            fold < boost::mpl::iterator_range< it, typename mpl_safe_next<it, seq_end>::type >
                 , boost::mpl::true_
                 , boost::mpl::and_< boost::mpl::_1
                                   , is_vectorizable_indexer< boost::mpl::_2, boost::mpl::size_t<1> >
                                   >
                 >::type init;

    typedef typename boost::mpl::
            fold < boost::mpl::iterator_range< typename mpl_safe_next<it, seq_end>::type, seq_end >
                 , init
                 , boost::mpl::and_< boost::mpl::_1
                                   , is_vectorizable_scalar< boost::mpl::_2 >
                                   >
                 >::type type;

    static const bool value = type::value;
  };

  template<class Children, class Data>
  struct is_vectorizable_indexers
       : is_vectorizable_indexers_impl<Children, typename meta::cardinal_of<typename meta::target_value<Data>::type>::type>
  {
  };

  template<class Children>
  struct is_contiguous_indexers
       : is_vectorizable_indexers_impl<Children, boost::mpl::size_t<0> >
  {
  };
} }

#endif


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_INTERFACE_SPARSIFIER_HPP
#define GSPARSE_INTERFACE_SPARSIFIER_HPP

#include <cstddef>       // size_t
#include <memory>        //shared_ptr
#include <Eigen/Dense>   // Eigen Matrix
#include "../Config.hpp" // Library configuration
#include "Graph.hpp"

namespace gSparse
{
    //!  An interface class for Graph Sparsifier
    /*!
        This class defines an interface for gSparse's Sparsifier
    */
	class ISparsifier
	{
	public:
        //! A pure virtual member to computer sparsifier weight.
		virtual COMPUTE_INFO Compute() = 0;
        //! A pure virtual member to get computation status of Sparsifier.
		virtual COMPUTE_INFO GetInfo() const = 0;
        //! A pure virtual member to sparsifier a graph
		virtual gSparse::Graph  GetSparsifiedGraph() = 0;
		virtual ~ISparsifier() = default;
	protected:
		ISparsifier() = default;
	};
	typedef std::shared_ptr<ISparsifier> Sparsifier;
}
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_FLAT_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_FLAT_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#include <boost/geometry/strategies/buffer.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer create flat ends
\ingroup strategies
\details This strategy can be used as EndStrategy for the buffer algorithm.
    It creates a flat end for each linestring-end. It can be applied
    for (multi)linestrings. Also it is applicable for spikes in (multi)polygons.
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_end_flat]
[heading Output]
[$img/strategies/buffer_end_flat.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_end_round end_round]
}
 */
class end_flat
{

public :

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a flat end
    template <typename Point, typename RangeOut, typename DistanceStrategy>
    inline void apply(Point const& penultimate_point,
                Point const& perp_left_point,
                Point const& ultimate_point,
                Point const& perp_right_point,
                buffer_side_selector side,
                DistanceStrategy const& distance,
                RangeOut& range_out) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;

        typedef typename geometry::select_most_precise
        <
            coordinate_type,
            double
        >::type promoted_type;

        promoted_type const dist_left = distance.apply(penultimate_point, ultimate_point, buffer_side_left);
        promoted_type const dist_right = distance.apply(penultimate_point, ultimate_point, buffer_side_right);

        bool reversed = (side == buffer_side_left && dist_right < 0 && -dist_right > dist_left)
                    || (side == buffer_side_right && dist_left < 0 && -dist_left > dist_right)
                    ;
        if (reversed)
        {
            range_out.push_back(perp_right_point);
            range_out.push_back(perp_left_point);
        }
        else
        {
            range_out.push_back(perp_left_point);
            range_out.push_back(perp_right_point);
        }
        // Don't add the ultimate_point (endpoint of the linestring).
        // The buffer might be generated completely at one side.
        // In other cases it does no harm but is further useless
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

    //! Returns the piece_type (flat end)
    static inline piece_type get_piece_type()
    {
        return buffered_flat_end;
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_FLAT_HPP


#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>

#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/octree/octree_pointcloud_occupancy.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/io.h>  // pcl::copyPointCloud
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/pca.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/segment_differences.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>

#include <pcl_ros/point_cloud.h>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>

/* Messages */
#include "apc_msgs/BoundingBoxDepth.h"

/* Services */
#include "apc_msgs/ObjectPlacementPoseFromCloud.h"

/*
NOTE
Custom service definitions should have the same name as the service that employs
them. I.E. the find_free_space service should have a custom service definition
called FindFreeSpace.
*/

// TODO convert to non-colour pointcloud at start
// TODO update draw_object function to operate on position and angle

bool does_object_intersect(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                           pcl::PointXYZ search_origin, pcl::PointXYZ object_dimensions,
                           double resolution) {
   for (double z=search_origin.z; z>search_origin.z-object_dimensions.z; z-=resolution) {
       for (double x=search_origin.x; x<search_origin.x+object_dimensions.x; x+=resolution) {
           for (double y=search_origin.y; y<search_origin.y+object_dimensions.y; y+=resolution) {
               if (tree.isVoxelOccupiedAtPoint(x, y, z)) {
                   return true;
               }
           }
       }
   }
   return false;
}

int count_vacancies_under_object(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                                 pcl::PointXYZRGB max_pt,
                                 pcl::PointXYZ object_origin,
                                 pcl::PointXYZ object_dimensions,
                                 double resolution) {
    int num_vacancies = 0;
    for (double z=max_pt.z; z>object_origin.z; z-=resolution) {
        for (double x=object_origin.x; x<object_origin.x+object_dimensions.x; x+=resolution) {
            for (double y=object_origin.y; y<object_origin.y+object_dimensions.y; y+=resolution) {
                if (!tree.isVoxelOccupiedAtPoint(x, y, z)) {
                    num_vacancies++;
                }
            }
        }
    }
    return num_vacancies;
}

bool get_valid_object_position(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                               pcl::PointXYZ object_dimensions,
                               pcl::PointXYZRGB min_pt,
                               pcl::PointXYZRGB max_pt,
                               double resolution,
                               pcl::PointXYZ &ret_valid_object_position) {
    // Do a bottom of tote up search for a position where the whole object fits
    pcl::PointXYZ search_origin;
    bool is_finished = false;
    for (double z=max_pt.z; z>min_pt.z+object_dimensions.z; z-=resolution) {
    // for (double z=min_pt.z; z<max_pt.z-object_dimensions.z; z+=resolution) {
        for (double x=min_pt.x; x<max_pt.x-object_dimensions.x; x+=resolution) {
            for (double y=min_pt.y; y<max_pt.y-object_dimensions.y; y+=resolution) {
                if (!tree.isVoxelOccupiedAtPoint(x, y, z)) {
                    search_origin.x = x;
                    search_origin.y = y;
                    search_origin.z = z;
                    if (!does_object_intersect(tree, search_origin, object_dimensions, resolution)) {
                        ROS_INFO_STREAM("Place origin of object at: " << search_origin.x << ", " << search_origin.y << ", " << search_origin.z);
                        is_finished = true;
                    }
                    // else {
                    //     ROS_INFO_STREAM("Object doesn't fit here. Continuing search...");
                    // }
                }
                if (is_finished) {break;}
            }
            if (is_finished) {break;}
        }
        if (is_finished) {break;}
    }

    if (!is_finished) {
        return false;
    }

    pcl::PointXYZ object_origin = search_origin;
    pcl::PointXYZ best_object_origin;

    // Trial all x,y positions at found depth to choose the spot with the least number of vacancies below
    // This was added to promote placing items on other items vs. over vacancies where smaller items might fit better
    int num_vacancies = 0;
    int min_vacancies = 1000000;
    for (double x=min_pt.x; x<max_pt.x-object_dimensions.x; x+=resolution) {
        for (double y=min_pt.y; y<max_pt.y-object_dimensions.y; y+=resolution) {
            object_origin.x = x;
            object_origin.y = y;
            if (!does_object_intersect(tree, object_origin, object_dimensions, resolution)) {
                num_vacancies = count_vacancies_under_object(tree, max_pt, object_origin, object_dimensions, resolution);

                if (num_vacancies < min_vacancies) {
                    min_vacancies = num_vacancies;
                    best_object_origin.x = x;
                    best_object_origin.y = y;
                    best_object_origin.z = object_origin.z;
                }
            }
        }
    }

    ret_valid_object_position = best_object_origin;

    return true;
}

void fill_under_objects(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                        pcl::PointXYZRGB min_pt, pcl::PointXYZRGB max_pt,
                        double resolution) {
    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector occupiedCells;
    tree.getOccupiedVoxelCenters(occupiedCells);
    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector::iterator it;

    for (it = occupiedCells.begin(); it != occupiedCells.end(); ++it) {
        for (double z=max_pt.z; z>it->z; z-=resolution) {
            pcl::PointXYZRGB point;
            point.x = it->x;
            point.y = it->y;
            point.z = z;
            tree.setOccupiedVoxelAtPoint(point);
        }
    }

    // Fill roof too to stop items being placed above
    double z = max_pt.z - 0.23;  // Storage is about 20cm high
    for (double x=min_pt.x; x<max_pt.x; x+=resolution) {
        for (double y=min_pt.y; y<max_pt.y; y+=resolution) {
            pcl::PointXYZRGB point;
            point.x = x;
            point.y = y;
            point.z = z;
            tree.setOccupiedVoxelAtPoint(point);
        }
    }

    // Fill ~2cm of walls too
    // tl, tr, bl, br
    double top_left_x = max_pt.x;
    double top_left_y = min_pt.y;
    double top_right_x = max_pt.x;
    double top_right_y = max_pt.y;
    double bottom_left_x = min_pt.x;
    double bottom_left_y = min_pt.y;
    double bottom_right_x = min_pt.x;
    double bottom_right_y = max_pt.y;
    double bottom_z = max_pt.z;
    double top_z = bottom_z - 0.23;
    double wall_width = 0.03;

    // ROS_INFO_STREAM("top_left_x = " << top_left_x);
    // ROS_INFO_STREAM("top_left_y = " << top_left_y);
    // ROS_INFO_STREAM("top_right_x = " << top_right_x);
    // ROS_INFO_STREAM("top_right_y = " << top_right_y);
    // ROS_INFO_STREAM("bottom_left_x = " << bottom_left_x);
    // ROS_INFO_STREAM("bottom_left_y = " << bottom_left_y);
    // ROS_INFO_STREAM("bottom_right_x = " << bottom_right_x);
    // ROS_INFO_STREAM("bottom_right_y = " << bottom_right_y);
    // ROS_INFO_STREAM("bottom_z = " << bottom_z);
    // ROS_INFO_STREAM("top_z = " << top_z);

    for (double z=bottom_z; z>top_z; z-=resolution) {
        // Fill top row
        for (double y=top_left_y; y<top_right_y; y+=resolution) {
            for (double x=top_left_x; x>top_left_x-wall_width; x-=resolution) {
                // ROS_INFO_STREAM("z = " << z << ", x = " << x << ", y = " << y);
                pcl::PointXYZRGB point;
                point.x = x;
                point.y = y;
                point.z = z;
                tree.setOccupiedVoxelAtPoint(point);
            }
        }
        // Fill bottom row
        for (double y=bottom_left_y; y<bottom_right_y; y+=resolution) {
            for (double x=bottom_left_x; x<bottom_left_x+wall_width; x+=resolution) {
                // ROS_INFO_STREAM("z = " << z << ", x = " << x << ", y = " << y);
                pcl::PointXYZRGB point;
                point.x = x;
                point.y = y;
                point.z = z;
                tree.setOccupiedVoxelAtPoint(point);
            }
        }
        // Fill left column
        for (double x=bottom_left_x; x<top_left_x; x+=resolution) {
            for (double y=bottom_left_y; y<bottom_left_y+wall_width; y+=resolution) {
                // ROS_INFO_STREAM("z = " << z << ", x = " << x << ", y = " << y);
                pcl::PointXYZRGB point;
                point.x = x;
                point.y = y;
                point.z = z;
                tree.setOccupiedVoxelAtPoint(point);
            }
        }
        // Fill right column
        for (double x=bottom_right_x; x<top_right_x; x+=resolution) {
            for (double y=bottom_right_y; y>bottom_right_y-wall_width; y-=resolution) {
                // ROS_INFO_STREAM("z = " << z << ", x = " << x << ", y = " << y);
                pcl::PointXYZRGB point;
                point.x = x;
                point.y = y;
                point.z = z;
                tree.setOccupiedVoxelAtPoint(point);
            }
        }
    }
}

void draw_object(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                 pcl::PointXYZ object_origin, pcl::PointXYZ object_dimensions,
                 double resolution) {
   for (double z=object_origin.z; z>object_origin.z-object_dimensions.z; z-=resolution) {
       for (double x=object_origin.x; x<object_origin.x+object_dimensions.x; x+=resolution) {
           for (double y=object_origin.y; y<object_origin.y+object_dimensions.y; y+=resolution) {
               pcl::PointXYZRGB point;
               point.x = x;
               point.y = y;
               point.z = z;
               tree.setOccupiedVoxelAtPoint(point);
           }
       }
   }
}

void save_pointcloud_version_of_octree(pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> &tree,
                                       std::string filename) {
    //how many occupied cells do we have in the tree?
    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector occupiedCells;
    tree.getOccupiedVoxelCenters(occupiedCells);

    //cloud to store the points
    pcl::PointCloud<pcl::PointXYZ> cloud;
    ROS_INFO_STREAM("num_occupied_cells = " << occupiedCells.size());
    // cloud.points.resize(occupiedCells.size());
    // cloud.width = 256426;
    cloud.width = occupiedCells.size();
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);
    // ROS_INFO_STREAM("cloud width = " << cloud.width);
    // ROS_INFO_STREAM("cloud height = " << cloud.height);

    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector::iterator it;
    int i=0;
    for (it = occupiedCells.begin(); it != occupiedCells.end(); ++it, i++)
    {
        //add point in point cloud
        cloud.points[i].x = it->x;
        cloud.points[i].y = it->y;
        cloud.points[i].z = it->z;
    }
    //save cloud
    pcl::io::savePCDFileASCII(filename, cloud);
}

bool get_valid_object_position_and_orientation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                                               double object_longest_side_length,
                                               double object_middlest_side_length,
                                               double object_shortest_side_length,
                                               double resolution,
                                               bool verbose,
                                               pcl::PointXYZ &ret_valid_object_position,
                                               double &ret_valid_object_orientation) {
    pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> pcl_tree (resolution);

    pcl_tree.setOccupiedVoxelsAtPointsFromCloud(cloud);

    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector occupiedCells;
    pcl_tree.getOccupiedVoxelCenters(occupiedCells);
    pcl::octree::OctreePointCloud<pcl::PointXYZRGB>::AlignedPointTVector::iterator it;

    std::vector<int> index;
    cloud->is_dense = false;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, index);

    pcl::PointXYZRGB min_pt, max_pt;
    pcl::getMinMax3D(*cloud, min_pt, max_pt);

    ROS_INFO_STREAM("min_x = " << min_pt.x << ", min_y" << min_pt.y << ", min_z" << min_pt.z);
    ROS_INFO_STREAM("max_x = " << max_pt.x << ", max_y" << max_pt.y << ", max_z" << max_pt.z);

    fill_under_objects(pcl_tree, min_pt, max_pt, resolution);

    if (verbose) {
        save_pointcloud_version_of_octree(pcl_tree, "octree_without_object.pcd");
    }

    pcl::PointXYZ object_dimensions_0;
    double degrees_to_global_y_0 = 90;
    object_dimensions_0.x = object_longest_side_length;
    object_dimensions_0.y = object_middlest_side_length;
    object_dimensions_0.z = object_shortest_side_length;
    pcl::PointXYZ object_dimensions_1;  // flipped width and height
    double degrees_to_global_y_1 = 0;
    object_dimensions_1.x = object_middlest_side_length;
    object_dimensions_1.y = object_longest_side_length;
    object_dimensions_1.z = object_shortest_side_length;

    pcl::PointXYZ object_dimensions;

    // Choose which z position is largest (i.e. furthest from camera / closest to floor)
    pcl::PointXYZ valid_position, valid_position_0, valid_position_1;
    bool success_0 = get_valid_object_position(pcl_tree, object_dimensions_0, min_pt, max_pt, resolution, valid_position_0);
    bool success_1 = get_valid_object_position(pcl_tree, object_dimensions_1, min_pt, max_pt, resolution, valid_position_1);

    if (success_0 && success_1) {
        // Choose which z position is largest (i.e. furthest from camera / closest to floor)
        if (valid_position_0.z > valid_position_1.z) {
            valid_position = valid_position_0;
            object_dimensions = object_dimensions_0;

            ret_valid_object_position.x = valid_position.x + object_longest_side_length/2.0;
            ret_valid_object_position.y = valid_position.y + object_middlest_side_length/2.0;
            ret_valid_object_position.z = valid_position.z - object_shortest_side_length;
            ret_valid_object_orientation = degrees_to_global_y_0;
        } else {
            valid_position = valid_position_1;
            object_dimensions = object_dimensions_1;

            ret_valid_object_position.x = valid_position.x + object_middlest_side_length/2.0;
            ret_valid_object_position.y = valid_position.y + object_longest_side_length/2.0;
            ret_valid_object_position.z = valid_position.z - object_shortest_side_length;
            ret_valid_object_orientation = degrees_to_global_y_1;
        }
    } else if (success_0) {
        valid_position = valid_position_0;
        object_dimensions = object_dimensions_0;

        ret_valid_object_position.x = valid_position.x + object_longest_side_length/2.0;
        ret_valid_object_position.y = valid_position.y + object_middlest_side_length/2.0;
        ret_valid_object_position.z = valid_position.z - object_shortest_side_length;
        ret_valid_object_orientation = degrees_to_global_y_0;
    } else if (success_1) {
        valid_position = valid_position_1;
        object_dimensions = object_dimensions_1;

        ret_valid_object_position.x = valid_position.x + object_middlest_side_length/2.0;
        ret_valid_object_position.y = valid_position.y + object_longest_side_length/2.0;
        ret_valid_object_position.z = valid_position.z - object_shortest_side_length;
        ret_valid_object_orientation = degrees_to_global_y_1;
    } else {
        return false;
    }

    if (verbose) {
        pcl::octree::OctreePointCloudOccupancy<pcl::PointXYZRGB> pcl_tree_empty (resolution);
        draw_object(pcl_tree_empty, valid_position, object_dimensions, resolution);
        save_pointcloud_version_of_octree(pcl_tree_empty, "object_in_valid_position.pcd");
    }

    return true;
}

bool object_placement_pose_from_cloud_service_callback(apc_msgs::ObjectPlacementPoseFromCloud::Request &req,
                                                       apc_msgs::ObjectPlacementPoseFromCloud::Response &res) {
   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
   cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

   double object_longest_side_length = req.object_longest_side_length.data;
   double object_middlest_side_length = req.object_middlest_side_length.data;
   double object_shortest_side_length = req.object_shortest_side_length.data;
   double resolution = req.resolution.data;

   Eigen::Affine3d camera_to_world_tf_eigen;
   tf::poseMsgToEigen(req.camera_to_world, camera_to_world_tf_eigen);

   pcl::fromROSMsg(req.cropped_cloud, *cloud);

   pcl::transformPointCloud(*cloud, *cloud, camera_to_world_tf_eigen);

   pcl::PointXYZ valid_object_position;
   double valid_object_orientation;
   bool verbose = true;

   bool success = get_valid_object_position_and_orientation(cloud,
                                                            object_longest_side_length,
                                                            object_middlest_side_length,
                                                            object_shortest_side_length,
                                                            resolution,
                                                            verbose,
                                                            valid_object_position,
                                                            valid_object_orientation);

   res.x.data = valid_object_position.x;
   res.y.data = valid_object_position.y;
   res.z.data = valid_object_position.z;
   res.degrees_to_global_y.data = valid_object_orientation;
   res.success.data = success;

   ROS_INFO_STREAM("Valid position = x: " << valid_object_position.x << ", y: " << valid_object_position.y << ", z: " << valid_object_position.z);
   ROS_INFO_STREAM("Valid orientation to global y = " << valid_object_orientation << " degrees");

   return true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "object_placement_pose_from_cloud_node");
    ros::NodeHandle n;

    ros::ServiceServer service_handle = n.advertiseService("object_placement_pose_from_cloud", object_placement_pose_from_cloud_service_callback);

    ros::spin();

    return 0;
}


#pragma once

#include <tuple>
#include <boost/unordered_map.hpp>
#include "types.hh"

/*!
  @brief given two runs r and r' of Abstraction automata A and B, check if (r,r') is a run of A x B

  @note This class is a function object.
 */
template<typename Abstraction>
class PartialRunChecker
{
public:

  PartialRunChecker (AbstractionAutomaton<Abstraction> &RA2,AbstractionAutomaton<Abstraction> &RA,const TAState TAStateSize) : NVar (RA.abstractedStates.front().second.getNumOfVar()),
                                                                                RA2(RA2),
                                                                                RA(RA),
                                                                                TAStateSize (TAStateSize)
  {
    if (NVar == 0) {
      for (auto it = RA2.abstractedStates.begin ();it != RA2.abstractedStates.end (); it++) {
        m_initialStates[std::make_tuple(it->first,it->second.toTuple(),it->second.toTuple())].push_back(it-RA2.abstractedStates.begin ());
      }
    } else {
      for (auto it = RA2.abstractedStates.begin ();it != RA2.abstractedStates.end (); it++) {
        std::shared_ptr<Abstraction> re1,re2;
        it->second.cutVars (re1,0,NVar-1);
        it->second.cutVars (re2,NVar,NVar*2-1);

        m_initialStates[std::make_tuple(it->first,re1->toTuple(),re2->toTuple())].push_back(it-RA2.abstractedStates.begin ());
      }
    }
  }
  bool operator()(const std::vector<RAState> &r1,const std::vector<RAState> &r2) {
    if (r1.size () != r2.size ()) {
      return false;
    }

    TAState iState = toIState (r1[0],r2[0]);

    Abstraction region1 = RA.abstractedStates[r1[0]].second;
    Abstraction region2 = RA.abstractedStates[r2[0]].second;

    // {
    //   std::shared_ptr<Abstraction> re1,re2;
    //   RA2.abstractedStates[RA2.initialStates.front()].second.cutVars(re1,0,NVar-1);
    //   RA2.abstractedStates[RA2.initialStates.front()].second.cutVars(re2,NVar,2*NVar-1);
    // }

    std::vector<RAState> CStates = m_initialStates[std::make_tuple(iState,region1.toTuple(),region2.toTuple())];
    assert(!CStates.empty());

    if (NVar == 0) {
      for (std::size_t i = 1; i < r1.size(); i++) {
        const TAState iState = toIState (r1[i],r2[i]);

        std::vector<RAState> NStates;

        for (const RAState s: CStates ) {
          for (const NFA::Edge &e: RA2.edges[s]) {
            if (RA2.abstractedStates[e.target].first == iState) {
              NStates.push_back (e.target);
            }
          }
        }
        if (NStates.empty()) {
          return false;
        }
        CStates = NStates;
      }
    } else {
      for (std::size_t i = 1; i < r1.size(); i++) {
        const TAState iState = toIState (r1[i],r2[i]);

        const Abstraction region1 = RA.abstractedStates[r1[i]].second;
        const Abstraction region2 = RA.abstractedStates[r2[i]].second;
        std::vector<RAState> NStates;

        for (const auto s: CStates ) {
          for (const NFA::Edge &e: RA2.edges[s]) {
            std::shared_ptr<Abstraction> re1,re2;
            RA2.abstractedStates[e.target].second.cutVars (re1,0,NVar-1);
            RA2.abstractedStates[e.target].second.cutVars (re2,NVar,NVar*2-1);

            if (RA2.abstractedStates[e.target].first == iState && *re1 == region1 && *re2 == region2) {
              NStates.push_back (e.target);
            }
          }
        }
        if (NStates.empty()) {
          return false;
        }
        CStates = NStates;
      }
    }

    return true;
  }

private:
  const std::size_t NVar;
  const TAState TAStateSize;
  AbstractionAutomaton<Abstraction> &RA2;
  AbstractionAutomaton<Abstraction> &RA;
  // (TAState,([(Int,Int)],[[Int]], [Int],),([(Int,Int)],[[Int]], [Int])) -> [RAState]
  // This seems ((s1,s2),\alpha1,\alpha2) -> ((s1,\alpha1),(s2,\alpha2))
  boost::unordered_map<std::tuple<TAState,typename Abstraction::Tuple,typename Abstraction::Tuple >,std::vector<RAState> > m_initialStates;
  inline TAState toIState(RAState s1,RAState s2) const {
    TAState taState1 = RA.abstractedStates[s1].first;
    TAState taState2 = RA.abstractedStates[s2].first;

    return taState1 + TAStateSize * taState2;
  }
};


#include "../src/data.hpp"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/options_description.hpp>
#include <cstddef>
#include <cstdlib>
#include <fstream>
#include <iostream>

using Number = double;
constexpr std::size_t DIMENSION = 3;

namespace po = boost::program_options;

int main(int argc, char **argv) {
  n_body::data::Bodies<Number, DIMENSION> bodies;
  po::options_description description("options");
  description.add_options()("help,h", "print help message");
  description.add_options()("number,n", po::value<unsigned>(),
                            "number of bodies");
  description.add_options()("density,d", po::value<Number>()->default_value(1),
                            "average density(number/m^3)");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, description), vm);
  po::notify(vm);

  if (vm.count("help"))
    std::cout << description << std::endl;
  return 0;

  boost::archive::xml_oarchive ar(std::cout);
  ar << BOOST_SERIALIZATION_NVP(bodies);
}


#include <vector>
#include <set>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <map>
#include <string>
#include <limits>
#include <cstdlib>
#include <algorithm>
#include <exception>
#include <chrono>
#include <thread>
using namespace std;

#include <boost/algorithm/string.hpp>
#include <sw2v.h>
#if LOCAL
#include <omp.h>
#endif

namespace sw2v {

bool SecondGreater (const pair<int, float> & a,
                 const pair<int, float> & b) {
  return a.second > b.second;
}

void SparseWord2Vec::LoadVocab(const char * fname) {
  ifstream in(fname);
  string line;
  getline(in, line);
  int size = atoi(line.c_str());
  words_ = vector<string>(size);
  freq_ = vector<int>(size, 0);
  while(getline(in, line)) {
    istringstream iss(line);
    string word;
    int index, freq;
    iss >> word >> index >> freq;
    words_[index] = word;
    freq_[index] = freq;
  }
  in.close();
  cout << "vocab size: " << size << endl;

  int nword = freq_.size();
#if LOCAL
  model_ = vector<float>(nword * nhidden_, 0);
  for(int i = 0; i < model_.size(); i++) {
    model_[i] = (RAND01() - 0.5) / sqrt(1.0 + (float)nhidden_);
  }
#else
  int model_size = nword * nhidden_;
  vector<ps::Key> keys(nword, 0);
  vector<float> vals(nword, 0);
  for(int i = 0; i < model_size; i++) {
    keys[i] = rand() % model_size;
    vals[i] = (RAND01() - 0.5) / sqrt(float(nhidden_) + 1.0);
  }
  kv_->Wait(kv_->Push(keys, vals));
  cout << "send init data ok" << endl;
#endif

  for(int i = 0; i < nword; i++) {
    int f = (int)pow((double)freq_[i], 0.75);
    for(int j = 0; j < f; j++) {
      negative_.push_back(i);
    }
  }

}

bool SparseWord2Vec::SkipFreqWord(int w) {
  float fw = (float)freq_[w];
  float t = 1000.0;
  float p = (fw - t) / fw;
  p -= sqrt(t / fw);
  if(RAND01() < p) return true;
  return false;
}

void SparseWord2Vec::SaveModel(const char * filename) {
  int model_size = freq_.size() * nhidden_;
  int nword = freq_.size();
  cout << "save model" << endl;
  ofstream out(filename);
  for(int w = 0; w < nword; w++) {
    if(freq_[w] < 5) continue;
    vector<ps::Key> keys(nhidden_, 0);
    for(int k = 0; k < nhidden_; k++) {
      keys[k] = w * nhidden_ + k;
    }
    vector<float> vals;
    kv_->Wait(kv_->Pull(keys, &vals));
    for(int k = 0; k < nhidden_; k++) {
      out << w << "\t" << words_[w] << "\t" << vals[k] << endl;
    }
  }
  out.close();
  cout << "save model ok" << endl;
}

float auc(vector< pair<int, float> > & label_preds) {
  sort(label_preds.begin(), label_preds.end(), SecondGreater);
  long long m = 0;
  long long n = 0;
  long long p = 0;
  int nsamples = label_preds.size();
  for(int i = 0; i < nsamples; i++) {
    pair<int, float> & e = label_preds[i];
    if(e.first == 1) {
      m += 1;
      p += (nsamples - i);
    }
    else n += 1;
  }
  p -= m * (m + 1);
  return (float)((double)(p) / (double)(m * n));
}

void SparseWord2Vec::Train(DataIter & iter) {
  int n = 0;
  cout << "begin training..." << endl;
  float pred = 0.0;
  while(true) {
    vector<int> data = iter.NextWords(10000);
    cout << "data size: " << data.size() << endl;
    if(data.size() == 0) break;
    vector<Sample> batch;
    batch.reserve(data.size() * (win_size_ + 1) + 1);
#if LOCAL
#pragma omp parallel for num_threads(20)
#endif
    for(int k = 0; k < data.size(); k++) {
      int start = MAX(0, k - win_size_);
      int end = MIN(data.size() - 1, k + win_size_);
      if(SkipFreqWord(data[k])) continue;
      Sample s(1.0, data[k]);
      for(int j = start; j <= end && j < data.size(); j++) {
        if(j == k) continue;
        s.context_.push_back(data[j]);
      }
#if LOCAL
      LockAll();
#endif
      batch.push_back(s);
#if LOCAL
      UnLockAll();
#endif
      for(int j = 0; j < win_size_; j++) {
        int k2 = rand() % negative_.size();
        if(negative_[k2] == data[k]) continue;
        if(SkipFreqWord(negative_[k2])) continue;
        Sample s2(0.0, negative_[k2]);
        s2.context_ = s.context_;
#if LOCAL
        LockAll();
#endif
        batch.push_back(s2);
#if LOCAL
        UnLockAll();
#endif
      }
    }

    int nbatch = batch.size() / batch_size_;
    vector< pair<int, float> > label_preds;
    label_preds.reserve(batch.size() + 1);

#if LOCAL
#pragma omp parallel for num_threads(20)
#endif
    for(int b = 0; b < nbatch; b++) {
      //if (b % 1000 == 0)
      //  cout << b << "\t" << nbatch << endl;
      int start = b * batch_size_;
      int end = MIN(start + batch_size_, batch.size() - 1);
      if(end <= start) continue;
      vector<Sample> sub_batch(batch.begin() + start, batch.begin() + end);
      vector< pair<int, float> > sub_label_preds = MiniBatch(sub_batch);
      for(int j = 0; j < sub_label_preds.size(); j++)
        label_preds.push_back(sub_label_preds[j]);
    }
    cout << n++ << "\t" << auc(label_preds) << endl;
    if(batch_size_ < 256) batch_size_ *= 2;
  }
  //SaveModel();
}

inline float dot(const vector<float> & f1, const vector<float> & f2) {
  int n = f1.size();
  float ret = 0.0;
  for(int i = 0; i < n; i++) {
    ret += f1[i] * f2[i];
  }
  return ret;
}

inline void AddVector(vector<float> & dst, const vector<float> & src) {
  int n = dst.size();
  for(int i = 0; i < n; i++) {
    dst[i] += src[i];
  }
}

inline float sign(const float x) {
  if(x < 0) return -1.0;
  else if(x > 0) return 1.0;
  else return 0.0;
}

vector< pair<int, float> > SparseWord2Vec::MiniBatch(const vector<Sample> & batch) {
  vector< pair<int, float> > ret;
  if(batch.size() == 0) return ret;
  ret.reserve(batch.size() + 1);
  unordered_map<int, vector<float> > batch_model;
  unordered_map<int, vector<float> > grads;
  set<int> words;
  for(int i = 0; i < batch.size(); i++) {
    words.insert(batch[i].target_);
    for(int j = 0; j < batch[i].context_.size(); j++) {
      words.insert(batch[i].context_[j]);
    }
  }
  vector<int> vwords(words.begin(), words.end());
  sort(vwords.begin(), vwords.end());

#if LOCAL
  vector<int> keys;
#else
  vector<ps::Key> keys;
#endif
  for(int i = 0; i < vwords.size(); i++) {
    int w = vwords[i];
    for(int j = w*nhidden_; j < (w+1)*nhidden_; j++) {
      keys.push_back(j);
    }
  }

  vector<float> vals;
#if LOCAL
  vals = vector<float>(keys.size(), 0);
  for(int i = 0; i < keys.size(); i++) {
    vals[i] = model_[keys[i]];
  }
#else
  kv_->Wait(kv_->Pull(keys, &vals));
#endif

  CHECK_EQ(vals.size(), keys.size());

  for(int i = 0; i < vwords.size(); i++) {
    CHECK_LT(i * nhidden_, keys.size());
    int w = keys[i*nhidden_] / nhidden_;
    vector<float> sub;
    for(int j = i * nhidden_; j < i * nhidden_ + nhidden_ && j < vals.size(); j++) {
      sub.push_back(vals[j]);
    }
    CHECK_EQ(sub.size(), nhidden_);
    batch_model[w] = sub;
  }

  for(unordered_map<int, vector<float> >::const_iterator i = batch_model.begin();
      i != batch_model.end(); i++) {
    grads[i->first] = vector<float>(nhidden_, 0);
  }

  for(int i = 0; i < batch.size(); i++) {
    float pred = OneStep(batch[i].target_, batch[i].context_, batch[i].label_,
                         batch_model, grads);
    ret.push_back(pair<int, float>((int)batch[i].label_, pred));
  }

  CHECK_EQ(grads.size(), vwords.size());

  for(int i = 0; i < vwords.size(); i++) {
    CHECK_LT(i * nhidden_, keys.size());
    int w = keys[i * nhidden_] / nhidden_;
    vector<float> & sub = grads[w];
    for(int j = 0; j < nhidden_; j++) {
      CHECK_LT(i * nhidden_ + j, vals.size());
      vals[i*nhidden_ + j] = sub[j];
    }
  }

#if LOCAL
  LockAll();
  for(int i = 0; i < keys.size(); i++) {
    model_[keys[i]] += vals[i];
  }
  UnLockAll();
#else
  kv_->Wait(kv_->Push(keys, vals));
  return ret;
#endif

}
float SparseWord2Vec::OneStep(int w1, const vector<int> & w2s, float label,
                              const unordered_map<int, vector<float> > & batch_model,
                              unordered_map<int, vector<float> > & grads) {
  float lambda_ = 0.00001;
  float pred = 0.0;
  unordered_map<int, vector<float> >::const_iterator mi;

  mi = batch_model.find(w1);
  CHECK_NE(mi, batch_model.end());
  const vector<float> & f1 = mi->second;
  CHECK_EQ(f1.size(), nhidden_);
  vector<float> sf2(nhidden_, 0.0);

  for(int i = 0; i < w2s.size(); i++) {
    int w2 = w2s[i];
    mi = batch_model.find(w2);
    CHECK_NE(mi, batch_model.end());
    const vector<float> & f2 = mi->second;
    AddVector(sf2, f2);
    pred += dot(f1, f2);
  }

  pred = Sigmoid(pred);
  float err = (label - pred);

  unordered_map<int, vector<float> >::iterator gi = grads.find(w1);
  CHECK_NE(gi, grads.end());
  vector<float> & d1 = gi->second;

  CHECK_EQ(f1.size(), nhidden_);
  CHECK_EQ(sf2.size(), nhidden_);
  for (int i = 0; i < nhidden_; i++) {
    float v1 = f1[i];
    float v2 = sf2[i];
    d1[i] += learning_rate_ * (err * v2 - lambda_ * v1);
  }

  for(int i = 0; i < w2s.size(); i++) {
    int w2 = w2s[i];
    mi = batch_model.find(w2);
    CHECK_NE(mi, batch_model.end());

    const vector<float> & f2 = mi->second;
    CHECK_EQ(f2.size(), nhidden_);

    gi = grads.find(w2);
    CHECK_NE(gi, grads.end());
    vector<float> & d2 = gi->second;
    CHECK_EQ(d2.size(), nhidden_);
    for(int j = 0; j < nhidden_; j++) {
      float v1 = f1[j];
      float v2 = f2[j];
      d2[j] += learning_rate_ * (err * v1 - lambda_ * v2);
    }
  }

  return pred;
}

}


//Copyright (c) 2015 Zachary Kann
//
//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.

// ---
// Author: Zachary Kann

// A set of functions for calculation of electric field / frequency maps
// as outlined in the papers of Skinner et al. See, for example, S.M. Gruenbaum,
// et al. J. Chem. Theory Comput. 2013 9 (7), 3109. These maps are necessary
// for calculation of vibrational spectra using the methodology  outline in the
// aforementioned papers.

// Abbreviations:
//   FTIR = fourier-transform infrared
//   SFG = sum-frequency generation

#include <armadillo>
#include "z_frequency_map.hpp"
#include "z_histogram.hpp"
#include "z_molecule_group.hpp"
#include "z_cx_tcf.hpp"

#ifndef _Z_INFRARED_MAP_HPP_
#define _Z_INFRARED_MAP_HPP_

class InfraredMap:
  public FrequencyMap {
 public:
  inline InfraredMap(const MoleculeGroup& group, const Chromophore chromophore,
                      const double timestep, const int correlation_length = 200)
      : FrequencyMap(group, chromophore, timestep, correlation_length) {
    mu_01_ = arma::zeros<arma::cube>(DIMS, steps_guess_, num_chromophores());
  }

 private:
  arma::cube mu_01_;

  inline void ResizeArrays() {
    omega_01_.resize(num_chromophores(), steps_guess_);
    mu_01_.resize(DIMS, steps_guess_, num_chromophores());
  }

  inline double MuOrAlphaProduct(const int chromophore, const int corr_start,
                                 const int i_corr) {
    return arma::dot(mu_01_.slice(chromophore).col(corr_start),
                     mu_01_.slice(chromophore).col(corr_start+i_corr));
  }

  // Calculates omega and mu/alpha using the frequency maps.
  void UseMapping(const int chromophore,
                  const arma::rowvec& box);
};
#endif


#ifndef LEPP3_FILTER_CLOUD_POST_PROB_FILTER_H__
#define LEPP3_FILTER_CLOUD_POST_PROB_FILTER_H__

#include "lepp3/filter/cloud/post/CloudPostFilter.hpp"
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

namespace lepp {

/**
 * An implementation of a `CloudPostFilter` where points are included only
 * if they have been seen in a certain percentage of frames of the last N
 * frames.
 */
template<class PointT>
class ProbFilter : public CloudPostFilter<PointT> {
  using MapPoint = lepp::cloud_post_filter::MapPoint;

public:
  ProbFilter(bool larger_voxelization = false)
    : larger_voxelization_(larger_voxelization) {}

  virtual void newFrame() override;
  virtual void newPoint(PointT& p, PointCloudT& filtered) override;
  virtual void getFiltered(PointCloudT& filtered) override;

private:
  boost::unordered_set<MapPoint> this_frame_;
  boost::unordered_map<MapPoint, uint32_t> all_points_;
  MapPoint min_pt;
  MapPoint max_pt;

  bool const larger_voxelization_;
};
}

template<class PointT>
void lepp::ProbFilter<PointT>::newFrame() {
  this_frame_.clear();
  min_pt.x = std::numeric_limits<int>::max();
  min_pt.y = std::numeric_limits<int>::max();
  min_pt.z = std::numeric_limits<int>::max();
  max_pt.x = std::numeric_limits<int>::min();
  max_pt.y = std::numeric_limits<int>::min();
  max_pt.z = std::numeric_limits<int>::min();
}

template<class PointT>
void lepp::ProbFilter<PointT>::newPoint(PointT& p, PointCloudT& filtered) {
  MapPoint map_point = MapPoint(p.x * 100, p.y * 100, p.z * 100);

  if (larger_voxelization_) {
    int const mask = 0xFFFFFFFE;
    map_point.x &= mask;
    map_point.y &= mask;
    map_point.z &= mask;
  }

  uint32_t& ref = all_points_[map_point];
  ref <<= 1;
  ref |= 1;
  this_frame_.insert(map_point);
  // Keep track of the min/max points so that we know the bounding box of
  // the current cloud.
  min_pt.x = std::min(min_pt.x, map_point.x);
  min_pt.y = std::min(min_pt.y, map_point.y);
  min_pt.z = std::min(min_pt.z, map_point.z);
  max_pt.x = std::max(max_pt.x, map_point.x);
  max_pt.y = std::max(max_pt.y, map_point.y);
  max_pt.z = std::max(max_pt.z, map_point.z);
}

template<class PointT>
void lepp::ProbFilter<PointT>::getFiltered(PointCloudT& filtered) {
  boost::unordered_map<MapPoint, uint32_t>::iterator it =
      all_points_.begin();
  while (it != all_points_.end()) {
    if (this_frame_.find(it->first) == this_frame_.end()) {
      it->second <<= 1;
    }
    int count = __builtin_popcount(it->second);
    if (count >= 10) {
      PointT pt;
      pt.x = it->first.x / 100.;
      pt.y = it->first.y / 100.;
      pt.z = it->first.z / 100.;
      filtered.push_back(pt);
    }

    // Allow for a bit of leeway with removing points at the very boundary of
    // the bounding box. To do this, the bounding box is increased by 10 [cm]
    // in each direction. Only points within *this* bounding box are kept in
    // the map -- all others removed.
    // TODO See if tweaking these values up/down yields any benefits.
    min_pt.x -= 10;
    min_pt.y -= 10;
    min_pt.z -= 10;
    max_pt.x += 10;
    max_pt.y += 10;
    max_pt.z += 10;
    MapPoint const& pt = it->first;
    if (pt.x < min_pt.x || pt.x > max_pt.x ||
        pt.y < min_pt.y || pt.y > max_pt.y ||
        pt.z < min_pt.z || pt.z > max_pt.z) {
      all_points_.erase(it++);
    } else {
      ++it;
    }
  }
}

#endif


#ifndef REFL_TUPLE_TRAITS_HPP
#define REFL_TUPLE_TRAITS_HPP

#include "type_traits.hpp"
#include <boost/hana/concat.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>

namespace reflect {

namespace metautils {

/**
 * @brief Concatenating multiple tuples into one
 */
constexpr decltype(auto) multiple_concat() {
  return ::boost::hana::make_tuple();
}

template <class T> constexpr decltype(auto) multiple_concat(T &&value) {
  return ::boost::hana::concat(::std::forward<T>(value), multiple_concat());
}

/**
 * @brief Concating multiple tuples into one
 *
 * @param value boost::hana::tuple
 * @param args template pack of boost::hana::tuple
 * @return concatenated tuple
 */
template <class T, class... Args>
constexpr decltype(auto) multiple_concat(T &&value, Args &&... args) {
  return ::boost::hana::concat(::std::forward<T>(value),
                               multiple_concat(::std::forward<Args>(args)...));
}

namespace detail {

/**
 * @brief Compare types index implementation
 *
 * @param A type A
 * @param B type B
 * @return tuple of index if A and B same type or empty tuple
 */
template <long long Index, class A, class B>
constexpr decltype(auto) compare_types_index_impl(A &&, B &&) {
  if constexpr (::std::is_same_v<::std::decay_t<A>, ::std::decay_t<B>>)
    return ::boost::hana::tuple_c<long long, Index>;
  else
    return ::boost::hana::make_tuple();
}

/**
 * @brief Find same type of object in tuple implementation
 *
 * @param std::integer_sequence<long long,Indices...> index sequence
 * @param value object to find
 * @param tup tuple where to find
 */
template <class T, class Tp, long long... Indices>
constexpr decltype(auto)
find_values_args_impl(::std::integer_sequence<long long, Indices...> &&,
                      T &&value, Tp &&tup) {
  return metautils::multiple_concat(compare_types_index_impl<Indices>(
      ::std::forward<T>(value),
      ::boost::hana::at_c<Indices>(::std::forward<Tp>(tup)))...);
}

/**
 * @brief Tuple range of integral constants implementation
 *
 * @param std::nteger_sequence<long long,Indices...> index sequence
 */
template <long long Offset, long long... Indices>
constexpr decltype(auto)
gen_inds_tup_seq_impl(::std::integer_sequence<long long, Indices...> &&) {
  return ::boost::hana::tuple_c<long long, Indices + Offset...>;
}

/**
 * @brief Create a copy of the tuple by the given element indices
 * @param ::std::nteger_sequence<long long,Indices...> - index sequence
 * @param tup - tuple to copy
 * @param seq_tup - tuple of element indices
 */
template <long long... Indices, class Tuple, class IndexTuple>
constexpr decltype(auto)
copy_tuple_sequence_impl(::std::integer_sequence<long long, Indices...> &&,
                         Tuple &&tup, IndexTuple &&seq_tup) {
  return ::boost::hana::make_tuple(
      ::boost::hana::at(tup, ::boost::hana::at_c<Indices>(seq_tup))...);
}

/**
 * @brief Concating all tuples in one
 * @param ::std::nteger_sequence<long long,Indices...> - index sequence
 * @param tup - tuple of tuples
 */
template <long long... Indices, class Tuple>
constexpr decltype(auto)
merge_tuple_of_tuples_impl(::std::integer_sequence<long long, Indices...> &&,
                           Tuple &&tup) {
  return multiple_concat(::boost::hana::at_c<Indices>(tup)...);
}
} // namespace detail

/**
 * @brief compare types
 * @return true if types same, othervise false
 */
template <class A, class B>
constexpr decltype(auto) compare_types(A &&, B &&) noexcept {
  if constexpr (::std::is_same_v<::std::decay_t<A>, ::std::decay_t<B>>)
    return true;
  else
    return false;
}

/**
 * @brief Function that finds same type of object in tuple
 *
 * @param value object to find
 * @param tup tuple where to find
 * @return Tuple of integral constants, which are indices of tuple elements with
 * same type
 */
template <class T, class... Args>
constexpr decltype(auto) find_value_types(T &&value,
                                          ::boost::hana::tuple<Args...> &&tup) {
  return detail::find_values_args_impl(
      ::std::make_integer_sequence<long long, sizeof...(Args)>(),
      ::std::forward<T>(value),
      ::std::forward<::boost::hana::tuple<Args...>>(tup));
}

/**
 * @brief Function that finds same type of object in tuple
 *
 * @param value Object to find
 * @param tup Tuple where to find
 * @return tuple of integral constants, which are indices of tuple elements with
 * same type
 */
template <class T, class... Args>
constexpr decltype(auto)
find_value_types(T const &value, ::boost::hana::tuple<Args...> const &tup) {
  return detail::find_values_args_impl(
      ::std::make_integer_sequence<long long, sizeof...(Args)>(), value, tup);
}

/**
 * @brief Generate tuple of integral constants from [Offset; N+Offset)
 * @return Ruple of intergral constants
 */
template <class N, class Offset = ::boost::hana::llong<0>>
constexpr decltype(auto) gen_inds_tup() {
  return detail::gen_inds_tup_seq_impl<::std::decay_t<Offset>::value>(
      ::std::make_integer_sequence<long long, ::std::decay_t<N>::value>());
}

/**
 * @brief Create a copy of the tuple by the given element indices
 * @param tup - tuple to copy
 * @param seq_tup - tuple of element indices
 */
template <class Tuple, class IndexTuple>
constexpr decltype(auto) copy_tuple_sequence(Tuple &&tup,
                                             IndexTuple &&seq_tup) {
  return detail::copy_tuple_sequence_impl(
      ::std::make_integer_sequence<long long, decltype(::boost::hana::size(
                                                  seq_tup))::value>(),
      ::std::forward<Tuple>(tup), ::std::forward<IndexTuple>(seq_tup));
}

/**
 * @brief Concating all tuples in one
 * @param tup - tuple of tuples
 */
template <class Tuple>
constexpr decltype(auto) merge_tuple_of_tuples(Tuple &&tup) {
  return detail::merge_tuple_of_tuples_impl(
      ::std::make_integer_sequence<long long,
                                   decltype(::boost::hana::size(tup))::value>(),
      ::std::forward<Tuple>(tup));
}
} // namespace metautils
} // namespace reflect

#endif // TUPLE_TRAITS_HPP


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_NOT_GREATER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NOT_GREATER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_not_greater capabilities

    Returns @ref True if x is not greater than y else returns @ref False.

    @par Semantic:

    @code
    as_logical_t<T> r = is_not_greater(x,y);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = ;
    @endcode

    @par Note:

    Due to existence of nan, this is not equivalent to @c is_less_equal(x, y)
    for floating types

    @par Alias:

    @c is_ngt

  **/
  const boost::dispatch::functor<tag::is_not_greater_> is_not_greater = {};
} }
#endif

#include <boost/simd/function/scalar/is_not_greater.hpp>
#include <boost/simd/function/simd/is_not_greater.hpp>

#endif


#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/geometry/_pure/_base.hpp"
#include "IceRay/geometry/volumetric/mist.hpp"

void expose_IceRay_geometry_mist()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( core );
  MAKE_SUBMODULE( geometry );

  typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D  GTs_coord3D;
  typedef GS_DDMRM::S_IceRay::S_type::GT_scalar    Tf_scalar;

  typedef GS_DDMRM::S_IceRay::S_geometry::S__pure::GC__base        GTs__base;
  typedef GS_DDMRM::S_IceRay::S_geometry::S_volumetric::GC_mist         Tf_mist;

  typedef  Tf_scalar const& (Tf_mist::*Tf_getScalar)(void) const;
  typedef  bool (Tf_mist::*Tf_setScalar  )(Tf_scalar const&);

  //typedef  GTs__base const& (Tf_mist::*Tf_getHull )(void) const;
  typedef  bool (Tf_mist::*Tf_setHull  )( GTs__base * );

  Tf_getScalar I_getPrecision = & Tf_mist::F_precision;
  Tf_setScalar I_setPrecision = & Tf_mist::F_precision;

  Tf_getScalar I_getDensity = & Tf_mist::F_density;
  Tf_setScalar I_setDensity = & Tf_mist::F_density;

//Tf_getHull I_getHull = &Tf_mist::F_hull;
  Tf_setHull I_setHull = &Tf_mist::F_hull;

  boost::python::class_< Tf_mist, boost::python::bases<GTs__base> >( "GeometryVolumetricMist" )
    .def( boost::python::init<>() )
    .def("precision",  I_getPrecision, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("precision",  I_setPrecision )
    .def("density",    I_getDensity, boost::python::return_value_policy<boost::python::copy_const_reference>() )
    .def("density",    I_setDensity )
  //.def( "hull", I_getHull, boost::python::return_internal_reference<>() )
    .def( "hull", I_setHull      )
    ;

 }


#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <map>
#include <boost/optional.hpp>

static constexpr size_t NUM_FLOORS = 4;
static constexpr uint64_t MAX_MOVES = 100;

// Example input
//static constexpr size_t NUM_ITEMS = 4;
//static std::string const NAMES[] = { "HG", "HM", "LG", "LM" };
//enum class ITEM
//{
//    HYDROGEN_RTG,
//    HYDROGEN_CHIP,
//    LITHIUM_RTG,
//    LITHIUM_CHIP,
//};

// Real input
static constexpr size_t NUM_ITEMS = 14;
static std::string const NAMES[] = { "PolG", "PolM", "ThuG", "ThuM", "ProG", "ProM", "RutG", "RutM", "CobG", "CobM", "EleG", "EleM", "DilG", "DilM" };
enum class ITEM
{
    polonium_generator,
    polonium_chip,
    thulium_generator,
    thulium_chip,
    promethium_generator,
    promethium_chip,
    ruthenium_generator,
    ruthenium_chip,
    cobalt_generator,
    cobalt_chip,
    elerium_generator,
    elerium_chip,
    dilithium_generator,
    dilithium_chip,
};

bool is_chip(ITEM item)
{
    return (int(item) & 1) == 1;
}

bool is_rtg(ITEM item)
{
    return (int(item) & 1) == 0;
}

bool are_paired(ITEM item_a, ITEM item_b)
{
    return (int(item_a) & ~1) == (int(item_b) & ~1);
}

class State
{
    uint64_t m_value;

public:
    State()
        : m_value(0)
    {
    }

    explicit State(int part)
        : m_value(0)
    {
        set_elevator(1);

        // Example start state
        //set_item(ITEM::HYDROGEN_RTG, 2);
        //set_item(ITEM::HYDROGEN_CHIP, 1);
        //set_item(ITEM::LITHIUM_RTG, 3);
        //set_item(ITEM::LITHIUM_CHIP, 1);

        // Real start state
        set_item(ITEM::polonium_generator, 1);
        set_item(ITEM::polonium_chip, 2);
        set_item(ITEM::thulium_generator, 1);
        set_item(ITEM::thulium_chip, 1);
        set_item(ITEM::promethium_generator, 1);
        set_item(ITEM::promethium_chip, 2);
        set_item(ITEM::ruthenium_generator, 1);
        set_item(ITEM::ruthenium_chip, 1);
        set_item(ITEM::cobalt_generator, 1);
        set_item(ITEM::cobalt_chip, 1);

        if (part == 1)
        {
            set_item(ITEM::elerium_generator, 4);
            set_item(ITEM::elerium_chip, 4);
            set_item(ITEM::dilithium_generator, 4);
            set_item(ITEM::dilithium_chip, 4);
        }
        else
        {
            set_item(ITEM::elerium_generator, 1);
            set_item(ITEM::elerium_chip, 1);
            set_item(ITEM::dilithium_generator, 1);
            set_item(ITEM::dilithium_chip, 1);
        }
    }

    State(const State &) = default;
    State &operator =(const State &) = default;
    ~State() = default;

    static State get_complete_state()
    {
        State result;
        result.set_elevator(NUM_FLOORS);
        for (int item_index = 0; item_index < NUM_ITEMS; ++item_index)
            result.set_item(ITEM(item_index), NUM_FLOORS);
        return result;
    }

    int get_elevator() const
    {
        return get_index(0);
    }

    int get_item(ITEM item) const
    {
        return get_index(int(item) + 1);
    }

    void set_elevator(int floor)
    {
        set_index(0, floor);
    }

    void set_item(ITEM item, int floor)
    {
        set_index(int(item) + 1, floor);
    }

    bool operator <(const State &other) const
    {
        return m_value < other.m_value;
    }

    bool is_complete_state() const
    {
        if (get_elevator() != NUM_FLOORS)
            return false;

        for (int item_index = 0; item_index < NUM_ITEMS; ++item_index)
        {
            if (get_item(ITEM(item_index)) != NUM_FLOORS)
                return false;
        }

        return true;
    }

    std::vector<ITEM> items_on_floor(int floor) const
    {
        std::vector<ITEM> result;
        for (int item_index = 0; item_index < NUM_ITEMS; ++item_index)
        {
            ITEM item = ITEM(item_index);

            if (get_item(item) == floor)
                result.push_back(item);
        }
        return result;
    }

    bool are_any_chips_fried() const
    {
        for (int chip_index = 0; chip_index < NUM_ITEMS; ++chip_index)
        {
            ITEM chip = ITEM(chip_index);

            if (is_chip(chip))
            {
                const int chip_floor = get_item(chip);

                bool has_matching_rtg = false;
                bool has_other_rtg = false;

                for (int rtg_index = 0; rtg_index < NUM_ITEMS; ++rtg_index)
                {
                    ITEM rtg = ITEM(rtg_index);

                    if (is_rtg(rtg)
                        && (get_item(rtg) == chip_floor))
                    {
                        if (are_paired(chip, rtg))
                            has_matching_rtg = true;
                        else
                            has_other_rtg = true;
                    }
                }

                if (has_other_rtg
                    && !has_matching_rtg)
                {
                    // There is another RTG on the same floor as this chip,
                    // and this chip's matching RTG isn't on this floor so
                    // the chip shielding is not working - it's FRIED!!!

                    return true;
                }
            }
        }

        return false;
    }

    State reduce() const
    {
        // CRITICAL optimization - copied from redit
        // It doesn't matter which pairs are on which floors -
        // only the combination of pairs and floors.
        // As such, we can reduce a state to an equivalent
        // state

        State result(*this);

        std::vector<std::pair<int, int>> pair_floors;

        for (int item_index = 0; item_index < NUM_ITEMS; item_index += 2)
        {
            ITEM gen = ITEM(item_index);
            ITEM chip = ITEM(item_index + 1);

            pair_floors.push_back(std::make_pair(get_item(gen), get_item(chip)));
        }

        std::sort(pair_floors.begin(), pair_floors.end());

        for (int index = 0; index < pair_floors.size(); ++index)
        {
            ITEM gen = ITEM(2 * index);
            ITEM chip = ITEM((2 * index) + 1);

            result.set_item(gen, pair_floors[index].first);
            result.set_item(chip, pair_floors[index].second);
        }

        return result;
    }

    void print() const
    {
        std::vector<std::stringstream> streams(NUM_FLOORS + 1);

        for (int floor = 1; floor <= NUM_FLOORS; ++floor)
        {
            streams[floor] << "F" << floor << " ";
            if (get_elevator() == floor)
                streams[floor] << "E ";
            else
                streams[floor] << ". ";
        }

        for (int item_index = 0; item_index < NUM_ITEMS; ++item_index)
        {
            ITEM item = ITEM(item_index);

            for (int floor = 1; floor <= NUM_FLOORS; ++floor)
            {
                if (get_item(item) == floor)
                    streams[floor] << NAMES[item_index] << " ";
                else
                    streams[floor] << '.' << std::string(NAMES[item_index].size(), ' ');
            }
        }

        for (int floor = NUM_FLOORS; floor >= 1; --floor)
        {
            std::cout << streams[floor].str() << std::endl;
        }
    }

private:
    void set_index(int index, int floor)
    {
        uint64_t mask = uint64_t(0x7) << (index * 3);
        uint64_t value = uint64_t(floor) << (index * 3);

        m_value &= ~mask;
        m_value |= value;
    }

    uint64_t get_index(int index) const
    {
        return (m_value >> (index * 3)) & 0x7;
    }
};

class StatePathLengths
{
public:
    StatePathLengths() = delete;
    ~StatePathLengths() = default;

    explicit StatePathLengths(std::string &&name)
        : m_name(std::move(name))
        , m_state_lengths()
    {
    }

    bool is_better(const State &state, uint64_t length) const
    {
        auto it = m_state_lengths.find(state);

        return (it == m_state_lengths.end())
            || (length < it->second);
    }

    void add(const State &state, uint64_t length)
    {
        auto it = m_state_lengths.find(state);

        if (it == m_state_lengths.end())
            m_state_lengths[state] = length;
        else if (length < it->second)
            it->second = length;
    }

    bool empty() const
    {
        return m_state_lengths.empty();
    }

    void remove_next(State &state, uint64_t &length)
    {
        auto it = m_state_lengths.begin();

        state = it->first;
        length = it->second;

        m_state_lengths.erase(it);
    }

    uint64_t get_length(const State &state)
    {
        auto it = m_state_lengths.find(state);
        assert(it != m_state_lengths.end());

        return it->second;
    }

private:
    std::string m_name;
    std::map<State, uint64_t> m_state_lengths;
};

uint64_t solve(int part)
{
    StatePathLengths paths_found("paths_found");
    StatePathLengths paths_to_test("paths_to_test");
    uint64_t max_moves_to_search = MAX_MOVES;

    paths_to_test.add(State(part), 0);

    while (!paths_to_test.empty())
    {
        // Get the next state to test and
        // add it to the paths that we've found

        State cur_state;
        uint64_t cur_moves;

        paths_to_test.remove_next(cur_state, cur_moves);

        paths_found.add(cur_state, cur_moves);

        // Don't do any more testing if we've already moved
        // too many moves

        if (cur_moves >= max_moves_to_search)
            continue;

        // See which floor we're on and which floors we
        // can move to from here

        const int cur_floor = cur_state.get_elevator();

        std::vector<int> new_floors;
        if (cur_floor > 1)
            new_floors.push_back(cur_floor - 1);
        if (cur_floor < NUM_FLOORS)
            new_floors.push_back(cur_floor + 1);

        // See which items are on this floor which we can move

        std::vector<ITEM> items_on_floor = cur_state.items_on_floor(cur_floor);

        // Now, create all of the new states that we can get to

        std::vector<State> new_states;

        for (int new_floor : new_floors)
        {
            // Moving one item

            for (int i = 0; i < items_on_floor.size(); ++i)
            {
                State new_state = cur_state;
                new_state.set_elevator(new_floor);
                new_state.set_item(items_on_floor[i], new_floor);

                new_states.push_back(new_state);
            }

            // Moving two items

            for (int i = 0; i < items_on_floor.size(); ++i)
            {
                for (int j = i + 1; j < items_on_floor.size(); ++j)
                {
                    State new_state = cur_state;
                    new_state.set_elevator(new_floor);
                    new_state.set_item(items_on_floor[i], new_floor);
                    new_state.set_item(items_on_floor[j], new_floor);

                    new_states.push_back(new_state);
                }
            }
        }

        const uint64_t new_moves = cur_moves + 1;

        for (const State potentially_duplicate_new_state : new_states)
        {
            // CRITICAL OPTIMIZATION!!!
            const State new_state = potentially_duplicate_new_state.reduce();

            if (!paths_to_test.is_better(new_state, new_moves)
                || !paths_found.is_better(new_state, new_moves))
            {
                // We've already found a quicker path to this state -
                // don't re-test

                continue;
            }

            if (new_state.are_any_chips_fried())
            {
                // This move has fried at least one chip - it's
                // a dead end - don't continue

                continue;
            }

            if (new_state.is_complete_state())
            {
                // This is the complete state - save it if we've taken
                // the shortest path

                paths_found.add(new_state, new_moves);

                // Also, save this as the new max number of moves that we should search for

                if (new_moves < max_moves_to_search)
                    max_moves_to_search = new_moves;

                continue;
            }

            // OK - we want to keep searching from
            // this new state

            paths_to_test.add(new_state, new_moves);
        }
    }

    return paths_found.get_length(State::get_complete_state());
}

int main(int argc, char *argv[])
{
    uint64_t answer1 = solve(1);

    std::cout << "Answer #1: " << answer1 << std::endl;

    uint64_t answer2 = solve(2);

    std::cout << "Answer #2: " << answer2 << std::endl;
}