// ======================================================================
/*!
 * \file NFmiArea.cpp
 * \brief Implementation of class NFmiArea
 */
// ======================================================================
/*!
 * \class NFmiArea
 *
 * Undocumented
 */
// ======================================================================

#include "NFmiArea.h"
#include "NFmiAreaFactory.h"
#include <boost/functional/hash.hpp>

// ----------------------------------------------------------------------
/*!
 * \bug Turha argumentti konstruktorille
 */
// ----------------------------------------------------------------------

void NFmiArea::Init(bool /* fKeepWorldRect */) { CheckForPacificView(); }
// ----------------------------------------------------------------------
/*!
 * \param newArea Undocumented
 */
// ----------------------------------------------------------------------

void NFmiArea::SetXYArea(const NFmiRect &newArea)
{
  Place(newArea.TopLeft());
  Size(newArea.Size());
  Init();
}

static void FixPacificLongitude(NFmiPoint &lonLat)
{
  if (lonLat.X() < 0)
  {
    NFmiLongitude lon(lonLat.X(), true);
    lonLat.X(lon.Value());
  }
}

#if 0
static void FixAtlanticLongitude(NFmiPoint &lonLat)
{
    if(lonLat.X() > 180)
    {
        NFmiLongitude lon(lonLat.X(), false);
        lonLat.X(lon.Value());
    }
}
#endif

// ----------------------------------------------------------------------
/*!
 * \param theArea Undocumented
 */
// ----------------------------------------------------------------------

const NFmiRect NFmiArea::XYArea(const NFmiArea *theArea) const
{
  if (PacificView() && theArea->PacificView() == false)
  {
    NFmiPoint topLeftLatlon = theArea->ToLatLon(theArea->TopLeft());
    ::FixPacificLongitude(topLeftLatlon);
    NFmiPoint bottomRightLatlon = theArea->ToLatLon(theArea->BottomRight());
    ::FixPacificLongitude(bottomRightLatlon);

    NFmiPoint topLeft(ToXY(topLeftLatlon));
    NFmiPoint bottomRight(ToXY(bottomRightLatlon));
    NFmiRect rect(topLeft, bottomRight);
    return rect;
  }
  else if (PacificView() == false && theArea->PacificView())
  {
    std::unique_ptr<NFmiArea> pacificAreaFromThis(DoForcePacificFix());

    NFmiPoint topLeft(pacificAreaFromThis->ToXY(theArea->ToLatLon(theArea->TopLeft())));
    NFmiPoint bottomRight(pacificAreaFromThis->ToXY(theArea->ToLatLon(theArea->BottomRight())));
    NFmiRect rect(topLeft, bottomRight);
    return rect;
  }
  else
  {
    NFmiPoint topLeft(ToXY(theArea->ToLatLon(theArea->TopLeft())));
    NFmiPoint bottomRight(ToXY(theArea->ToLatLon(theArea->BottomRight())));
    NFmiRect rect(topLeft, bottomRight);
    return rect;
  }
}

// ----------------------------------------------------------------------
/*!
 * \param file Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

std::ostream &NFmiArea::Write(std::ostream &file) const
{
  file << itsXYRectArea;
  return file;
}

// ----------------------------------------------------------------------
/*!
 * \param file Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

std::istream &NFmiArea::Read(std::istream &file)
{
  file >> itsXYRectArea;
  return file;
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiArea::WorldXYSize() const { return WorldRect().Size(); }
// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiArea::WorldXYPlace() const { return WorldRect().Place(); }
// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiArea::WorldXYWidth() const { return WorldRect().Width(); }
// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiArea::WorldXYHeight() const { return WorldRect().Height(); }
// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiArea::WorldXYAspectRatio() const { return WorldXYWidth() / WorldXYHeight(); }
// ----------------------------------------------------------------------
/*!
 * Creates a new area from the current one by altering the corner points only.
 * Previously this method required the new area to be inside the original
 * one, but I failed to see the point in restricing the functionality - Mika
 *
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 *
 * \todo Should return an boost::shared_ptr
 * \todo Remove the unnecessary cast in the last return statement
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiArea::CreateNewArea(const NFmiPoint &theBottomLeftLatLon,
                                  const NFmiPoint &theTopRightLatLon) const
{
  return NewArea(theBottomLeftLatLon, theTopRightLatLon);
}

// ----------------------------------------------------------------------
/*!
 *
 * Creates a new sub-area inside the current "mother" area.
 * The new area is defined by the input local rectangle 'theRect'
 *
 * \note
 *   - the input rectangle defines a local XY coordinate area only, NOT the metric
 *     XY world rectangle
 *   - the sub-area MUST fit completely inside the current local area
 *   - the sub-area gets all the projection-specific properties but its dimensions
 *     from its "mother" area. For example, the orientation stays the same.
 *
 * \param theRect Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiArea::CreateNewArea(const NFmiRect &theRect) const
{
  NFmiPoint newBottomLeftXY = theRect.BottomLeft();
  NFmiPoint newTopRightXY = theRect.TopRight();

  NFmiPoint newBottomLeftLatLon = ToLatLon(newBottomLeftXY);
  NFmiPoint newTopRightLatLon = ToLatLon(newTopRightXY);

  NFmiArea *newArea = NewArea(newBottomLeftLatLon, newTopRightLatLon);
  return newArea;
}

// ----------------------------------------------------------------------
/*!
 * Creates a new area with its aspect ratio defined by the input parameters
 *
 * \note
 *    - the new area MAY OR MAY NOT completely reside inside the current local area
 *    - the new area gets all the projection-specific properties but its dimensions
 *      from its "mother" area. For example, the orientation stays the same.
 *
 * \param theNewAspectRatioXperY Undocumented
 * \param theFixedPoint Undocumented
 * \param fShrinkArea Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiArea::CreateNewArea(double theNewAspectRatioXperY,
                                  FmiDirection theFixedPoint,
                                  bool fShrinkArea)
{
  double originalAspectRatio = WorldXYAspectRatio();

  bool keepWidth;

  if (fShrinkArea)
  {
    // The new area will be "shrunk" to completely fit inside the current area
    if ((theNewAspectRatioXperY < originalAspectRatio))
      keepWidth = false;  // Maintain height, compute width
    else
      keepWidth = true;  // Maintain width, compute height
  }
  else
  {
    // The new area will in part grow out of the current area
    if ((theNewAspectRatioXperY < originalAspectRatio))
      keepWidth = true;  // Maintain width, compute height
    else
      keepWidth = false;  // Maintain height, compute width
  }

  // Create copy of the original "world rectangle" to be freely modified
  NFmiRect newWorldRect = WorldRect();

  // REDIMENSIONING OF THE WORLD RECTANGLE
  //----------------------------------------

  if (!newWorldRect.AdjustAspectRatio(theNewAspectRatioXperY, keepWidth, theFixedPoint))
    return nullptr;

  // Create a new area with the new aspect ratio
  NFmiArea *newArea =
      NewArea(WorldXYToLatLon(newWorldRect.TopLeft()), WorldXYToLatLon(newWorldRect.BottomRight()));

  // Return the re-dimensioned copy of the original area
  return newArea;
}

// ----------------------------------------------------------------------
/*!
 * Creates a new sub-area inside the current "mother" area.
 * The new area is defined by the input metric rectangle 'theWorldRect'
 *
 * \note
 *   - the input rectangle defines a metric XY world rectangle
 *   - the sub-area MUST fit completely inside the current local area
 *   - the sub-area gets all the projection-specific properties but its dimensions
 *   - from its "mother" area. For example, the orientation stays the same.
 *
 * \param theWorldRect
 * \return Undocumented
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiArea::CreateNewAreaByWorldRect(const NFmiRect &theWorldRect)
{
  NFmiPoint newBottomLeftXY = theWorldRect.BottomLeft();
  NFmiPoint newTopRightXY = theWorldRect.TopRight();

  NFmiPoint newBottomLeftLatLon = WorldXYToLatLon(newBottomLeftXY);
  NFmiPoint newTopRightLatLon = WorldXYToLatLon(newTopRightXY);

  if (!IsInside(newBottomLeftLatLon) || !IsInside(newTopRightLatLon)) return nullptr;

  auto *newArea = static_cast<NFmiArea *>(NewArea(newBottomLeftLatLon, newTopRightLatLon));

  if (!IsInside(*newArea)) return nullptr;

  return newArea;
}

// ----------------------------------------------------------------------
/*!
 * Measured from the input point, returns the azimuth angle between
 * the true geodetic north direction and the "up" Y-axis of the map
 * area rectangle. Azimuth angle runs 0..360 degrees clockwise, with
 * north zero degrees.
 *
 * \param theLatLonPoint Undocumented
 * \param theLatitudeEpsilon Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiAngle NFmiArea::TrueNorthAzimuth(const NFmiPoint &theLatLonPoint,
                                           double theLatitudeEpsilon) const
{
  using namespace std;

  NFmiPoint xyWorldPoint = LatLonToWorldXY(theLatLonPoint);
  NFmiPoint latLonIncr =
      NFmiPoint(0., theLatitudeEpsilon);  // Arbitrary small latitude increment in degrees

  // Move up toward geo-north along the meridian of the input point
  NFmiPoint xyDistanceAlongMeridian = LatLonToWorldXY(theLatLonPoint + latLonIncr) - xyWorldPoint;

  // Get the angle between 'xyDistanceAlongMeridian.X()' and map "up" direction Y-axis
  if (xyDistanceAlongMeridian.Y() == 0.)
    return xyDistanceAlongMeridian.X() > 0.
               ? NFmiAngle(90.)
               : NFmiAngle(
                     270.);  // Azimuth is exactly east 90 degrees or west 270 degrees, respectively

  return NFmiAngle(FmiDeg(atan2(xyDistanceAlongMeridian.X(), xyDistanceAlongMeridian.Y())));
}

// ----------------------------------------------------------------------
/*!
 * \brief Return center latlon coordinate
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiArea::CenterLatLon() const
{
  NFmiPoint bl = BottomLeft();
  NFmiPoint tr = TopRight();

  NFmiPoint center(0.5 * (bl.X() + tr.X()), 0.5 * (bl.Y() + tr.Y()));

  return ToLatLon(center);
}

bool NFmiArea::IsPacificLongitude(double theLongitude)
{
  if (theLongitude > 180 && theLongitude <= 360)
    return true;
  else
    return false;
}

void NFmiArea::CheckForPacificView()
{
  fPacificView = NFmiArea::IsPacificView(BottomLeftLatLon(), TopRightLatLon());
}

// 1. Tarkistaa kattaako annetut pisteet Atlantic vai Pacific alueen
// 2. Jos Pacific, korjaa longitudet pacific:eiksi
// 3. Palauta originaali/korjatut pisteet pair:issa
PacificPointFixerData NFmiArea::PacificPointFixer(const NFmiPoint &theBottomLeftLatlon,
                                                  const NFmiPoint &theTopRightLatlon)
{
  bool usePacificView = NFmiArea::IsPacificView(theBottomLeftLatlon, theTopRightLatlon);
  if (usePacificView)
  {
    NFmiPoint bottomLeftLatLon = theBottomLeftLatlon;
    NFmiPoint topRightLatLon = theTopRightLatlon;
    NFmiAreaFactory::DoPossiblePacificFix(bottomLeftLatLon, topRightLatLon, usePacificView);
    return PacificPointFixerData(bottomLeftLatLon, topRightLatLon, usePacificView);
  }
  else
    return PacificPointFixerData(theBottomLeftLatlon, theTopRightLatlon, usePacificView);
}

bool NFmiArea::IsPacificView(const NFmiPoint &bottomleftLatlon, const NFmiPoint &toprightLatlon)
{
  // Obvious case
  if (bottomleftLatlon.X() >= 0 && toprightLatlon.X() < 0) return true;
  // 0...360 coordinate system is used
  if (IsPacificLongitude(bottomleftLatlon.X()) || IsPacificLongitude(toprightLatlon.X()))
    return true;
  return false;
}

double NFmiArea::FixLongitude(double theLon) const
{
  if (!fPacificView)
  {
    if (theLon > 180.00000001)  // Tämä ei voi olla tasan 180, koska SmartMet maailman rajaviivat
                                // shapessa (maps\\shapes\\ne_10m_admin_0_countries) ja niiden
                                // piirto imagine-kirjastolla menee jossain kohdissa sekaisin
      // reunoilla, koska siellä on käytetty vähän yli 180-pituuspiirin
      // meneviä arvoja Tyynenmeren 180 asteen pituuspiirin reunoilla
      return theLon - 360;
    else
      return theLon;
  }
  else if (theLon < 0)
    return theLon + 360;
  else
    return theLon;
}

NFmiArea *NFmiArea::DoPossiblePacificFix() const
{
  // On olemassa pari erikoistapausta, mitkä halutaan eri areoissa korjata, että alueet toimisivat
  // paremmin newbase:ssa.
  if (fPacificView)
  {
    bool usedPacificViewState = fPacificView;
    NFmiPoint bottomleft = BottomLeftLatLon();
    NFmiPoint topright = TopRightLatLon();
    bool createNewArea =
        NFmiAreaFactory::DoPossiblePacificFix(bottomleft, topright, usedPacificViewState);

    if (createNewArea)
    {
      NFmiArea *newArea = NewArea(bottomleft, topright);
      if (newArea)
      {
        newArea->PacificView(usedPacificViewState);
        return newArea;
      }
    }
  }
  return nullptr;
}

NFmiArea *NFmiArea::DoForcePacificFix() const
{
  // Joskus on pakko muuttaa atlantic-area pacific tyyppiseksi vaikka väkisin
  if (!fPacificView)
  {
    NFmiPoint bottomleftLatlon = BottomLeftLatLon();
    ::FixPacificLongitude(bottomleftLatlon);
    NFmiPoint toprightLatlon = TopRightLatLon();
    ::FixPacificLongitude(toprightLatlon);

    NFmiArea *newArea = NewArea(bottomleftLatlon, toprightLatlon, false);
    if (newArea)
    {
      newArea->PacificView(true);
      return newArea;
    }
  }
  return nullptr;
}

// ----------------------------------------------------------------------
/*!
 * \brief Return hash value for the base NFmiArea components
 */
// ----------------------------------------------------------------------

std::size_t NFmiArea::HashValue() const
{
  std::size_t hash = itsXYRectArea.HashValue();
  boost::hash_combine(hash, boost::hash_value(fPacificView));
  boost::hash_combine(hash, boost::hash_value(WKT()));
  return hash;
}


#pragma once
#include <geometry_msgs/Point.h>
#include <mil_blueview_driver/BlueViewPing.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>

#include <tf/transform_listener.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_msgs/TFMessage.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>

#include <opencv2/core/core.hpp>
#include <stdexcept>
#include "opencv2/opencv.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

#include <boost/circular_buffer.hpp>

#include <sub8_msgs/Bounds.h>

#include <waypoint_validity.hpp>

#include <Classification.hpp>

class OGridGen
{
public:
  OGridGen();
  void publish_ogrid(const ros::TimerEvent &);

  void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg);

private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_to_imaging_sonar_;

  tf::TransformListener listener_;

  ros::Publisher pub_grid_;
  ros::Publisher pub_point_cloud_filtered_;
  ros::Publisher pub_point_cloud_raw_;
  ros::Timer timer_;

  cv::Mat mat_ogrid_;
  float ogrid_size_;
  float resolution_;
  float pool_depth_;
  int min_intensity_;

  ros::ServiceClient service_get_bounds_;
  tf::StampedTransform transform_;

  boost::circular_buffer<pcl::PointXYZI> point_cloud_buffer_;

  std::vector<cv::Point> bounds_;

  Classification classification_;
};


#define BOOST_TEST_MODULE SignatureTest
#include <boost/test/unit_test.hpp>

#include <fc/crypto/elliptic.hpp>
#include <fc/crypto/sha256.hpp>
#include <fc/time.hpp>
#include <fc/thread/thread.hpp>
#include <iostream>
#include <algorithm>
#include <fc/crypto/sha512.hpp>

#include <bts/utilities/deterministic_openssl_rand.hpp>

BOOST_AUTO_TEST_CASE(ecc_signatures)
{
  std::string plaintext("Here is some text to sign");
  fc::sha256::encoder sha_encoder;
  sha_encoder.write(plaintext.c_str(), plaintext.size());
  fc::sha256 hash_of_plaintext = sha_encoder.result();

  // code for generating a key:
  //fc::ecc::private_key signing_key = fc::ecc::private_key::generate();
  //std::string private_key_as_string = fc::variant(signing_key).as_string();
  //fc::ecc::private_key reconstructed_private_key = fc::variant(private_key_as_string).as<fc::ecc::private_key>();
  //assert(signing_key == reconstructed_private_key);
  fc::ecc::private_key signing_key = fc::variant("940ff36f9c6d4d19d8c965bfbbe50bd318774a2af3d8c76021e681af462f943e").as<fc::ecc::private_key>();

  // generate two signatures before injecting our code into openssl
  fc::ecc::signature first_nondeterministic_signature = signing_key.sign(hash_of_plaintext);
  fc::ecc::signature second_nondeterministic_signature = signing_key.sign(hash_of_plaintext);
  BOOST_REQUIRE(first_nondeterministic_signature != second_nondeterministic_signature);

  // set our fake random number generator, generate two signatures
  bts::utilities::set_random_seed_for_testing(fc::sha512());

  fc::ecc::signature first_deterministic_signature = signing_key.sign(hash_of_plaintext);
  fc::ecc::signature second_deterministic_signature = signing_key.sign(hash_of_plaintext);
  BOOST_REQUIRE(first_deterministic_signature != second_deterministic_signature);
  BOOST_REQUIRE(first_deterministic_signature != first_nondeterministic_signature);
  BOOST_REQUIRE(second_deterministic_signature != second_nondeterministic_signature);

  bts::utilities::set_random_seed_for_testing(fc::sha512());
  fc::ecc::signature third_deterministic_signature = signing_key.sign(hash_of_plaintext);
  fc::ecc::signature fourth_deterministic_signature = signing_key.sign(hash_of_plaintext);
  BOOST_REQUIRE(third_deterministic_signature == first_deterministic_signature);
  BOOST_REQUIRE(fourth_deterministic_signature == second_deterministic_signature);
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2020.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Eugen Netz $
// $Authors: Eugen Netz $
// --------------------------------------------------------------------------

#include <OpenMS/ANALYSIS/XLMS/XQuestScores.h>
#include <OpenMS/MATH/STATISTICS/StatisticFunctions.h>
#include <boost/math/distributions/binomial.hpp>
#include <numeric>

using namespace std;

namespace OpenMS
{

  float XQuestScores::preScore(Size matched_alpha, Size ions_alpha, Size matched_beta, Size ions_beta)
  {

    if ( (matched_alpha <= 0 && matched_beta <= 0) || ions_alpha <= 0 || ions_beta <= 0)
    {
      return 0.0;
    }

    // avoid 0 values in multiplication, adds a "dynamic range" among candidates with no matching linear peaks to one of the peptides
    float matched_alpha_float = matched_alpha;
    if (matched_alpha <= 0)
    {
      matched_alpha_float = 0.1f;
    }
    float matched_beta_float = matched_beta;
    if (matched_beta <= 0)
    {
      matched_beta_float = 0.1f;
    }

      float result = sqrt((static_cast<float>(matched_alpha_float) / static_cast<float>(ions_alpha)) * (static_cast<float>(matched_beta_float) / static_cast<float>(ions_beta)));
      return result;
  }

  float XQuestScores::preScore(Size matched_alpha, Size ions_alpha)
  {
    if (ions_alpha <= 0)
    {
      return 0.0;
    }

    float result = static_cast<float>(matched_alpha) / static_cast<float>(ions_alpha);
    return result;
  }

  double XQuestScores::matchOddsScore(const PeakSpectrum& theoretical_spec,  const Size matched_size, double fragment_mass_tolerance, bool fragment_mass_tolerance_unit_ppm, bool is_xlink_spectrum, Size n_charges)
  {
    using boost::math::binomial;
    Size theo_size = theoretical_spec.size();

    if (matched_size < 1 || theo_size < 1)
    {
      return 0;
    }

    double range = theoretical_spec[theo_size-1].getMZ() -  theoretical_spec[0].getMZ();

    // Compute fragment tolerance in Da for the mean of MZ values, if tolerance in ppm (rough approximation)
    double mean = 0.0;
    for (Size i = 0; i < theo_size; ++i)
    {
      mean += theoretical_spec[i].getMZ();
    }
    mean = mean / theo_size;
    double tolerance_Th = fragment_mass_tolerance_unit_ppm ? mean * 1e-6 * fragment_mass_tolerance : fragment_mass_tolerance;

    // A priori probability of a random match given info about the theoretical spectrum
    double a_priori_p = 0;

    if (is_xlink_spectrum)
    {
      a_priori_p = (1 - ( pow( (1 - 2 * tolerance_Th / (0.5 * range)),  (static_cast<double>(theo_size) / static_cast<double>(n_charges)))));
    }
    else
    {
      a_priori_p = (1 - ( pow( (1 - 2 * tolerance_Th / (0.5 * range)),  static_cast<int>(theo_size))));
    }

    double match_odds = 0;

    binomial flip(theo_size, a_priori_p);
    // min double number to avoid 0 values, causing scores with the value "inf"
    match_odds = -log(cdf(complement(flip, matched_size)) + std::numeric_limits<double>::min());

    // score lower than 0 does not make sense, but can happen if cfd = 0, -log( 1 + min() ) < 0
    if (match_odds >= 0.0)
    {
      return match_odds;
    }
    else
    {
      return 0;
    }
  }

  double XQuestScores::matchOddsScoreSimpleSpec(const std::vector< SimpleTSGXLMS::SimplePeak >& theoretical_spec,  const Size matched_size, double fragment_mass_tolerance, bool fragment_mass_tolerance_unit_ppm, bool is_xlink_spectrum, Size n_charges)
  {
    using boost::math::binomial;
    Size theo_size = theoretical_spec.size();

    if (matched_size < 1 || theo_size < 1)
    {
      return 0;
    }

    double range = theoretical_spec[theo_size-1].mz - theoretical_spec[0].mz;

    // Compute fragment tolerance in Da for the mean of MZ values, if tolerance in ppm (rough approximation)
    double mean = 0.0;
    for (Size i = 0; i < theo_size; ++i)
    {
      mean += theoretical_spec[i].mz;
    }
    mean = mean / theo_size;
    double tolerance_Th = fragment_mass_tolerance_unit_ppm ? mean * 1e-6 * fragment_mass_tolerance : fragment_mass_tolerance;

    // A priori probability of a random match given info about the theoretical spectrum
    double a_priori_p = 0;

    if (is_xlink_spectrum)
    {
      a_priori_p = (1 - ( pow( (1 - 2 * tolerance_Th / (0.5 * range)),  (static_cast<double>(theo_size) / static_cast<double>(n_charges)))));
    }
    else
    {
      a_priori_p = (1 - ( pow( (1 - 2 * tolerance_Th / (0.5 * range)),  static_cast<int>(theo_size))));
    }

    double match_odds = 0;

    binomial flip(theo_size, a_priori_p);
    // min double number to avoid 0 values, causing scores with the value "inf"
    match_odds = -log(cdf(complement(flip, matched_size)) + std::numeric_limits<double>::min());

    // score lower than 0 does not make sense, but can happen if cfd = 0, -log( 1 + min() ) < 0
    if (match_odds >= 0.0)
    {
      return match_odds;
    }
    else
    {
      return 0;
    }
  }

  double XQuestScores::logOccupancyProb(const PeakSpectrum& theoretical_spec,  const Size matched_size, double fragment_mass_tolerance, bool fragment_mass_tolerance_unit_ppm)
  {
    using boost::math::binomial;
    Size theo_size = theoretical_spec.size();

    if (matched_size < 1 || theo_size < 1)
    {
      return 0;
    }

    double range;
    double used_tolerance;

    if (fragment_mass_tolerance_unit_ppm)
    {
      range = std::log(theoretical_spec.back().getMZ()) - std::log(theoretical_spec[0].getMZ());
      used_tolerance = fragment_mass_tolerance / 1e6;
    }
    else
    {
      range = theoretical_spec.back().getMZ() - theoretical_spec[0].getMZ();
      used_tolerance = fragment_mass_tolerance;
    }

    // A priori probability of a random match given info about the theoretical spectrum
    double a_priori_p = 0;
    a_priori_p = 1 - pow(1 - 2 * used_tolerance / range,  static_cast<double>(theo_size));

    double log_occu_prob = 0;
    binomial flip(theo_size, a_priori_p);
    // min double number to avoid 0 values, causing scores with the value "inf"
    log_occu_prob = -log(cdf(complement(flip, matched_size)) + std::numeric_limits<double>::min());

    // score lower than 0 does not make sense, but can happen, if cfd = 0, then -log( 1 + <double>::min() ) < 0
    if (log_occu_prob >= 0.0)
    {
      return log_occu_prob;
    }
    else // underflow warning?
    {
      return 0;
    }
  }

  double XQuestScores::weightedTICScoreXQuest(Size alpha_size, Size beta_size, double intsum_alpha, double intsum_beta, double total_current, bool type_is_cross_link)
  {
    // maxdigestlength and mindigestlength from standard settings of xQuest
    double maxdigestlength = 50;
    double mindigestlength = 5;
    if (!type_is_cross_link)
    {
      beta_size = ( maxdigestlength + mindigestlength ) - alpha_size;
    }

    double aatotal = alpha_size + beta_size;

    double invMax = 1 / (mindigestlength / (mindigestlength + maxdigestlength));
    double invFrac_alpha = 1 / (alpha_size / aatotal);
    double invFrac_beta = 1 / (beta_size / aatotal);
    double TIC_weight_alpha = invFrac_alpha / invMax;
    double TIC_weight_beta = invFrac_beta / invMax;

    double wTIC = TIC_weight_alpha * (intsum_alpha / total_current ) + TIC_weight_beta * (intsum_beta / total_current);
    return wTIC;
  }

  double XQuestScores::weightedTICScore(Size alpha_size, Size beta_size, double intsum_alpha, double intsum_beta, double total_current, bool type_is_cross_link)
  {
    if (!type_is_cross_link)
    {
      beta_size = alpha_size;
    }

    double aatotal = alpha_size + beta_size;

    // deviation from xQuest algorithm: invMax is not a constant anymore
    // and scales by the actual length difference between alpha and beta, rather than the maximal possible difference between any two peptides
    // this results in a local scaling, rather than a global one
    double invMax = 1 / (min(alpha_size, beta_size) / aatotal);

    double invFrac_alpha = 1 / (alpha_size / aatotal);
    double invFrac_beta = 1 / (beta_size / aatotal);
    double TIC_weight_alpha = invFrac_alpha / invMax;
    double TIC_weight_beta = invFrac_beta / invMax;

    double wTIC = TIC_weight_alpha * (intsum_alpha / total_current ) + TIC_weight_beta * (intsum_beta / total_current);
    return wTIC;
  }

  double XQuestScores::matchedCurrentChain(const std::vector< std::pair< Size, Size > >& matched_spec_linear, const std::vector< std::pair< Size, Size > >& matched_spec_xlinks, const PeakSpectrum& spectrum_linear_peaks, const PeakSpectrum& spectrum_xlink_peaks)
  {
    double intsum = 0;
    for (SignedSize j = 0; j < static_cast<SignedSize>(matched_spec_linear.size()); ++j)
    {
      intsum += spectrum_linear_peaks[matched_spec_linear[j].second].getIntensity();
    }
    for (SignedSize j = 0; j < static_cast<SignedSize>(matched_spec_xlinks.size()); ++j)
    {
      intsum += spectrum_xlink_peaks[matched_spec_xlinks[j].second].getIntensity();
    }
    return intsum;
  }

  double XQuestScores::totalMatchedCurrent(const std::vector< std::pair< Size, Size > >& matched_spec_linear_alpha, const std::vector< std::pair< Size, Size > >& matched_spec_linear_beta, const std::vector< std::pair< Size, Size > >& matched_spec_xlinks_alpha, const std::vector< std::pair< Size, Size > >& matched_spec_xlinks_beta, const PeakSpectrum& spectrum_linear_peaks, const PeakSpectrum& spectrum_xlink_peaks)
  {
    // make vectors of matched peak indices
    double intsum(0);
    std::vector< Size > indices_linear;
    std::vector< Size > indices_xlinks;
    for (Size j = 0; j < matched_spec_linear_alpha.size(); ++j)
    {
      indices_linear.push_back(matched_spec_linear_alpha[j].second);
    }
    for (Size j = 0; j < matched_spec_linear_beta.size(); ++j)
    {
      indices_linear.push_back(matched_spec_linear_beta[j].second);
    }
    for (Size j = 0; j < matched_spec_xlinks_alpha.size(); ++j)
    {
      indices_xlinks.push_back(matched_spec_xlinks_alpha[j].second);
    }
    for (Size j = 0; j < matched_spec_xlinks_beta.size(); ++j)
    {
      indices_xlinks.push_back(matched_spec_xlinks_beta[j].second);
    }

    // make the indices in the vectors unique, to not sum up peak intensities multiple times
    sort(indices_linear.begin(), indices_linear.end());
    sort(indices_xlinks.begin(), indices_xlinks.end());
    std::vector< Size >::iterator last_unique_linear = unique(indices_linear.begin(), indices_linear.end());
    std::vector< Size >::iterator last_unique_xlinks = unique(indices_xlinks.begin(), indices_xlinks.end());
    indices_linear.erase(last_unique_linear, indices_linear.end());
    indices_xlinks.erase(last_unique_xlinks, indices_xlinks.end());

    // sum over intensities under the unique indices
    for (Size j = 0; j < indices_linear.size(); ++j)
    {
      intsum += spectrum_linear_peaks[indices_linear[j]].getIntensity();
    }
    for (Size j = 0; j < indices_xlinks.size(); ++j)
    {
      intsum += spectrum_xlink_peaks[indices_xlinks[j]].getIntensity();
    }
    return intsum;
  }

  std::vector< double > XQuestScores::xCorrelation(const PeakSpectrum & spec1, const PeakSpectrum & spec2, Int maxshift, double tolerance)
  {
    // generate vector of results, filled with zeroes
    std::vector< double > results(maxshift * 2 + 1, 0);

    // return 0 = no correlation, when one of the spectra is empty
    if (spec1.size() == 0 || spec2.size() == 0) {
      return results;
    }

    double maxionsize = std::max(spec1[spec1.size()-1].getMZ(), spec2[spec2.size()-1].getMZ());
    Int table_size = ceil(maxionsize / tolerance)+1;
    std::vector< double > ion_table1(table_size, 0);
    std::vector< double > ion_table2(table_size, 0);

    // Build tables of the same size, each bin has the size of the tolerance
    for (Size i = 0; i < spec1.size(); ++i)
    {
      Size pos = static_cast<Size>(ceil(spec1[i].getMZ() / tolerance));
      ion_table1[pos] = 10.0;
    }
    for (Size i = 0; i < spec2.size(); ++i)
    {
      Size pos =static_cast<Size>(ceil(spec2[i].getMZ() / tolerance));
      ion_table2[pos] = 10.0;
    }

    // Compute means
    double mean1 = (std::accumulate(ion_table1.begin(), ion_table1.end(), 0.0)) / table_size;
    double mean2 = (std::accumulate(ion_table2.begin(), ion_table2.end(), 0.0)) / table_size;

    // Compute denominator
    double s1 = 0;
    double s2 = 0;
    for (Int i = 0; i < table_size; ++i)
    {
      s1 += pow((ion_table1[i] - mean1), 2);
      s2 += pow((ion_table2[i] - mean2), 2);
    }
    double denom = sqrt(s1 * s2);

    // Calculate correlation for each shift
    for (Int shift = -maxshift; shift <= maxshift; ++shift)
    {
      double s = 0;
      for (Int i = 0; i < table_size; ++i)
      {
        Int j = i + shift;
        if ( (j >= 0) && (j < table_size))
        {
          s += (ion_table1[i] - mean1) * (ion_table2[j] - mean2);
        }
      }
      if (denom > 0)
      {
        results[shift + maxshift] = s / denom;
      }
    }
    return results;
  }

  double XQuestScores::xCorrelationPrescore(const PeakSpectrum & spec1, const PeakSpectrum & spec2, double tolerance)
  {
    // return 0 = no correlation, when one of the spectra is empty
    if (spec1.size() == 0 || spec2.size() == 0) {
      return 0.0;
    }

    double maxionsize = std::max(spec1[spec1.size()-1].getMZ(), spec2[spec2.size()-1].getMZ());
    Int table_size = ceil(maxionsize / tolerance)+1;
    std::vector< double > ion_table1(table_size, 0);
    std::vector< double > ion_table2(table_size, 0);

    // Build tables of the same size, each bin has the size of the tolerance
    for (Size i = 0; i < spec1.size(); ++i)
    {
      Size pos = static_cast<Size>(ceil(spec1[i].getMZ() / tolerance));
      ion_table1[pos] = 1;
    }
    for (Size i = 0; i < spec2.size(); ++i)
    {
      Size pos =static_cast<Size>(ceil(spec2[i].getMZ() / tolerance));
      ion_table2[pos] = 1;

    }

    double dot_product = 0.0;
    for (Size i = 0; i < ion_table1.size(); ++i)
    {
      dot_product += ion_table1[i] * ion_table2[i];
    }

    // determine the smaller spectrum and normalize by the number of peaks in it
    double peaks = std::min(spec1.size(), spec2.size());
    return dot_product / peaks;
  }

}


#include "ros/ros.h"
#include "rovi2/xyz.h"
#include "rovi2/Movexyz.h"
#include "rovi2/State.h"
#include "rovi2/Q.h"

#include <rw/rw.hpp>
#include <rw/math.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/kinematics/Frame.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/models/Device.hpp>
#include <rw/math/Q.hpp>
#include <boost/algorithm/string.hpp>
#include <rw/math/RPY.hpp>
#include <sstream>

rovi2::State robot_state;

void robot_status_callback(const rovi2::State &msg)
{
  ROS_INFO("ROBOT_CALLBACK");
  robot_state = msg;
  //ROS_INFO("Got a state");
}

rw::math::Q toRw(const rovi2::Q& q)
{ // From RobWork
  rw::math::Q res(q.data.size());
  for (std::size_t i = 0; i < q.data.size(); ++i)
  {
    res(i) = q.data[i];
  }
  return res;
}

int main(int argc, char** argv)
{
  ROS_INFO("vision_test_robot_node starting...");
  ros::init(argc, argv, "vision_test_robot_node");
  ros::NodeHandle n;

  ros::Subscriber subscribe_robot_status = n.subscribe("/rovi2/robot_node/Robot_state",0,robot_status_callback);
  ros::Duration(10).sleep();
  ros::spinOnce();
  rw::math::Vector3D<double> transP(-0.116075, -1.67057, 1.33754);
  rw::math::Rotation3D<double> transR(rw::math::RPY<double>(-0.0375525, -0.0132076, -2.0942).toRotation3D());
  rw::math::Transform3D<double> trans(transP, transR);

  rw::kinematics::Frame* robot_base;
  rw::kinematics::Frame* TCP_marker;

  rw::models::WorkCell::Ptr _workcell = rw::loaders::WorkCellLoader::Factory::load("/home/mathias/catkin_ws/src/rovi2/WorkStation_3/WC3_Scene.wc.xml");
  rw::models::Device::Ptr _device = _workcell->findDevice("UR1");
  rw::kinematics::State _state =  _workcell->getDefaultState();

  // Get frames
  robot_base = _workcell->findFrame("UR1.Base");
  TCP_marker = _workcell->findFrame("WSG50.BallError");
  if(robot_base == NULL)
  {
    std::cout << "robot_base not found" << std::endl;
  }
  if(TCP_marker == NULL)
  {
    std::cout << "TCP_marker not found" << std::endl;
  }

  rw::math::Q q = toRw(robot_state.q);

  std::stringstream buf;
  buf << *(robot_base) << " , " << *(TCP_marker) << std::endl;
  ROS_INFO("%s",buf.str().c_str());
  //rw::math::Q q(6,-0.4588, -1.7026, -0.9179, -0.5139, 0.4561, 0.0188);
  //rw::math::Q q(6,-0.45886514427998293, -1.7026772253206577, -0.9179848271019003, -0.513925856428815, 0.45612188890540106, 0.0188171385940185);
  _device->setQ(q,_state);
  rw::math::Vector3D<double> robot_p = robot_base->fTf(TCP_marker,_state).P();
  ROS_INFO("Robot start:");
  buf.str("");
  buf << "Position: "<< robot_p(0) << " " << robot_p(1) << " " << robot_p(2);
  ROS_INFO("%s",buf.str().c_str());

  rovi2::Movexyz service_call;
  //rovi2::xyz;
  ROS_INFO("#1");
  rw::math::Vector3D<double> pos_robot(-0.26, -0.13, 1.22);
  pos_robot = trans*pos_robot;

  service_call.request.target.data.push_back(robot_p(0));//-0.4);//[0] = -0.4;
  service_call.request.target.data.push_back(robot_p(1));//-0.4); //[1] = -0.4;
  service_call.request.target.data.push_back(robot_p(2)-0.02);//0.6);
  service_call.request.target.data.push_back(0);
  service_call.request.target.data.push_back(0);
  service_call.request.target.data.push_back(0);
  for(int i = 0; i < 6; i++)
    ROS_INFO("%f",service_call.request.target.data[i]);
  ros::service::call("/rovi2/robot_node/MoveXYZ",service_call);
  ROS_INFO("Response: %d",service_call.response.success);
  //ros::Duration(20).sleep();
  ros::spinOnce();
/*  ROS_INFO("#2");
  service_call.request.target.data.clear();
  service_call.request.target.data.push_back(-0.4);//[0] = -0.4;
  service_call.request.target.data.push_back(-0.4); //[1] = -0.4;
  service_call.request.target.data.push_back(0.7);
  service_call.request.target.data.push_back(0);
  service_call.request.target.data.push_back(0);
  service_call.request.target.data.push_back(0);
  ros::service::call("/rovi2/robot_node/MoveXYZ",service_call);
  ROS_INFO("Response: %d",service_call.response.success);
*/
  ros::Duration(2).sleep();
  ros::spinOnce();
  ROS_INFO("#3");
  ros::Duration(2).sleep();
  ros::spinOnce();
  ROS_INFO("#4");
  ros::Duration(2).sleep();
  ros::spinOnce();
}


#ifndef ASLAM_PROB_DATA_ASSOC_POLICY_HPP
#define ASLAM_PROB_DATA_ASSOC_POLICY_HPP

#include <math.h>

#include <boost/shared_ptr.hpp>

#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/PerIterationCallback.hpp>

#include <vector>

namespace aslam {
namespace backend {

// Update the weights of the error terms using a gaussian of the squared error,
// i.e: exp(-lambda/2*(y -f(x))^2)
// The matrix of error terms contains, in each row, the error terms whose
// weights must be normalized together
class ProbDataAssocPolicy : public PerIterationCallback {
 public:
  typedef boost::shared_ptr<ErrorTerm> ErrorTermPtr;
  typedef boost::shared_ptr<std::vector<ErrorTermPtr>> ErrorTermGroup;
  typedef boost::shared_ptr<std::vector<ErrorTermGroup>> ErrorTermGroups;

  ProbDataAssocPolicy(ErrorTermGroups error_terms, double lambda);
  // The optimizer will call this function before each iteration.
  void callback() override;

 private:
  ErrorTermGroups error_terms_;
  double scaling_factor_;
};
}  // namespace backend
}  // namespace aslam

#endif /*ASLAM_PROB_DATA_ASSOC_POLICY_HPP*/


//==================================================================================================
/**
  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_FUNCTION_SIMD_ACOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_ACOSH_HPP_INCLUDED

#include <boost/simd/function/scalar/acosh.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/acosh.hpp>

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_ARCH_COMMON_SCALAR_FUNCTION_CLZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_CLZ_HPP_INCLUDED

#ifndef __GNUC__
#include <boost/simd/function/scalar/ffs.hpp>
#include <boost/simd/function/scalar/reversebits.hpp>
#endif
#ifdef BOOST_MSVC
#include <intrin.h>
#endif
#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( clz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type64_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      result_t t1 = bitwise_cast<result_t>(a0);
      BOOST_ASSERT_MSG( t1, "clz not defined for 0" );

    #ifdef __GNUC__
      return __builtin_clzll(t1);
    #else
      return ffs(reversebits(t1))-1;
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( clz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type32_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      result_t t1 = bitwise_cast<result_t>(a0);
      BOOST_ASSERT_MSG( t1, "clz not defined for 0" );

    #ifdef __GNUC__
      return __builtin_clz(t1);
    #else
      return ffs(reversebits(t1))-1;
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( clz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type16_<A0> >
                          )
  {
    BOOST_FORCEINLINE  bd::as_integer_t<A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      using i_t = typename bd::as_integer_t<A0, unsigned>;
      i_t t1 = bitwise_cast<i_t>(a0);
      return clz(uint32_t(t1))-16;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( clz_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::type8_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      using i_t = typename bd::as_integer_t<A0, unsigned>;
      i_t t1 = bitwise_cast<i_t>(a0);
      return clz(uint32_t(t1))-24;
    }
  };
} } }

#endif