/**
 * @Copyright 2020, Wuhan University of Technology
 * @Author: Pengwei Zhou
 * @Date: 2020/12/28 下午3:53
 * @FileName: read_file.hpp
 * @Description: read point cloud from .txt .dat .bin .pcd
 * @License: See LICENSE for the license information
 */

#ifndef TLOAM_READ_FILE_HPP
#define TLOAM_READ_FILE_HPP

#include <fstream>
#include <string>
#include <iostream>
#include <Eigen/Core>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <memory>
#include <ros/ros.h>

namespace tloam {
class Point3f {
public:
  Point3f() : vec_(0.0f, 0.0f, 0.0f) {}

  Point3f(float x, float y, float z) : vec_(x, y, z) {
    assert(!HasNan());
  }

  Point3f(const Point3f &p) : vec_(p.vec_[0], p.vec_[1], p.vec_[2]) {
    assert(!p.HasNan());
  }

  static bool HasNan(float a, float b, float c) {
    return std::isnan(a) || std::isnan(b) || std::isnan(c);
  }

  bool HasNan() const {
    return std::isnan(vec_[0]) || std::isnan(vec_[1]) || std::isnan(vec_[2]);
  }

  float operator[](size_t i) const {
    assert(i >= 0 && i <= 2);
    return vec_[i];
  }

  float &operator[](size_t i) {
    assert(i >= 0 && i <= 2);
    return vec_[i];
  }

  inline const float &x() const {
    return vec_[0];
  }

  inline float &x() {
    return vec_[0];
  }

  inline const float &y() const {
    return vec_[1];
  }

  inline float &y() {
    return vec_[1];
  }

  inline const float &z() const {
    return vec_[2];
  }

  inline float &z() {
    return vec_[2];
  }

  friend std::ostream &operator<<(std::ostream &out, const Point3f &p) {
    out.width(6);
    out.precision(4);
    out << p.vec_[0] << ", " << p.vec_[1] << " , " << p.vec_[2] << std::endl;
    return out;
  }

  Eigen::Vector3f vec_;
};

class Point4f {
public:
  Point4f() : vec_(0.f, 0.f, 0.f, 1.f) {}

  Point4f(float x, float y, float z, float intensity) : vec_(x, y, z, intensity) {
    assert(!HasNaNs());
  }

  Point4f(const Point4f &p) : vec_(p.vec_[0], p.vec_[1], p.vec_[2], p.vec_[3]) {
    assert(!p.HasNaNs());
  }

  float operator[](int i) const {
    assert(i >= 0 && i <= 3);
    return vec_[i];
  }

  float &operator[](int i) {
    assert(i >= 0 && i <= 3);
    return vec_[i];
  }

  inline const float &x() const {
    return vec_[0];
  }

  inline float &x() {
    return vec_[0];
  }

  inline const float &y() const {
    return vec_[1];
  }

  inline float &y() {
    return vec_[1];
  }

  inline const float &z() const {
    return vec_[2];
  }

  inline float &z() {
    return vec_[2];
  }

  inline const float &intensity() const {
    return vec_[3];
  }

  inline float &intensity() {
    return vec_[3];
  }

  static bool HasNaNs(float a, float b, float c, float d) {
    return std::isnan(a) || std::isnan(b) || std::isnan(c) || std::isnan(d);
  }

  bool HasNaNs() const {
    return std::isnan(vec_[0]) || std::isnan(vec_[1]) || std::isnan(vec_[2]) || std::isnan(vec_[3]);
  }

  friend std::ostream &operator<<(std::ostream &out, const Point4f &p) {
    out.width(4);
    out.precision(3);
    out << p.vec_[0] << ", " << p.vec_[1] << ", " << p.vec_[2] << ", " << p.vec_[3] << std::endl;
    return out;
  }

  Eigen::Vector4f vec_;
};

class Point6f {
public:
  Point6f() {
    vec_.setZero();
  }

  Point6f(float x, float y, float z, float n_x, float n_y, float n_z) {
    vec_ << x, y, z, n_x, n_y, n_z;
    assert(!HasNaNs());
  }

  static bool HasNaNs(float a, float b, float c, float d, float e, float f) {
    return std::isnan(a) || std::isnan(b) || std::isnan(c) || std::isnan(d) || std::isnan(e) || std::isnan(f);
  }

  bool HasNaNs() const {
    return std::isnan(vec_[0]) || std::isnan(vec_[1]) || std::isnan(vec_[2]) || std::isnan(vec_[3]) ||
           std::isnan(vec_[4]) || std::isnan(vec_[5]);
  }

  float operator[](int i) const {
    assert(i >= 0 && i <= 5);
    return vec_[i];
  }

  float &operator[](int i) {
    assert(i >= 0 && i <= 5);
    return vec_[i];
  }

  inline const float &x() const {
    return vec_[0];
  }

  inline float &x() {
    return vec_[0];
  }

  inline const float &y() const {
    return vec_[1];
  }

  inline float &y() {
    return vec_[1];
  }

  inline const float &z() const {
    return vec_[2];
  }

  inline float &z() {
    return vec_[2];
  }

  inline const float &n_x() const {
    return vec_[3];
  }

  inline float &n_x() {
    return vec_[3];
  }

  inline const float &n_y() const {
    return vec_[4];
  }

  inline float &n_y() {
    return vec_[4];
  }

  inline const float &n_z() const {
    return vec_[5];
  }

  inline float &n_z() {
    return vec_[5];
  }

  friend std::ostream &operator<<(std::ostream &out, const Point6f &p) {
    out.width(4);
    out.precision(3);
    out << p.vec_[0] << ", " << p.vec_[1] << ", " << p.vec_[2] << ", " << p.vec_[3] << ", " << p.vec_[4] << ", "
        << p.vec_[5] << std::endl;
    return out;
  }

  Eigen::Matrix<float, 6, 1> vec_;
};

template<typename PointT, typename ContainerT>
void readPointsDat(const std::string &filename, ContainerT &points, int row_length) {
  std::ifstream in(filename.c_str());
  std::string line;
  boost::char_separator<char> separator(" ");
  // read .dat format file.
  // the first three dimensions represent normal vectors, and the second three dimensions represent coordinate information
  while (!in.eof()) {
    std::getline(in, line);
    in.peek();
    boost::tokenizer<boost::char_separator<char>> tokenIterator(line, separator);
    std::vector<std::string> tokens(tokenIterator.begin(), tokenIterator.end());
    if (tokens.size() != row_length)
      continue;
    auto x = boost::lexical_cast<float>(tokens[3]);
    auto y = boost::lexical_cast<float>(tokens[4]);
    auto z = boost::lexical_cast<float>(tokens[5]);
    points.emplace_back(PointT(x, y, z));
  }
  in.close();
}

template<typename PointT, typename ContainerT>
void readPointsText(const std::string &filename, ContainerT &points, int row_length) {
  std::ifstream in(filename.c_str());
  std::string line;
  boost::char_separator<char> separator(" ");
  while (!in.eof()) {
    std::getline(in, line);
    in.peek();
    boost::tokenizer<boost::char_separator<char>> tokenizer(line, separator);
    std::vector<std::string> tokens(tokenizer.begin(), tokenizer.end());
    if (tokens.size() != row_length)
      continue;
    auto x = boost::lexical_cast<float>(tokens[0]);
    auto y = boost::lexical_cast<float>(tokens[1]);
    auto z = boost::lexical_cast<float>(tokens[2]);

    points.emplace_back(PointT(x, y, z));
  }
  in.close();
}

template<typename PointT, typename ContainerT>
void readPoints(const std::string &filename, ContainerT &points, int row_length) {
  std::string surfix = filename.substr(filename.size() - 3, filename.size());
  if (std::strcmp(surfix.c_str(), "dat") == 0) {
    readPointsDat<PointT>(filename, points, row_length);
    return;
  }
  if (std::strcmp(surfix.c_str(), "txt") == 0) {
    readPointsText<PointT>(filename, points, row_length);
    return;
  }
  std::cerr << "wrong file format !!! " << std::endl;
  exit(EXIT_FAILURE);
}

template<typename T>
typename std::shared_ptr<T> readVelodyneToO3d(const std::string &filename) {
  std::fstream readFile(filename.c_str(), std::ios::in | std::ios::binary);
  if (!readFile.good()) {
    std::cerr << "Cloud not file..." << std::endl;
    exit(EXIT_FAILURE);
  }
  std::shared_ptr<T> pointCloud(new T);
  readFile.seekg(0, std::ios::beg);
  for (size_t i = 0; readFile.good() && !readFile.eof(); i++) {
    Point4f point;
    readFile.read((char *) &point.x(), 3 * sizeof(float));
    readFile.read((char *) &point.intensity(), sizeof(float));
    if (!point.HasNaNs()) {
      pointCloud->points_.emplace_back(Eigen::Vector3d(point.x(), point.y(), point.z()));
      pointCloud->intensity_.emplace_back(static_cast<double>(point.intensity()));
    }
  }
  readFile.close();
  return pointCloud;
}

}

#endif //TLOAM_READ_FILE_HPP


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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_UTIL_GET_CS_AS_RADIAN_HPP
#define BOOST_GEOMETRY_UTIL_GET_CS_AS_RADIAN_HPP

// obsolete? It is not used anymore (get_as_radian is usually OK)

#include <boost/geometry/core/cs.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

    /*!
        \brief Small meta-function defining the specified coordinate system,
        but then in radian units
    */
    template <typename CoordinateSystem>
    struct get_cs_as_radian {};

    template <typename Units>
    struct get_cs_as_radian<cs::geographic<Units> >
    {
        typedef cs::geographic<radian> type;
    };

    template <typename Units>
    struct get_cs_as_radian<cs::spherical<Units> >
    {
        typedef cs::spherical<radian> type;
    };

} // namespace detail
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_GET_CS_AS_RADIAN_HPP


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 4, 1> test_double_4_1(const Eigen::Matrix<double, 4, 1>& M) { return M; }
void export_double_4_1() { boost::python::def("test_double_4_1", test_double_4_1); }


#include "Geometry.h"
#include "State.h"
#include <boost/move/make_unique.hpp>
#include <engines/gis/Engine.h>
#include <gis/CoordinateTransformation.h>
#include <gis/SpatialReference.h>
#include <macgyver/Exception.h>
#include <ogr_geometry.h>

namespace SmartMet
{
namespace Plugin
{
namespace Dali
{
namespace Geometry
{
// ----------------------------------------------------------------------
/*!
 * \brief Get the GeoJSON specific geometry name
 */
// ----------------------------------------------------------------------

std::string name_geojson(const OGRGeometry& theGeom)
{
  auto id = theGeom.getGeometryType();
  switch (id)
  {
    case wkbPoint:
    case wkbPoint25D:
      return "Point";
    case wkbLineString:
    case wkbLineString25D:
    case wkbLinearRing:
      return "LineString";
    case wkbPolygon:
    case wkbPolygon25D:
      return "Polygon";
    case wkbMultiPoint:
    case wkbMultiPoint25D:
      return "MultiPoint";
    case wkbMultiLineString:
    case wkbMultiLineString25D:
      return "MultiLineString";
    case wkbMultiPolygon:
    case wkbMultiPolygon25D:
      return "MultiPolygon";
    case wkbGeometryCollection:
    case wkbGeometryCollection25D:
      return "GeometryCollection";
    default:
      throw Fmi::Exception(
          BCP, "Encountered an unknown geometry component when extracting geometry name");
  }

  throw Fmi::Exception(BCP, "Unknown geometry type when extracting geometry name");
}

// ----------------------------------------------------------------------
/*!
 * \brief Get the geometry name for the given format
 */
// ----------------------------------------------------------------------

std::string name(const OGRGeometry& theGeom, const std::string& theType)
{
  if (theType == "geojson")
    return name_geojson(theGeom);

  // By default we use WKT names
  return theGeom.getGeometryName();
}

// ----------------------------------------------------------------------
/*!
 * \brief Export the coordinates to GeoJSON format
 */
// ----------------------------------------------------------------------

std::string toGeoJSON(const OGRGeometry& theGeom,
                      const Fmi::Box& /* theBox */,
                      const Fmi::SpatialReference& theSRS)
{
  // Reproject to WGS84. TODO: Optimize if theSRS == WGS84.

  Fmi::CoordinateTransformation transformation(theSRS, "WGS84");

  // Reproject a clone
  boost::movelib::unique_ptr<OGRGeometry> geom(theGeom.clone());
  auto err = geom->transform(transformation.get());
  if (err != OGRERR_NONE)
    throw Fmi::Exception(BCP, "Failed to project geometry to WGS84 GeoJSON");

  // Fix winding rule to be CCW for shells
  boost::movelib::unique_ptr<OGRGeometry> geom2(Fmi::OGR::reverseWindingOrder(*geom));

  char* tmp = geom2->exportToJson();
  std::string ret = tmp;
  CPLFree(tmp);

  // Extract the coordinates

  std::size_t pos1 = ret.find('[');
  std::size_t pos2 = ret.rfind(']');

  if (pos1 == std::string::npos || pos2 == std::string::npos)
    throw Fmi::Exception(BCP, "Failed to extract GeoJSON from GDAL output");

  return ret.substr(pos1, pos2 - pos1 + 1);
}

// ----------------------------------------------------------------------
/*!
 * \brief Export the coordinates to KML format
 */
// ----------------------------------------------------------------------

std::string toKML(const OGRGeometry& theGeom,
                  const Fmi::Box& /* theBox */,
                  const Fmi::SpatialReference& theSRS)
{
  // Reproject to WGS84. TODO: Optimize if theSRS == WGS84.

  Fmi::CoordinateTransformation transformation(theSRS, "WGS84");

  // Reproject a clone
  boost::movelib::unique_ptr<OGRGeometry> geom(theGeom.clone());
  auto err = geom->transform(transformation.get());
  if (err != OGRERR_NONE)
    throw Fmi::Exception(BCP, "Failed to project geometry to WGS84 KML");

  char* tmp = geom->exportToKML();
  std::string ret = tmp;
  CPLFree(tmp);

  // Extract the coordinates

  return ret;
}

// ----------------------------------------------------------------------
/*!
 * \brief Export the coordinates to the given format
 */
// ----------------------------------------------------------------------

std::string toString(const OGRGeometry& theGeom,
                     const std::string& theType,
                     const Fmi::Box& theBox,
                     const Fmi::SpatialReference& theSRS,
                     double thePrecision)
{
  if (theType == "geojson")
    return toGeoJSON(theGeom, theBox, theSRS);

  if (theType == "kml")
    return toKML(theGeom, theBox, theSRS);

  return Fmi::OGR::exportToSvg(theGeom, theBox, thePrecision);
}

}  // namespace Geometry
}  // namespace Dali
}  // namespace Plugin
}  // namespace SmartMet


#pragma once
#include "libs/ml_models/general_types.h"
#include "utils/missing_values.hpp"
#include <boost/range/algorithm/transform.hpp>
#include <opencv2/core.hpp>
#include <opencv/ml.h>
#include <cstdint>
#include <limits>

namespace mlmodels
{

namespace detail
{

void copy_result(const cv::Mat& from, testing_result& to,
        std::size_t expected_size);

class_data copy_result(const cv::Mat& from, std::size_t expected_size);

cv::Mat prepare_samples(const training_data& from, int type = CV_32F);
cv::Mat prepare_samples(const class_data& from, int type = CV_32F);
cv::Mat prepare_labels(const classes_data& from, int type = CV_32SC1);
cv::Mat prepare_labels(const class_data& from, int type = CV_32F);
class_data::value_type predict_prob(class_data::value_type from);

template<typename ModelType>
struct general_model
{
    using model_t = ModelType;

    general_model() :
        handle{model_t::create()}
    {
    }

    general_model(cv::Ptr<model_t> m) :
        handle{m}
    {
    }

    bool class_train(const training_data& samples,
            const class_data& lables, int stype = CV_32F,
            int ctype = CV_32SC1)
    {
        return this->train(samples, lables, stype, ctype);
    }

    bool reg_train(const training_data& samples,
            const class_data& lables, int stype = CV_32F,
            int ctype = CV_32F)
    {
        return this->train(samples, lables, stype, ctype);
    }

    bool predict(const testing_data& testInput,
            testing_result& results) const
    {
        if (testInput.empty()) {
            return false;
        }
        results.reserve(rows(testInput));
        cv::Mat out_res;
        auto data(prepare_samples(testInput));
        handle->predict(data, out_res);
        copy_result(out_res, results, rows(testInput));
        return !results.empty();
    }

    value_type predict(const class_data& samples) const
    {
        if (samples.empty()) {
            return missing_value<value_type>();
        }
        auto data(prepare_samples(samples));
        return handle->predict(data);
    }

    class_data test(const testing_data& testInput, int classes) const
    {
        static_assert(std::is_same<value_type, class_data::value_type>::value,
                "we want ot return from here floating point value");
        const auto test_bin_classes = [this](auto&& res, const auto& ti) {
            boost::transform(ti, std::back_inserter(res), [this](auto row) {
                        cv::Mat tmp_data{1, (int)row.size(), CV_32F, (void*)row.origin()};
                        const auto p = this->handle->predict(tmp_data, cv::noArray(), cv::ml::StatModel::RAW_OUTPUT);
                        return predict_prob(p);
                    }
                );
            return res;
        };

        const auto test_any = [this] (auto&& res, const auto& ti) {
                    boost::transform(ti, std::back_inserter(res), [this](auto row) {
                        cv::Mat tmp_data{1, (int)row.size(), CV_32F, (void*)row.origin()};
                        cv::Mat ret;
                        this->handle->predict(tmp_data, ret,
                                        cv::ml::StatModel::RAW_OUTPUT);
                        return ret.at<float>(0, 0);;
                    }
                );
                return res;
        };

        class_data result;
        if (!empty(testInput)) {
            result.reserve(rows(testInput));
            if (classes == 2) {
                return test_bin_classes(result, testInput);
            } else {
                return test_any(result, testInput);
            }
        }
        return result;
    }

    void save(const char* path) const
    {
        handle->save(path);
    }

    static cv::Ptr<model_t> load(const char* path)
    {
        return cv::ml::StatModel::load<model_t>(path);
    }

    cv::Ptr<model_t> handle;
private:
    bool train(const training_data& samples,
            const class_data& lables, int stype,
            int ctype)
    {
        if (empty(samples) || lables.empty()) {
            return false;
        }
        auto data(prepare_samples(samples, stype));
        auto classes(prepare_labels(lables, ctype));
        cv::Ptr<cv::ml::TrainData> train_p =
            cv::ml::TrainData::create(data, cv::ml::SampleTypes::ROW_SAMPLE, classes);
        return handle->train(train_p);
    }
};

}   // end of namespace detail
}   // end of namespace clsasifier


/*
 * Copyright (C) 2012 Incognito
 *
 * 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 "grid.h"

#include "core.h"
#include "main.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/intrusive_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/variant.hpp>

#include <Eigen/Core>

#include <cmath>
#include <vector>

Grid::Grid()
{
	cellDistance = 360000.0f;
	cellSize = 300.0f;
	globalCell = SharedCell(new Cell());
	calculateTranslationMatrix();
}

void Grid::addArea(const Item::SharedArea &area)
{
	if (area->size > cellDistance)
	{
		globalCell->areas.insert(std::make_pair(area->areaID, area));
		area->cell.reset();
	}
	else
	{
		Eigen::Vector2f position = Eigen::Vector2f::Zero();
		switch (area->type)
		{
			case STREAMER_AREA_TYPE_CIRCLE:
			{
				if (area->attach)
				{
					position = Eigen::Vector2f(area->attach->position[0], area->attach->position[1]);
				}
				else
				{
					position = boost::get<Eigen::Vector2f>(area->position);
				}
				break;
			}
			case STREAMER_AREA_TYPE_RECTANGLE:
			{
				boost::geometry::centroid(boost::get<Box2D>(area->position), position);
				break;
			}
			case STREAMER_AREA_TYPE_SPHERE:
			{
				if (area->attach)
				{
					position = Eigen::Vector2f(area->attach->position[0], area->attach->position[1]);
				}
				else
				{
					position = Eigen::Vector2f(boost::get<Eigen::Vector3f>(area->position)[0], boost::get<Eigen::Vector3f>(area->position)[1]);
				}
				break;
			}
			case STREAMER_AREA_TYPE_CUBE:
			{
				Eigen::Vector3f centroid = boost::geometry::return_centroid<Eigen::Vector3f>(boost::get<Box3D>(area->position));
				position = Eigen::Vector2f(centroid[0], centroid[1]);
				break;
			}
			case STREAMER_AREA_TYPE_POLYGON:
			{
				boost::geometry::centroid(boost::get<Polygon2D>(area->position).get<0>(), position);
				break;
			}
		}
		CellID cellID = getCellID(position);
		cells[cellID]->areas.insert(std::make_pair(area->areaID, area));
		area->cell = cells[cellID];
	}
}

void Grid::addCheckpoint(const Item::SharedCheckpoint &checkpoint)
{
	if (checkpoint->streamDistance > cellDistance)
	{
		globalCell->checkpoints.insert(std::make_pair(checkpoint->checkpointID, checkpoint));
		checkpoint->cell.reset();
	}
	else
	{
		CellID cellID = getCellID(Eigen::Vector2f(checkpoint->position[0], checkpoint->position[1]));
		cells[cellID]->checkpoints.insert(std::make_pair(checkpoint->checkpointID, checkpoint));
		checkpoint->cell = cells[cellID];
	}
}

void Grid::addMapIcon(const Item::SharedMapIcon &mapIcon)
{
	if (mapIcon->streamDistance > cellDistance)
	{
		globalCell->mapIcons.insert(std::make_pair(mapIcon->mapIconID, mapIcon));
		mapIcon->cell.reset();
	}
	else
	{
		CellID cellID = getCellID(Eigen::Vector2f(mapIcon->position[0], mapIcon->position[1]));
		cells[cellID]->mapIcons.insert(std::make_pair(mapIcon->mapIconID, mapIcon));
		mapIcon->cell = cells[cellID];
	}
}

void Grid::addObject(const Item::SharedObject &object)
{
	if (object->streamDistance > cellDistance)
	{
		globalCell->objects.insert(std::make_pair(object->objectID, object));
		object->cell.reset();
	}
	else
	{
		Eigen::Vector2f position = Eigen::Vector2f::Zero();
		if (object->attach)
		{
			position = Eigen::Vector2f(object->attach->position[0], object->attach->position[1]);
		}
		else
		{
			position = Eigen::Vector2f(object->position[0], object->position[1]);
		}
		CellID cellID = getCellID(Eigen::Vector2f(position[0], position[1]));
		cells[cellID]->objects.insert(std::make_pair(object->objectID, object));
		object->cell = cells[cellID];
	}
}

void Grid::addPickup(const Item::SharedPickup &pickup)
{
	if (pickup->streamDistance > cellDistance)
	{
		globalCell->pickups.insert(std::make_pair(pickup->pickupID, pickup));
		pickup->cell.reset();
	}
	else
	{
		CellID cellID = getCellID(Eigen::Vector2f(pickup->position[0], pickup->position[1]));
		cells[cellID]->pickups.insert(std::make_pair(pickup->pickupID, pickup));
		pickup->cell = cells[cellID];
	}
}

void Grid::addRaceCheckpoint(const Item::SharedRaceCheckpoint &raceCheckpoint)
{
	if (raceCheckpoint->streamDistance > cellDistance)
	{
		globalCell->raceCheckpoints.insert(std::make_pair(raceCheckpoint->raceCheckpointID, raceCheckpoint));
		raceCheckpoint->cell.reset();
	}
	else
	{
		CellID cellID = getCellID(Eigen::Vector2f(raceCheckpoint->position[0], raceCheckpoint->position[1]));
		cells[cellID]->raceCheckpoints.insert(std::make_pair(raceCheckpoint->raceCheckpointID, raceCheckpoint));
		raceCheckpoint->cell = cells[cellID];
	}
}

void Grid::addTextLabel(const Item::SharedTextLabel &textLabel)
{
	if (textLabel->streamDistance > cellDistance)
	{
		globalCell->textLabels.insert(std::make_pair(textLabel->textLabelID, textLabel));
		textLabel->cell.reset();
	}
	else
	{
		Eigen::Vector2f position = Eigen::Vector2f::Zero();
		if (textLabel->attach)
		{
			position = Eigen::Vector2f(textLabel->attach->position[0], textLabel->attach->position[1]);
		}
		else
		{
			position = Eigen::Vector2f(textLabel->position[0], textLabel->position[1]);;
		}
		CellID cellID = getCellID(position);
		cells[cellID]->textLabels.insert(std::make_pair(textLabel->textLabelID, textLabel));
		textLabel->cell = cells[cellID];
	}
}

void Grid::rebuildGrid()
{
	cells.clear();
	globalCell = SharedCell(new Cell());
	calculateTranslationMatrix();
	for (boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a)
	{
		addArea(a->second);
	}
	for (boost::unordered_map<int, Item::SharedCheckpoint>::iterator c = core->getData()->checkpoints.begin(); c != core->getData()->checkpoints.end(); ++c)
	{
		addCheckpoint(c->second);
	}
	for (boost::unordered_map<int, Item::SharedMapIcon>::iterator m = core->getData()->mapIcons.begin(); m != core->getData()->mapIcons.end(); ++m)
	{
		addMapIcon(m->second);
	}
	for (boost::unordered_map<int, Item::SharedObject>::iterator o = core->getData()->objects.begin(); o != core->getData()->objects.end(); ++o)
	{
		addObject(o->second);
	}
	for (boost::unordered_map<int, Item::SharedPickup>::iterator p = core->getData()->pickups.begin(); p != core->getData()->pickups.end(); ++p)
	{
		addPickup(p->second);
	}
	for (boost::unordered_map<int, Item::SharedRaceCheckpoint>::iterator r = core->getData()->raceCheckpoints.begin(); r != core->getData()->raceCheckpoints.end(); ++r)
	{
		addRaceCheckpoint(r->second);
	}
	for (boost::unordered_map<int, Item::SharedTextLabel>::iterator t = core->getData()->textLabels.begin(); t != core->getData()->textLabels.end(); ++t)
	{
		addTextLabel(t->second);
	}
}

void Grid::removeAllItems(int type)
{
	boost::unordered_map<CellID, SharedCell> temporaryCells(cells);
	switch (type)
	{
		case STREAMER_TYPE_OBJECT:
		{
			globalCell->objects.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->objects.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_PICKUP:
		{
			globalCell->pickups.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->pickups.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_CP:
		{
			globalCell->checkpoints.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->checkpoints.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_RACE_CP:
		{
			globalCell->raceCheckpoints.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->raceCheckpoints.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_MAP_ICON:
		{
			globalCell->mapIcons.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->mapIcons.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_3D_TEXT_LABEL:
		{
			globalCell->textLabels.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->textLabels.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
		case STREAMER_TYPE_AREA:
		{
			globalCell->areas.clear();
			for (boost::unordered_map<CellID, SharedCell>::iterator c = temporaryCells.begin(); c != temporaryCells.end(); ++c)
			{
				c->second->areas.clear();
				eraseCellIfEmpty(c->second);
			}
			break;
		}
	}
}

void Grid::removeArea(const Item::SharedArea &area, bool reassign)
{
	bool found = false;
	if (area->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(area->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedArea>::iterator a = c->second->areas.find(area->areaID);
			if (a != c->second->areas.end())
			{
				c->second->areas.quick_erase(a);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedArea>::iterator a = globalCell->areas.find(area->areaID);
		if (a != globalCell->areas.end())
		{
			globalCell->areas.quick_erase(a);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addArea(area);
		}
		else
		{
			if (area->attach)
			{
				core->getStreamer()->attachedAreas.erase(area);
			}
		}
	}
}

void Grid::removeCheckpoint(const Item::SharedCheckpoint &checkpoint, bool reassign)
{
	bool found = false;
	if (checkpoint->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(checkpoint->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedCheckpoint>::iterator d = c->second->checkpoints.find(checkpoint->checkpointID);
			if (d != c->second->checkpoints.end())
			{
				c->second->checkpoints.quick_erase(d);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedCheckpoint>::iterator c = globalCell->checkpoints.find(checkpoint->checkpointID);
		if (c != globalCell->checkpoints.end())
		{
			globalCell->checkpoints.quick_erase(c);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addCheckpoint(checkpoint);
		}
	}
}

void Grid::removeMapIcon(const Item::SharedMapIcon &mapIcon, bool reassign)
{
	bool found = false;
	if (mapIcon->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(mapIcon->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedMapIcon>::iterator m = c->second->mapIcons.find(mapIcon->mapIconID);
			if (m != c->second->mapIcons.end())
			{
				c->second->mapIcons.quick_erase(m);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedMapIcon>::iterator m = globalCell->mapIcons.find(mapIcon->mapIconID);
		if (m != globalCell->mapIcons.end())
		{
			globalCell->mapIcons.quick_erase(m);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addMapIcon(mapIcon);
		}
	}
}

void Grid::removeObject(const Item::SharedObject &object, bool reassign)
{
	bool found = false;
	if (object->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(object->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedObject>::iterator o = c->second->objects.find(object->objectID);
			if (o != c->second->objects.end())
			{
				c->second->objects.quick_erase(o);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedObject>::iterator o = globalCell->objects.find(object->objectID);
		if (o != globalCell->objects.end())
		{
			globalCell->objects.quick_erase(o);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addObject(object);
		}
		else
		{
			if (object->attach)
			{
				core->getStreamer()->attachedObjects.erase(object);
			}
			if (object->move)
			{
				core->getStreamer()->movingObjects.erase(object);
			}
		}
	}
}

void Grid::removePickup(const Item::SharedPickup &pickup, bool reassign)
{
	bool found = false;
	if (pickup->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(pickup->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedPickup>::iterator p = c->second->pickups.find(pickup->pickupID);
			if (p != c->second->pickups.end())
			{
				c->second->pickups.quick_erase(p);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedPickup>::iterator p = globalCell->pickups.find(pickup->pickupID);
		if (p != globalCell->pickups.end())
		{
			globalCell->pickups.quick_erase(p);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addPickup(pickup);
		}
	}
}

void Grid::removeRaceCheckpoint(const Item::SharedRaceCheckpoint &raceCheckpoint, bool reassign)
{
	bool found = false;
	if (raceCheckpoint->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(raceCheckpoint->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedRaceCheckpoint>::iterator r = c->second->raceCheckpoints.find(raceCheckpoint->raceCheckpointID);
			if (r != c->second->raceCheckpoints.end())
			{
				c->second->raceCheckpoints.quick_erase(r);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedRaceCheckpoint>::iterator r = globalCell->raceCheckpoints.find(raceCheckpoint->raceCheckpointID);
		if (r != globalCell->raceCheckpoints.end())
		{
			globalCell->raceCheckpoints.quick_erase(r);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addRaceCheckpoint(raceCheckpoint);
		}
	}
}

void Grid::removeTextLabel(const Item::SharedTextLabel &textLabel, bool reassign)
{
	bool found = false;
	if (textLabel->cell)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(textLabel->cell->cellID);
		if (c != cells.end())
		{
			boost::unordered_map<int, Item::SharedTextLabel>::iterator t = c->second->textLabels.find(textLabel->textLabelID);
			if (t != c->second->textLabels.end())
			{
				c->second->textLabels.quick_erase(t);
				eraseCellIfEmpty(c->second);
				found = true;
			}
		}
	}
	else
	{
		boost::unordered_map<int, Item::SharedTextLabel>::iterator t = globalCell->textLabels.find(textLabel->textLabelID);
		if (t != globalCell->textLabels.end())
		{
			globalCell->textLabels.quick_erase(t);
			found = true;
		}
	}
	if (found)
	{
		if (reassign)
		{
			addTextLabel(textLabel);
		}
		else
		{
			if (textLabel->attach)
			{
				core->getStreamer()->attachedTextLabels.erase(textLabel);
			}
		}
	}
}

CellID Grid::getCellID(const Eigen::Vector2f &position, bool insert)
{
	static Box2D box;
	box.min_corner()[0] = std::floor((position[0] / cellSize)) * cellSize;
	box.min_corner()[1] = std::floor((position[1] / cellSize)) * cellSize;
	box.max_corner()[0] = box.min_corner()[0] + cellSize;
	box.max_corner()[1] = box.min_corner()[1] + cellSize;
	Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(box);
	CellID cellID = std::make_pair(static_cast<int>(centroid[0]), static_cast<int>(centroid[1]));
	if (insert)
	{
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(cellID);
		if (c == cells.end())
		{
			SharedCell cell(new Cell(cellID));
			cells[cellID] = cell;
		}
	}
	return cellID;
}

void Grid::processDiscoveredCells(Player &player, std::vector<SharedCell> &playerCells, const boost::unordered_set<CellID> &discoveredCells)
{
	playerCells.push_back(SharedCell(new Cell()));;
	if (player.enabledItems[STREAMER_TYPE_OBJECT])
	{
		boost::unordered_map<int, Item::SharedObject>::iterator o = player.visibleCell->objects.begin();
		while (o != player.visibleCell->objects.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(o->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				o = player.visibleCell->objects.erase(o);
			}
			else
			{
				++o;
			}
		}
		playerCells.back()->objects.swap(player.visibleCell->objects);
	}
	if (player.enabledItems[STREAMER_TYPE_CP])
	{
		boost::unordered_map<int, Item::SharedCheckpoint>::iterator c = player.visibleCell->checkpoints.begin();
		while (c != player.visibleCell->checkpoints.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(c->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				c = player.visibleCell->checkpoints.erase(c);
			}
			else
			{
				++c;
			}
		}
		playerCells.back()->checkpoints.swap(player.visibleCell->checkpoints);
	}
	if (player.enabledItems[STREAMER_TYPE_RACE_CP])
	{
		boost::unordered_map<int, Item::SharedRaceCheckpoint>::iterator t = player.visibleCell->raceCheckpoints.begin();
		while (t != player.visibleCell->raceCheckpoints.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(t->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				t = player.visibleCell->raceCheckpoints.erase(t);
			}
			else
			{
				++t;
			}
		}
		playerCells.back()->raceCheckpoints.swap(player.visibleCell->raceCheckpoints);
	}
	if (player.enabledItems[STREAMER_TYPE_MAP_ICON])
	{
		boost::unordered_map<int, Item::SharedMapIcon>::iterator m = player.visibleCell->mapIcons.begin();
		while (m != player.visibleCell->mapIcons.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(m->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				m = player.visibleCell->mapIcons.erase(m);
			}
			else
			{
				++m;
			}
		}
		playerCells.back()->mapIcons.swap(player.visibleCell->mapIcons);
	}
	if (player.enabledItems[STREAMER_TYPE_3D_TEXT_LABEL])
	{
		boost::unordered_map<int, Item::SharedTextLabel>::iterator t = player.visibleCell->textLabels.begin();
		while (t != player.visibleCell->textLabels.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(t->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				t = player.visibleCell->textLabels.erase(t);
			}
			else
			{
				++t;
			}
		}
		playerCells.back()->textLabels.swap(player.visibleCell->textLabels);
	}
	if (player.enabledItems[STREAMER_TYPE_AREA])
	{
		boost::unordered_map<int, Item::SharedArea>::iterator a = player.visibleCell->areas.begin();
		while (a != player.visibleCell->areas.end())
		{
			boost::unordered_set<CellID>::iterator d = discoveredCells.find(a->second->cell->cellID);
			if (d != discoveredCells.end())
			{
				a = player.visibleCell->areas.erase(a);
			}
			else
			{
				++a;
			}
		}
		playerCells.back()->areas.swap(player.visibleCell->areas);
	}
}

void Grid::findAllCells(Player &player, std::vector<SharedCell> &playerCells)
{
	boost::unordered_set<CellID> discoveredCells;
	for (int i = 0; i < translationMatrix.cols(); ++i)
	{
		Eigen::Vector2f position = Eigen::Vector2f(player.position[0], player.position[1]) + translationMatrix.col(i);
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(getCellID(position, false));
		if (c != cells.end())
		{
			discoveredCells.insert(c->first);
			playerCells.push_back(c->second);
		}
	}
	processDiscoveredCells(player, playerCells, discoveredCells);
	playerCells.push_back(globalCell);
}

void Grid::findMinimalCells(Player &player, std::vector<SharedCell> &playerCells)
{
	for (int i = 0; i < translationMatrix.cols(); ++i)
	{
		Eigen::Vector2f position = Eigen::Vector2f(player.position[0], player.position[1]) + translationMatrix.col(i);
		boost::unordered_map<CellID, SharedCell>::iterator c = cells.find(getCellID(position, false));
		if (c != cells.end())
		{
			playerCells.push_back(c->second);
		}
	}
	playerCells.push_back(globalCell);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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 BOOST_SIMD_META_REGISTER_COUNT_HPP_INCLUDED
#define BOOST_SIMD_META_REGISTER_COUNT_HPP_INCLUDED

#include <boost/mpl/size_t.hpp>
#include <boost/simd/sdk/simd/native_fwd.hpp>
#include <boost/simd/sdk/simd/pack/forward.hpp>
#include <boost/simd/sdk/simd/extensions.hpp>
#include <boost/type_traits/is_floating_point.hpp>

namespace boost { namespace simd { namespace meta
{
  /*!
    @brief SIMD register file size

    Return an @mplint containing the current architecture register file
    size for a given type.

    @par Models:

    @metafunction

    @tparam T Type to check
  **/
  template<typename T>
  struct  register_count
        : boost::mpl::size_t< boost::is_floating_point<T>::value
                            ? BOOST_SIMD_VR_COUNT
                            : BOOST_SIMD_GPR_COUNT
                            >
  {};

  template<typename T, typename X>
  struct  register_count< native<T,X> >
        : boost::mpl::size_t<BOOST_SIMD_VR_COUNT>
  {};

  template<typename T, std::size_t N>
  struct  register_count< pack<T,N> >
        : register_count< typename pack<T,N>::data_type >
  {};

  template<typename Expr, typename Result>
  struct  register_count< expression<Expr,Result> >
        : register_count<Result>
  {};
} } }

#endif


//============================================================================//
//-------------------- pnt_integrity/AquisitionCheck.hpp -------*- C++ -*-----//
//============================================================================//
// BSD 3-Clause License
//
// Copyright (C) 2019 Integrated Solutions for Systems, Inc
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
// may be used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//----------------------------------------------------------------------------//
//
//  Class defined for the acquisition level checks
//  Josh Clanton <josh.clanton@is4s.com>
//  September 30, 2019
//============================================================================//
#include "pnt_integrity/AcquisitionCheck.hpp"
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <thread>

// #include "gnsscommon/GNSSConstants.hpp"

using namespace if_data_utils;

namespace pnt_integrity
{
//==============================================================================
//---------------------------- acquisitionSetup() ------------------------------
//==============================================================================
void AcquisitionCheck::acquisitionSetup()
{
  samplesPerIntPeriod_ = samplingFrequency_ * integrationPeriod_;
  samplesPerCode_ =
    std::round(samplingFrequency_ / (codeFrequencyBasis_ / codeLength_));

  generateCaCodeMap();
  generateFreqBins();

  std::stringstream setupMsg;
  setupMsg << "AcquisitionCheck::acquisitionSetup: "
           << "samps per int period = " << samplesPerIntPeriod_
           << ", num freq bins = " << freqBins_.size();
  logMsg_(setupMsg.str(), logutils::LogLevel::Warn);

  logMsg_("AcquisitionCheck::acquisitionSetup(): Code replicas initialized",
          logutils::LogLevel::Info);

  replicasInitialized_ = true;
}

//==============================================================================
//---------------------------- generateFreqBins() ------------------------------
//==============================================================================
void AcquisitionCheck::generateFreqBins()
{
  // numFreqBins_ = std::round(acquisitionSearchBand_ * 2) + 1;
  for (float curFreq = (intermediateFrequency_ - acquisitionSearchBand_);
       curFreq <= (intermediateFrequency_ + acquisitionSearchBand_);
       curFreq += searchStepSize_)
  {
    freqBins_.push_back(curFreq);
  }
}

//==============================================================================
//---------------------------- generateCaCodeMap()------------------------------
//==============================================================================
void AcquisitionCheck::generateCaCodeMap()
{
  prnList_.clear();
  for (int ii = 1; ii <= 32; ++ii)
  {
    prnList_.push_back(ii);

    caCodeMap_.insert(CodeMapEntry(ii,
                                   upsampleCaCode(generateCaCode(ii, 0),
                                                  samplingFrequency_,
                                                  codeFrequencyBasis_,
                                                  samplesPerIntPeriod_)));

    std::vector<std::complex<float> > caCodeFD;
    fftEngine_.fwd(caCodeFD, caCodeMap_[ii]);

    // convert to Eigen VectorXcf and take conjugate
    Eigen::Map<Eigen::VectorXcf> caFD_map(&caCodeFD[0], caCodeFD.size());
    caCodeMapFD_.insert(CodeFreqMapEntry(ii, caFD_map.conjugate()));
  }
}

//==============================================================================
//------------------------------ generateCacode() ------------------------------
//==============================================================================
std::vector<float> AcquisitionCheck::generateCaCode(signed int   _prn,
                                                    unsigned int _chip_shift)
{
  // initialize vector to hold result
  std::vector<float> ca_code;

  unsigned int G1[1023];
  unsigned int G2[1023];
  unsigned int G1_register[10], G2_register[10];
  unsigned int feedback1, feedback2;
  unsigned int lcv, lcv2;
  unsigned int delay;
  signed int   prn = _prn - 1;  // Move the PRN code to fit an array indices

  // G2 Delays as defined in IS-GPS-200E
  signed int delays[32] = {5,   6,   7,   8,   17,  18,  139, 140,
                           141, 251, 252, 254, 255, 256, 257, 258,
                           469, 470, 471, 472, 473, 474, 509, 512,
                           513, 514, 515, 516, 859, 860, 861, 862};
  // PRN sequences 33 through 37 are reserved for other uses (e.g. ground
  // transmitters)

  // A simple error check
  if ((prn < 0) || (prn > 32))
    return ca_code;

  for (lcv = 0; lcv < 10; lcv++)
  {
    G1_register[lcv] = 1;
    G2_register[lcv] = 1;
  }

  // Generate G1 & G2 Register
  for (lcv = 0; lcv < 1023; lcv++)
  {
    G1[lcv] = G1_register[0];
    G2[lcv] = G2_register[0];

    feedback1 = G1_register[7] ^ G1_register[0];
    feedback2 = (G2_register[8] + G2_register[7] + G2_register[4] +
                 G2_register[2] + G2_register[1] + G2_register[0]) &
                0x1;

    for (lcv2 = 0; lcv2 < 9; lcv2++)
    {
      G1_register[lcv2] = G1_register[lcv2 + 1];
      G2_register[lcv2] = G2_register[lcv2 + 1];
    }

    G1_register[9] = feedback1;
    G2_register[9] = feedback2;
  }

  // Set the delay
  delay = 1023 - delays[prn];
  delay += _chip_shift;
  delay %= 1023;
  // Generate PRN from G1 and G2 Registers
  for (lcv = 0; lcv < 1023; lcv++)
  {
    ca_code.push_back(G1[(lcv + _chip_shift) % 1023] ^ G2[delay]);
    if (ca_code[lcv] == 0.0)  // javi
    {
      ca_code[lcv] = -1.0;
    }
    delay++;
    delay %= 1023;
  }

  return ca_code;
}

//==============================================================================
//------------------------------ upsampleCaCode() ------------------------------
//==============================================================================
std::vector<float> AcquisitionCheck::upsampleCaCode(std::vector<float> codes,
                                                    double sampleFrequency,
                                                    double codeFrequency,
                                                    size_t samples,
                                                    double chip_shift)
{
  double codePhaseStep    = codeFrequency / sampleFrequency;
  int    samples_per_code = sampleFrequency / 1000;  // code is 1ms

  // if samples argument is set to 0, generate 1 ms of samples
  if (samples == 0)
    samples = samples_per_code;

  std::vector<float> digitized;

  // generate array that counts from 0 to 1023 and
  // has the same # of values as 1 period of the
  // sampled CA code

  int ca_idx = 0;
  for (size_t idx = 0; idx < samples; idx++)
  {
    // calculate C/A code index
    // add 1 to index to make it exactly match matlab code
    ca_idx = ceil((codePhaseStep * (idx + 1)) + chip_shift) - 1;
    // ca_idx = ((codePhaseStep * (idx)) + chip_shift) ;

    // wrap index into range of codes vector
    ca_idx = (ca_idx + 1023) % 1023;

    digitized.push_back(codes[ca_idx]);
  }

  return digitized;
}

//==============================================================================
//------------------------------ generateCarrier() -----------------------------
//==============================================================================
double AcquisitionCheck::generateCarrier(double           initPhase,
                                         double           phaseStep,
                                         size_t           length,
                                         Eigen::VectorXf& sine,
                                         Eigen::VectorXf& cosine)
{
  Eigen::VectorXf CarrArg(length);
  // TOOD: remove this if statement and test
  if (length == 1)
  {
    CarrArg(0) = initPhase + phaseStep;
  }
  else
  {
    CarrArg = Eigen::VectorXf::LinSpaced(
      length, initPhase + phaseStep, initPhase + phaseStep * (length));
  }

  sine   = CarrArg.array().sin();
  cosine = CarrArg.array().cos();

  return CarrArg(length - 1);
}

//==============================================================================
//--------------------------------- sinFast() ----------------------------------
//==============================================================================
float AcquisitionCheck::sinFast(float x)
{
  x = fmod(x + M_PI, M_PI * 2) - M_PI;  // restrict x so that -pi<x<pi

  if (x < 0)
  {
    return 1.27323954f * x + 0.405284735f * x * x;
  }
  else
  {
    return 1.27323954f * x - 0.405284735f * x * x;
  }
}

//==============================================================================
//------------------------- generateAcquisitionPlane() -------------------------
//==============================================================================
bool AcquisitionCheck::generateAcquisitionPlane(
  const Eigen::ArrayXcf& signalSamples)
{
  auto start = std::chrono::high_resolution_clock::now();

  // make sure CA tables have been initialized and settings set
  // before attempting acquisition
  if (!replicasInitialized_)
  {
    logMsg_("CA code replicas must be initialized before acquiring.",
            logutils::LogLevel::Error);
    return false;
  }

  // TODO: check that samples size and ca replica size matches
  size_t numSamples = signalSamples.size();

  // generate time vector for carrier replica
  Eigen::VectorXcf phasePoints(numSamples);
  for (size_t ii = 0; ii < numSamples; ii++)
  {
    phasePoints[ii] = twoGpsPi_ * (double)ii / samplingFrequency_;
  }

  // define PRN to search for
  std::vector<std::thread> fftThreads;

  Eigen::ArrayXXf results(freqBins_.size(), numSamples);

  for (PrnList::iterator prnIt = prnList_.begin(); prnIt != prnList_.end();
       ++prnIt)
  {
    // make sure PRN is between 1 and 32
    if ((*prnIt < 1) || (*prnIt > 32))
    {
      logMsg_("PRN must be between 1 and 32.", logutils::LogLevel::Error);
      return false;
    }
    // add the prn entry to the results map
    correlationResultsMap_.insert(
      std::pair<int, Eigen::ArrayXXf>(*prnIt, results));

    fftThreads.push_back(
      std::thread(std::bind(&AcquisitionCheck::acquisitionCorrelation,
                            this,
                            *prnIt,
                            signalSamples,
                            phasePoints)));
  }

  // close the FFT worker threads
  // size_t threadCount = 1;
  for (auto threadIt = fftThreads.begin(); threadIt != fftThreads.end();
       ++threadIt)
  {
    (*threadIt).join();
  }

  // publish the correlation data

  auto finish = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elapsed = finish - start;
  std::stringstream             timeMsg;
  timeMsg << "Elapsed time: " << elapsed.count() << " s";
  logMsg_(timeMsg.str(), logutils::LogLevel::Debug);

  if (publishAquisitionData_)
  {
    publishAquisitionData_(correlationResultsMap_);
  }
  if (publishPeakData_)
  {
    publishPeakData_(lastProcessTime_, peakResultsMap_);
  }

  return true;
}

//==============================================================================
//-------------------------- acquisitionCorrelation ----------------------------
//==============================================================================
void AcquisitionCheck::acquisitionCorrelation(
  const int&              prn,
  const Eigen::ArrayXcf&  signalSamples,
  const Eigen::VectorXcf& phasePoints)
{
  // initialize fft engine
  Eigen::FFT<float> fftEngine;

  size_t numSamples = signalSamples.size();

  std::vector<std::complex<float> > inputSignalDemod(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       inputSignalDemodMap(&inputSignalDemod[0],
                                                  inputSignalDemod.size());

  std::vector<std::complex<float> > signalFft(numSamples);
  Eigen::Map<Eigen::ArrayXcf> signalFftMap(&signalFft[0], signalFft.size());

  std::vector<std::complex<float> > corrFreqDom(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       corrFreqDomMap(&corrFreqDom[0],
                                             corrFreqDom.size());

  std::vector<std::complex<float> > corrTimeDom(numSamples);
  Eigen::Map<Eigen::ArrayXcf>       corrTimeDomMap(&corrTimeDom[0],
                                             corrTimeDom.size());

  // initialize arrays for peak
  auto samplesPerCodeChip =
    std::round(samplingFrequency_ / codeFrequencyBasis_);

  // set variables to save peak and it's location
  float                  peakValue      = 0.0;
  size_t                 peakFreqBinIdx = 0;
  Eigen::VectorXf::Index peakCodeIdx    = 0;

  // initialize array to hold results of acqusition tests
  size_t curBin = 0;
  for (auto freqIt = freqBins_.begin(); freqIt != freqBins_.end(); ++freqIt)
  {
    inputSignalDemodMap = (intermediateFrequency_ + *freqIt) * phasePoints *
                          std::complex<float>(0, 1);

    inputSignalDemodMap = inputSignalDemodMap.exp() * signalSamples;
    fftEngine.fwd(signalFft, inputSignalDemod);

    // multiply the complex conjugate of the CA replica with the demodulated
    // signal to get correlation in the frequency domain
    Eigen::Map<Eigen::ArrayXcf> caFftConj(&caCodeMapFD_[prn][0],
                                          caCodeMapFD_[prn].size());

    corrFreqDomMap = caFftConj * signalFftMap;

    fftEngine.inv(corrTimeDom, corrFreqDom);

    auto binResult                          = corrTimeDomMap.abs2();
    correlationResultsMap_[prn].row(curBin) = binResult;
    // resultsPtr->row(curBin)                 = binResult;

    // find the peak in this bin and the corresponding code idx
    Eigen::VectorXf::Index peakInBinCodeIdx;
    auto                   peakInBin = binResult.maxCoeff(&peakInBinCodeIdx);
    // if the bin peak is bigger than the previous, save it
    if (peakInBin > peakValue)
    {
      peakValue      = peakInBin;
      peakFreqBinIdx = curBin;
      peakCodeIdx    = peakInBinCodeIdx;
    }

    curBin++;
  }
  // define the exclusion zone around the peak
  auto excludeRangeLowIdx  = peakCodeIdx - samplesPerCodeChip;
  auto excludeRangeHighIdx = peakCodeIdx + samplesPerCodeChip;

  // pull out the frequency bin that has the max peak
  auto freqBinWithPeak = correlationResultsMap_[prn].row(peakFreqBinIdx);

  float secondPeakValue = 0.0;
  // auto secondPeakCodeIdx = 0;
  // find the second peak in this freqBin w.r.t. the exculsion zone
  for (auto codeIdx = 0; codeIdx < freqBinWithPeak.size(); codeIdx++)
  {
    // test for max everywehre but exclusion zone
    if ((codeIdx <= excludeRangeLowIdx) or (codeIdx >= excludeRangeHighIdx))
    {
      if (freqBinWithPeak(codeIdx) > secondPeakValue)
      {
        // secondPeakCodeIdx = codeIdx;
        secondPeakValue = freqBinWithPeak(codeIdx);
      }
    }
  }

  peakResultsMap_[prn] = std::pair<double, double>(peakValue, secondPeakValue);
}

//==============================================================================
//-------------------------------- runCheck ------------------------------------
//==============================================================================
bool AcquisitionCheck::runCheck()
{
  setPrnAssuranceLevels();
  calculateAssuranceLevel(lastProcessTime_);
  return true;
}

//==============================================================================
//-------------------------- setPrnAssuranceLevels ----------------------------
//==============================================================================
void AcquisitionCheck::setPrnAssuranceLevels()
{
  std::map<int, double> ratioMap;
  // look at the peak results map and make determinations bas
  for (auto prnIt = peakResultsMap_.begin(); prnIt != peakResultsMap_.end();
       ++prnIt)
  {
    double peakRatio       = (prnIt->second.first / prnIt->second.second);
    ratioMap[prnIt->first] = peakRatio;

    if (prnIt->second.first > highPowerThreshold_)
    {
      // power level is suspect, check ratio of 1st and 2nd peak

      if (peakRatio > peakRatioThreshold_)
      {
        prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Unassured;
      }
      else
      {
        prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Inconsistent;
      }
    }
    // check the first peak against the threshold
    else if (prnIt->second.first > acquisitionThreshold_)
    {
      prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Assured;
    }
    // PRN is not visible
    else
    {
      prnAssuranceLevels_[prnIt->first] = data::AssuranceLevel::Unavailable;
    }
  }

  if (publishDiagnostics_)
  {
    diagnostics_.ratioMap = ratioMap;
  }
}

//==============================================================================
//---------------------------- setAssuranceLevel -------------------------------
//==============================================================================
void AcquisitionCheck::calculateAssuranceLevel(const double& checkTime)
{
  // go throuh the prn assurance map and determine how many have been flagged
  // to set the overall level
  int assuredCount      = 0;
  int unavailableCount  = 0;
  int unassuredCount    = 0;
  int inconsistentCount = 0;
  for (auto it = prnAssuranceLevels_.begin(); it != prnAssuranceLevels_.end();
       ++it)
  {
    if (it->second == data::AssuranceLevel::Unassured)
    {
      unassuredCount++;
    }
    else if (it->second == data::AssuranceLevel::Inconsistent)
    {
      inconsistentCount++;
    }
    else if (it->second == data::AssuranceLevel::Assured)
    {
      assuredCount++;
    }
    else if (it->second == data::AssuranceLevel::Unavailable)
    {
      unavailableCount++;
    }

    if (unassuredCount >= assuranceUnassuredThresh_)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Unassured);
    }
    else if (inconsistentCount >= assuranceInconsistentThresh_)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Inconsistent);
    }
    else if (assuredCount >= 4)
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Assured);
    }
    else
    {
      changeAssuranceLevel(checkTime, data::AssuranceLevel::Unavailable);
    }

  }  // end for loop

  if (publishDiagnostics_)
  {
    diagnostics_.highPowerThresh    = highPowerThreshold_;
    diagnostics_.peakRatioThresh    = peakRatioThreshold_;
    diagnostics_.acquisitionThresh  = acquisitionThreshold_;
    diagnostics_.inconsistentThresh = assuranceInconsistentThresh_;
    diagnostics_.unassuredThresh    = assuranceUnassuredThresh_;
    diagnostics_.unassuredCount     = unassuredCount;
    diagnostics_.inconsistentCount  = inconsistentCount;

    publishDiagnostics_(lastProcessTime_, diagnostics_);
  }
  // std::stringstream levelMsg;
  // levelMsg << std::endl;
  // levelMsg << "Unusable : " << unassuredCount << " prns " << std::endl;
  // levelMsg << "Unknown : " << inconsistentCount << " prns " << std::endl;
  // levelMsg << "Assured : " << assuredCount << " prns " << std::endl;
  // levelMsg << "Unavailable : " << unavailableCount << " prns ";
  // logMsg_(levelMsg.str(), "info");

  // std::stringstream msg;
  // msg << "AcquisitionCheck::calculateAssuranceLevel() : Calculated level : "
  //     << (int)assuranceLevel_;
  // logMsg_(msg.str(), "info");
}

}  // namespace pnt_integrity