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

#ifndef IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL
#define IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL

#include <cstdint>

namespace iox
{
namespace popo
{
template <typename T, typename base_publisher_t>
TypedPublisher<T, base_publisher_t>::TypedPublisher(const capro::ServiceDescription& service)
    : base_publisher_t(service)
{
}

template <typename T, typename base_publisher_t>
inline uid_t TypedPublisher<T, base_publisher_t>::getUid() const noexcept
{
    return base_publisher_t::getUid();
}

template <typename T, typename base_publisher_t>
inline cxx::expected<Sample<T>, AllocationError> TypedPublisher<T, base_publisher_t>::loan() noexcept
{
    return base_publisher_t::loan(sizeof(T));
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::publish(Sample<T>&& sample) noexcept
{
    return base_publisher_t::publish(std::forward<Sample<T>>(sample));
}

template <typename T, typename base_publisher_t>
template <typename Callable, typename... ArgTypes>
inline cxx::expected<AllocationError> TypedPublisher<T, base_publisher_t>::publishResultOf(Callable c,
                                                                                ArgTypes... args) noexcept
{
    static_assert(
        cxx::is_callable<Callable, T*, ArgTypes...>::value,
        "TypedPublisher<T>::publishResultOf expects a valid callable with a specific signature as the first argument");
    static_assert(cxx::has_signature<Callable, void(T*, ArgTypes...)>::value,
                  "callable provided to TypedPublisher<T>::publishResultOf must have signature void(T*, ArgsTypes...)");

    return loan().and_then([&](Sample<T>& sample) {
        c(sample.get(), std::forward<ArgTypes>(args)...);
        publish(std::move(sample));
    });
}

template <typename T, typename base_publisher_t>
inline cxx::expected<AllocationError> TypedPublisher<T, base_publisher_t>::publishCopyOf(const T& val) noexcept
{
    return loan().and_then([&](Sample<T>& sample) {
        *sample.get() = val; // Copy assignment of value into sample's memory allocation.
        publish(std::move(sample));
    });
}

template <typename T, typename base_publisher_t>
inline cxx::optional<Sample<T>> TypedPublisher<T, base_publisher_t>::loanPreviousSample() noexcept
{
    return base_publisher_t::loanPreviousSample();
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::offer() noexcept
{
    return base_publisher_t::offer();
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::stopOffer() noexcept
{
    return base_publisher_t::stopOffer();
}

template <typename T, typename base_publisher_t>
inline bool TypedPublisher<T, base_publisher_t>::isOffered() const noexcept
{
    return base_publisher_t::isOffered();
}

template <typename T, typename base_publisher_t>
inline bool TypedPublisher<T, base_publisher_t>::hasSubscribers() const noexcept
{
    return base_publisher_t::hasSubscribers();
}

} // namespace popo
} // namespace iox

#endif // IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL
<commit_msg>iox-#218 Use type traits from master.<commit_after>// Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL
#define IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL

#include <cstdint>

namespace iox
{
namespace popo
{
template <typename T, typename base_publisher_t>
TypedPublisher<T, base_publisher_t>::TypedPublisher(const capro::ServiceDescription& service)
    : base_publisher_t(service)
{
}

template <typename T, typename base_publisher_t>
inline uid_t TypedPublisher<T, base_publisher_t>::getUid() const noexcept
{
    return base_publisher_t::getUid();
}

template <typename T, typename base_publisher_t>
inline cxx::expected<Sample<T>, AllocationError> TypedPublisher<T, base_publisher_t>::loan() noexcept
{
    return base_publisher_t::loan(sizeof(T));
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::publish(Sample<T>&& sample) noexcept
{
    return base_publisher_t::publish(std::forward<Sample<T>>(sample));
}

template <typename T, typename base_publisher_t>
template <typename Callable, typename... ArgTypes>
inline cxx::expected<AllocationError> TypedPublisher<T, base_publisher_t>::publishResultOf(Callable c,
                                                                                ArgTypes... args) noexcept
{
    static_assert(
        cxx::is_invocable<Callable, T*, ArgTypes...>::value,
        "TypedPublisher<T>::publishResultOf expects a valid callable with a specific signature as the first argument");
    static_assert(cxx::has_signature<Callable, void(T*, ArgTypes...)>::value,
                  "callable provided to TypedPublisher<T>::publishResultOf must have signature void(T*, ArgsTypes...)");

    return loan().and_then([&](Sample<T>& sample) {
        c(sample.get(), std::forward<ArgTypes>(args)...);
        publish(std::move(sample));
    });
}

template <typename T, typename base_publisher_t>
inline cxx::expected<AllocationError> TypedPublisher<T, base_publisher_t>::publishCopyOf(const T& val) noexcept
{
    return loan().and_then([&](Sample<T>& sample) {
        *sample.get() = val; // Copy assignment of value into sample's memory allocation.
        publish(std::move(sample));
    });
}

template <typename T, typename base_publisher_t>
inline cxx::optional<Sample<T>> TypedPublisher<T, base_publisher_t>::loanPreviousSample() noexcept
{
    return base_publisher_t::loanPreviousSample();
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::offer() noexcept
{
    return base_publisher_t::offer();
}

template <typename T, typename base_publisher_t>
inline void TypedPublisher<T, base_publisher_t>::stopOffer() noexcept
{
    return base_publisher_t::stopOffer();
}

template <typename T, typename base_publisher_t>
inline bool TypedPublisher<T, base_publisher_t>::isOffered() const noexcept
{
    return base_publisher_t::isOffered();
}

template <typename T, typename base_publisher_t>
inline bool TypedPublisher<T, base_publisher_t>::hasSubscribers() const noexcept
{
    return base_publisher_t::hasSubscribers();
}

} // namespace popo
} // namespace iox

#endif // IOX_EXPERIMENTAL_POSH_POPO_TYPED_PUBLISHER_INL
<|endoftext|>


<commit_before>#ifndef OPCODE_BASE_H
#define OPCODE_BASE_H
#include "csoundCore.h"
#include <cstdarg>

/**
* Template base class, or pseudo-virtual base class,
* for writing Csound opcodes in C++.
* Derive opcode implementation classes like this:
*
* DerivedClass : public OpcodeBase<DerivedClass>
* {
* public:
*     // All output fields must be declared first as MYFLT *:
*     MYFLT *aret1;
*     // All input fields must be declared next as MYFLT *:
*     MYFLT *iarg1;
*     MYFLT *karg2;
*     MYFLT *aarg3;
*     // All internal state variables must be declared after that:
*     size_t state1;
*     double state2;
*     MYFLT state3;
*     // Declare and implement only whichever of these are required:
*     void initialize();
*     void kontrol();
*     void audio;
*     void deinitialize();
* };
*/
template<typename T>
class OpcodeBase
{
public:
    int init(ENVIRON *csound)
    {
        return NOTOK;
    }
    static int init_(void *csound, void *opcode)
    {
        return reinterpret_cast<T *>(opcode)->init(reinterpret_cast<ENVIRON *>(csound));
    }
    int kontrol(ENVIRON *csound)
    {
        return NOTOK;
    }
    static int kontrol_(void *csound, void *opcode)
    {
        return reinterpret_cast<T *>(opcode)->kontrol(reinterpret_cast<ENVIRON *>(csound));
    }
    int audio(ENVIRON *csound)
    {
        return NOTOK;
    }
    static int audio_(void *csound, void *opcode)
    {
        return reinterpret_cast<T *>(opcode)->audio(reinterpret_cast<ENVIRON *>(csound));
    }
    int deinit(void *csound)
    {
        return NOTOK;
    }
    static int deinit_(void *csound, void *opcode)
    {
        return reinterpret_cast<T *>(opcode)->deinit(reinterpret_cast<ENVIRON *>(csound));
    }
    void log(ENVIRON *csound, const char *format,...)
    {
      va_list args;
      va_start(args, format);
      if(csound) {
            csound->MessageV(csound, 0, format, args);
      }
      else {
            vfprintf(stdout, format, args);
      }
      va_end(args);
    }
    void warn(ENVIRON *csound, const char *format,...)
    {
      va_list args;
      va_start(args, format);
      if(csound) {
          if(csound->GetMessageLevel(csound) & WARNMSG ||
             csound->GetDebug(csound)) {
              csound->MessageV(csound, 0, format, args);
          }
      }
      else {
            vfprintf(stdout, format, args);
      }
      va_end(args);
    }
    OPDS h;
};

#endif
<commit_msg>Changed to use new deinit callback, removed deinit OENTRY member.<commit_after>#ifndef OPCODE_BASE_H
#define OPCODE_BASE_H
#include "csoundCore.h"
#include <cstdarg>

/**
 * Template base class, or pseudo-virtual base class,
 * for writing Csound opcodes in C++.
 * Derive opcode implementation classes like this:
 *
 * DerivedClass : public OpcodeBase<DerivedClass>
 * {
 * public:
 *     // All output fields must be declared first as MYFLT *:
 *     MYFLT *aret1;
 *     // All input fields must be declared next as MYFLT *:
 *     MYFLT *iarg1;
 *     MYFLT *karg2;
 *     MYFLT *aarg3;
 *     // All internal state variables must be declared after that:
 *     size_t state1;
 *     double state2;
 *     MYFLT state3;
 *     // Declare and implement only whichever of these are required:
 *     void init();
 *     void kontrol();
 *     void audio;
 *     void noteoff();
 *     void deinit();
 * };
 */
template<typename T>
class OpcodeBase
{
public:
  int init(ENVIRON *csound)
  {
    return NOTOK;
  }
  static int init_(void *csound_, void *opcode_)
  {
    ENVIRON *csound = reinterpret_cast<ENVIRON *>(csound_);
    T *opcode = reinterpret_cast<T *>(opcode_);
    csound->RegisterDeinitCallback(csound, &opcode->h, OpcodeBase<T>::noteoff_);
    return opcode->init(csound);
  }
  int kontrol(ENVIRON *csound)
  {
    return NOTOK;
  }
  static int kontrol_(void *csound, void *opcode)
  {
    return reinterpret_cast<T *>(opcode)->kontrol(reinterpret_cast<ENVIRON *>(csound));
  }
  int audio(ENVIRON *csound)
  {
    return NOTOK;
  }
  static int audio_(void *csound, void *opcode)
  {
    return reinterpret_cast<T *>(opcode)->audio(reinterpret_cast<ENVIRON *>(csound));
  }
  int noteoff(ENVIRON *csound)
  {
    return OK;
  }
  static int noteoff_(void *csound, void *opcode)
  {
    return reinterpret_cast< OpcodeBase<T> *>(opcode)->noteoff(reinterpret_cast<ENVIRON *>(csound));
  }
  void log(ENVIRON *csound, const char *format,...)
  {
    va_list args;
    va_start(args, format);
    if(csound) {
      csound->MessageV(csound, 0, format, args);
    }
    else {
      vfprintf(stdout, format, args);
    }
    va_end(args);
  }
  void warn(ENVIRON *csound, const char *format,...)
  {
    va_list args;
    va_start(args, format);
    if(csound) {
      if(csound->GetMessageLevel(csound) & WARNMSG ||
	 csound->GetDebug(csound)) {
	csound->MessageV(csound, 0, format, args);
      }
    }
    else {
      vfprintf(stdout, format, args);
    }
    va_end(args);
  }
  OPDS h;
};

#endif
<|endoftext|>


<commit_before>#include "storage_manager.hpp"

namespace opossum {

void storage_manager::add_table(std::string name, std::shared_ptr<table> tp) {
	_tables.insert(std::make_pair(name, std::move(tp)));
}

void storage_manager::print(std::ostream &out) const {
	out << "===== Tables " << std::endl;

	auto cnt = 0;
	for(auto const &tab : _tables) {
		out << "\t table #" << cnt << ": \t" << tab.first;
		// out << "(" << tab.second->col_count() << "columns, " << tab.second->row_count() << " rows)" << std::endl;
		out << std::endl;
		cnt++;
	}
}

}<commit_msg>Finished first version of storage manager's print<commit_after>#include "storage_manager.hpp"

namespace opossum {

void storage_manager::add_table(std::string name, std::shared_ptr<table> tp) {
	_tables.insert(std::make_pair(name, std::move(tp)));
}

void storage_manager::print(std::ostream &out) const {
	out << "==================" << std::endl;
	out << "===== Tables =====" << std::endl << std::endl;

	auto cnt = 0;
	for(auto const &tab : _tables) {
		out << "==== table >> " << tab.first << " <<";
		out << " (" << tab.second->col_count() << " columns, " << tab.second->row_count() << " rows)";
		out << std::endl << std::endl;
		tab.second->print();
		cnt++;
	}
}

}<|endoftext|>


<commit_before>/*

*/

#include "lineFollowing.h"
#include "../control.h"

using namespace std;
using namespace cv;

void compress_lines(vector<Vec2f> &condensed, const vector<Vec2f> &tmp_list);

void draw_lines(Mat &image, const vector<Vec2f> &lines);

vector<Vec2f> condense_lines(vector<Vec2f> lines);

double deg2rad(double deg);

void LineFollowing::detect_lines(Mat &original_frame) {

  Mat hsv;
  Mat mask;
  width = original_frame.cols;
  height = original_frame.rows;

  cvtColor(original_frame, hsv, CV_BGR2HSV); // Image is now HSV

  Scalar lower(minH, minS, minV);
  Scalar upper(maxH, maxS, maxV);
  inRange(hsv, lower, upper, mask); // Create a mask of only the desired color
  Canny(mask, mask, 50, 200, 3);

  vector<Vec2f> lines;
  HoughLines(mask, lines, 1, CV_PI / 180, 100, 0, 0);

  // printf("Adding in %d lines\n", (int) lines.size());

  vector<Vec2f> tmp = condense_lines(lines);
  if (tmp.size() > 0) {
    found_lines = tmp;
  }
  draw_lines(original_frame, found_lines);

}

vector<Vec2f> condense_lines(vector<Vec2f> lines) {
  vector<Vec2f> condensed;
  vector<Vec2f> tmp_list;
  for (int i = 0; i < lines.size(); i++) {
    if (lines[i][1] > CV_PI / 2) {
      lines[i][1] -= CV_PI;
      lines[i][0] *= -1;
    }
    else if (lines[i][1] < ((CV_PI / 2) * -1)) {
      lines[i][1] += CV_PI;
      lines[i][0] *= -1;
    }

    //put in order of theta, rho
    swap(lines[i][0], lines[i][1]);
  }

  //Order from least to greatest theta
  sort(lines.begin(), lines.end(),
       [](const Vec2f &a, const Vec2f &b) {
         return a[0] < b[0];
       });

  while (!lines.empty()) {
    Vec2f to_manipulate = lines.front();
    lines.erase(lines.begin());

    if (tmp_list.empty()) {
      tmp_list.push_back(to_manipulate);
      continue;
    } else {
      if (abs(to_manipulate[0] - tmp_list.front()[0]) < deg2rad(20)) {
        //The angles are similar
        if (abs(to_manipulate[1] - tmp_list.front()[1]) < 50) {
          //the distances are similar
          tmp_list.push_back(to_manipulate);
          continue;
        }
      } else {
        //Need to clear out the tmp_list
        compress_lines(condensed, tmp_list);

        tmp_list.clear();
        tmp_list.push_back(to_manipulate);

      }
    }
  }
  if (!tmp_list.empty()) {
    compress_lines(condensed, tmp_list);
  }
  return condensed;
}

double deg2rad(double deg) {
  return deg * (CV_PI / 180.0);
}

void draw_lines(Mat &image, const vector<Vec2f> &lines) {
  for (size_t i = 0; i < lines.size(); i++) {
    float theta = lines[i][0], rho = lines[i][1];
    // float rho = lines[i][0], theta = lines[i][1];
    Point pt1, pt2;
    double a = cos(theta), b = sin(theta);
    double x0 = a * rho, y0 = b * rho;
    pt1.x = cvRound(x0 + 1000 * (-b));
    pt1.y = cvRound(y0 + 1000 * (a));
    pt2.x = cvRound(x0 - 1000 * (-b));
    pt2.y = cvRound(y0 - 1000 * (a));
    line(image, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);
  }
}

void compress_lines(vector<Vec2f> &condensed, const vector<Vec2f> &tmp_list) {
  Vec2f new_point;
  float angle_sum = 0;
  float rho_sum = 0;
  for (int j = 0; j < tmp_list.size(); ++j) {
    angle_sum += tmp_list[j][0];
    rho_sum += tmp_list[j][1];
  }
  angle_sum /= tmp_list.size();
  rho_sum /= tmp_list.size();

  new_point[0] = angle_sum;
  new_point[1] = rho_sum;

  condensed.push_back(new_point);
}

LineFollowing::LineFollowing(Control *control) {
  namedWindow("line_window", CV_WINDOW_NORMAL);
  control_ptr = control;
  minH = 50;
  minS = 20;
  minV = 150;
  maxH = 125;
  maxS = 100;
  maxV = 255;
  return;
}

void LineFollowing::close() {
  return;
}

void LineFollowing::fly() {
  // cout << "line following\n";
  detect_lines(control_ptr->image);

  control_ptr->velocities.vx = 0;
  control_ptr->velocities.vy = 0;
  control_ptr->velocities.vz = 0;
  control_ptr->velocities.vr = 0;

  if (found_lines.size() < 1) {
    //I found no lines
    // printf("Found nothing\n");
    return;
  }
  double calculated_distance = distance_from_center(found_lines[0][1], found_lines[0][0], control_ptr->image.cols,
                                                    control_ptr->image.rows);

  // printf("calc dist of %f ", calculated_distance);
  int origin_x = 0 + (control_ptr->image.cols / 2);
  int origin_y = 0 + (control_ptr->image.rows / 2);
  Point pt1 = cvPoint(origin_x, origin_y);
  Point pt2 = cvPoint(origin_x + cvRound(calculated_distance), origin_y);
  line(control_ptr->image, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);

  if (found_lines.size() > 1) {
    // I need to turn
    //TODO: move until line is centered
    //TODO: look for symbol, and turn

    Vec2i point = find_intersection(found_lines[0], found_lines[1]);
    printf("My coords are x: %3d y %3d\n", point[0], point[1]);
    line(control_ptr->image, cvPoint(point[0]+10, point[1]), cvPoint(point[0]-10, point[1]), Scalar(0, 255,0 ), 3, CV_AA);
    line(control_ptr->image, cvPoint(point[0], point[1]+10), cvPoint(point[0], point[1]-10), Scalar(0, 255,0), 3, CV_AA);
    return;
  }

  else {
    // I need to snap myself to the line
    if (found_lines[0][0] >= deg2rad(5)) {
      control_ptr->velocities.vr = -.2;
      printf("Turning Right\n");
    }
    else if (found_lines[0][0] <= deg2rad(-1 * 5)) {
      control_ptr->velocities.vr = .2;
      printf("Turning Left\n");
    } else {
      printf("Checking Distance\n");

      double offset = calculated_distance;
      // printf("Offset is: %5.2f with a distance of %5.2f and width of %5.2f halved to %5.2f\n", offset,
      // found_lines[0][1],
      // (double) control_ptr->image.cols, (control_ptr->image.cols / 2.0));
      if (-100 <= offset && offset <= 100) {
        printf("No need to move\n");
      } else if (offset < 0) {
        //we are to the right of the line
        //we need to move left
        control_ptr->velocities.vy = 1;
        printf("Move left\n");
      } else {
        //we need to move right
        control_ptr->velocities.vy = -1;
        printf("Move right\n");
      }
    }
  }

  return;
}

double LineFollowing::distance_from_center(float rho, float theta, double width, double height) {
  // printf("width: %f height: %f\n", width, height);
  theta = -1 * (theta);
  double rh = rho * cos(theta);
  double rv = rho * sin(theta);
  // rh = abs(rh);
  // rv *= -1;
  double excess = (width / 2.0) - rh;
  // excess *=-1;
  double lv = rv + (height / 2.0);
  double lh = lv * tan(theta);
  // lh = abs(lh);
  double x = lh - excess;

  // printf("ro: %4f th: %4f rh: %4f rv: %4f ex: %4f lv: %4f lh: %4f x: %4f\n", rho, theta ,rh, rv, excess, lv,lh,x);

  return x;
}

cv::Vec2i LineFollowing::find_intersection(cv::Vec2f a, cv::Vec2f b) {
  Vec2i rv;
  float theta1, theta2, rho1, rho2;
  theta1 = a[0];
  rho1 = a[1];
  theta2 = b[0];
  rho2 = b[1];
  float a1 = 1 / atan(theta1);
  float a2 = 1 / atan(theta2);
  float b1 = rho1;
  float b2 = rho2;
  float x = (b2 - b1) / (a1 - a2);
  float y = (a1 * b2 - a2 * b1) / (a1 - a2);
  rv[0] = (int) round(x);
  rv[1] = (int) round(y);

  return rv;
}
<commit_msg>should have correct coordinate system for the intersection<commit_after>/*

*/

#include "lineFollowing.h"
#include "../control.h"

using namespace std;
using namespace cv;

void compress_lines(vector<Vec2f> &condensed, const vector<Vec2f> &tmp_list);

void draw_lines(Mat &image, const vector<Vec2f> &lines);

vector<Vec2f> condense_lines(vector<Vec2f> lines);

double deg2rad(double deg);

void LineFollowing::detect_lines(Mat &original_frame) {

  Mat hsv;
  Mat mask;
  width = original_frame.cols;
  height = original_frame.rows;

  cvtColor(original_frame, hsv, CV_BGR2HSV); // Image is now HSV

  Scalar lower(minH, minS, minV);
  Scalar upper(maxH, maxS, maxV);
  inRange(hsv, lower, upper, mask); // Create a mask of only the desired color
  Canny(mask, mask, 50, 200, 3);

  vector<Vec2f> lines;
  HoughLines(mask, lines, 1, CV_PI / 180, 100, 0, 0);

  // printf("Adding in %d lines\n", (int) lines.size());

  vector<Vec2f> tmp = condense_lines(lines);
  if (tmp.size() > 0) {
    found_lines = tmp;
  }
  draw_lines(original_frame, found_lines);

}

vector<Vec2f> condense_lines(vector<Vec2f> lines) {
  vector<Vec2f> condensed;
  vector<Vec2f> tmp_list;
  for (int i = 0; i < lines.size(); i++) {
    if (lines[i][1] > CV_PI / 2) {
      lines[i][1] -= CV_PI;
      lines[i][0] *= -1;
    }
    else if (lines[i][1] < ((CV_PI / 2) * -1)) {
      lines[i][1] += CV_PI;
      lines[i][0] *= -1;
    }

    //put in order of theta, rho
    swap(lines[i][0], lines[i][1]);
  }

  //Order from least to greatest theta
  sort(lines.begin(), lines.end(),
       [](const Vec2f &a, const Vec2f &b) {
         return a[0] < b[0];
       });

  while (!lines.empty()) {
    Vec2f to_manipulate = lines.front();
    lines.erase(lines.begin());

    if (tmp_list.empty()) {
      tmp_list.push_back(to_manipulate);
      continue;
    } else {
      if (abs(to_manipulate[0] - tmp_list.front()[0]) < deg2rad(20)) {
        //The angles are similar
        if (abs(to_manipulate[1] - tmp_list.front()[1]) < 50) {
          //the distances are similar
          tmp_list.push_back(to_manipulate);
          continue;
        }
      } else {
        //Need to clear out the tmp_list
        compress_lines(condensed, tmp_list);

        tmp_list.clear();
        tmp_list.push_back(to_manipulate);

      }
    }
  }
  if (!tmp_list.empty()) {
    compress_lines(condensed, tmp_list);
  }
  return condensed;
}

double deg2rad(double deg) {
  return deg * (CV_PI / 180.0);
}

void draw_lines(Mat &image, const vector<Vec2f> &lines) {
  for (size_t i = 0; i < lines.size(); i++) {
    float theta = lines[i][0], rho = lines[i][1];
    // float rho = lines[i][0], theta = lines[i][1];
    Point pt1, pt2;
    double a = cos(theta), b = sin(theta);
    double x0 = a * rho, y0 = b * rho;
    pt1.x = cvRound(x0 + 1000 * (-b));
    pt1.y = cvRound(y0 + 1000 * (a));
    pt2.x = cvRound(x0 - 1000 * (-b));
    pt2.y = cvRound(y0 - 1000 * (a));
    line(image, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);
  }
}

void compress_lines(vector<Vec2f> &condensed, const vector<Vec2f> &tmp_list) {
  Vec2f new_point;
  float angle_sum = 0;
  float rho_sum = 0;
  for (int j = 0; j < tmp_list.size(); ++j) {
    angle_sum += tmp_list[j][0];
    rho_sum += tmp_list[j][1];
  }
  angle_sum /= tmp_list.size();
  rho_sum /= tmp_list.size();

  new_point[0] = angle_sum;
  new_point[1] = rho_sum;

  condensed.push_back(new_point);
}

LineFollowing::LineFollowing(Control *control) {
  namedWindow("line_window", CV_WINDOW_NORMAL);
  control_ptr = control;
  minH = 50;
  minS = 20;
  minV = 150;
  maxH = 125;
  maxS = 100;
  maxV = 255;
  return;
}

void LineFollowing::close() {
  return;
}

void LineFollowing::fly() {
  // cout << "line following\n";
  detect_lines(control_ptr->image);

  control_ptr->velocities.vx = 0;
  control_ptr->velocities.vy = 0;
  control_ptr->velocities.vz = 0;
  control_ptr->velocities.vr = 0;

  if (found_lines.size() < 1) {
    //I found no lines
    // printf("Found nothing\n");
    return;
  }
  double calculated_distance = distance_from_center(found_lines[0][1], found_lines[0][0], control_ptr->image.cols,
                                                    control_ptr->image.rows);

  // printf("calc dist of %f ", calculated_distance);
  int origin_x = 0 + (control_ptr->image.cols / 2);
  int origin_y = 0 + (control_ptr->image.rows / 2);
  Point pt1 = cvPoint(origin_x, origin_y);
  Point pt2 = cvPoint(origin_x + cvRound(calculated_distance), origin_y);
  line(control_ptr->image, pt1, pt2, Scalar(0, 0, 255), 3, CV_AA);

  if (found_lines.size() > 1) {
    // I need to turn
    //TODO: move until line is centered
    //TODO: look for symbol, and turn

    Vec2i point = find_intersection(found_lines[0], found_lines[1]);
    printf("My coords are x: %3d y %3d\n", point[0], point[1]);
    line(control_ptr->image, cvPoint(origin_x + point[0]+10, origin_y+point[1]), cvPoint(origin_x+point[0]-10, origin_y+point[1]), Scalar(0, 255,0 ), 3, CV_AA);
    line(control_ptr->image, cvPoint(origin_x+point[0], origin_y+point[1]+10), cvPoint(origin_x+point[0], origin_y+point[1]-10), Scalar(0, 255,0), 3, CV_AA);
    return;
  }

  else {
    // I need to snap myself to the line
    if (found_lines[0][0] >= deg2rad(5)) {
      control_ptr->velocities.vr = -.2;
      printf("Turning Right\n");
    }
    else if (found_lines[0][0] <= deg2rad(-1 * 5)) {
      control_ptr->velocities.vr = .2;
      printf("Turning Left\n");
    } else {
      printf("Checking Distance\n");

      double offset = calculated_distance;
      // printf("Offset is: %5.2f with a distance of %5.2f and width of %5.2f halved to %5.2f\n", offset,
      // found_lines[0][1],
      // (double) control_ptr->image.cols, (control_ptr->image.cols / 2.0));
      if (-100 <= offset && offset <= 100) {
        printf("No need to move\n");
      } else if (offset < 0) {
        //we are to the right of the line
        //we need to move left
        control_ptr->velocities.vy = 1;
        printf("Move left\n");
      } else {
        //we need to move right
        control_ptr->velocities.vy = -1;
        printf("Move right\n");
      }
    }
  }

  return;
}

double LineFollowing::distance_from_center(float rho, float theta, double width, double height) {
  // printf("width: %f height: %f\n", width, height);
  theta = -1 * (theta);
  double rh = rho * cos(theta);
  double rv = rho * sin(theta);
  // rh = abs(rh);
  // rv *= -1;
  double excess = (width / 2.0) - rh;
  // excess *=-1;
  double lv = rv + (height / 2.0);
  double lh = lv * tan(theta);
  // lh = abs(lh);
  double x = lh - excess;

  // printf("ro: %4f th: %4f rh: %4f rv: %4f ex: %4f lv: %4f lh: %4f x: %4f\n", rho, theta ,rh, rv, excess, lv,lh,x);

  return x;
}

cv::Vec2i LineFollowing::find_intersection(cv::Vec2f a, cv::Vec2f b) {
  Vec2i rv;
  float theta1, theta2, rho1, rho2;
  theta1 = a[0];
  rho1 = a[1];
  theta2 = b[0];
  rho2 = b[1];
  float a1 = 1 / atan(theta1);
  float a2 = 1 / atan(theta2);
  float b1 = rho1;
  float b2 = rho2;
  float x = (b2 - b1) / (a1 - a2);
  float y = (a1 * b2 - a2 * b1) / (a1 - a2);
  rv[0] = (int) round(x);
  rv[1] = (int) round(y);

  return rv;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
 *
 * 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 PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file gnss.cpp
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
 * @author Andrew Chambers <achamber@gmail.com>
 *
 */

#include "gnss.hpp"
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <mathlib/mathlib.h>

const char *const UavcanGnssBridge::NAME = "gnss";

UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
	_node(node),
	_sub_fix(node),
	_sub_fix2(node),
	_report_pub(nullptr)
{
}

int UavcanGnssBridge::init()
{
	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
	if (res < 0) {
		warnx("GNSS fix sub failed %i", res);
		return res;
	}

	res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
	if (res < 0) {
		warnx("GNSS fix2 sub failed %i", res);
		return res;
	}

	return res;
}

unsigned UavcanGnssBridge::get_num_redundant_channels() const
{
	return (_receiver_node_id < 0) ? 0 : 1;
}

void UavcanGnssBridge::print_status() const
{
	printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());

	if (_receiver_node_id < 0) {
		printf("N/A\n");

	} else {
		printf("%d\n", _receiver_node_id);
	}
}

void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
	const bool valid_pos_cov = !msg.position_covariance.empty();
	const bool valid_vel_cov = !msg.velocity_covariance.empty();

	float pos_cov[9];
	msg.position_covariance.unpackSquareMatrix(pos_cov);

	float vel_cov[9];
	msg.velocity_covariance.unpackSquareMatrix(vel_cov);

	process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
}

void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
{
	if (_old_fix_subscriber_active) {
		warnx("GNSS Fix2 message detected, disabling support for the old Fix message");
		_sub_fix.stop();
		_old_fix_subscriber_active = false;
		_receiver_node_id = -1;
	}

	float pos_cov[9]{};
	float vel_cov[9]{};
	bool valid_covariances = true;

	switch (msg.covariance.size()) {
	// Scalar matrix
	case 1: {
		const auto x = msg.covariance[0];

		pos_cov[0] = x;
		pos_cov[4] = x;
		pos_cov[8] = x;

		vel_cov[0] = x;
		vel_cov[4] = x;
		vel_cov[8] = x;
		break;
	}

	// Diagonal matrix (the most common case)
	case 6: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[4] = msg.covariance[1];
		pos_cov[8] = msg.covariance[2];

		vel_cov[0] = msg.covariance[3];
		vel_cov[4] = msg.covariance[4];
		vel_cov[8] = msg.covariance[5];
		break;
	}

	// Upper triangular matrix.
	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
	// 0  1  2
	// 1  6  7
	// 2  7 11
	//         15 16 17
	//         16 18 19
	//         17 19 20
	case 21: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[1] = msg.covariance[1];
		pos_cov[2] = msg.covariance[2];
		pos_cov[3] = msg.covariance[1];
		pos_cov[4] = msg.covariance[6];
		pos_cov[5] = msg.covariance[7];
		pos_cov[6] = msg.covariance[2];
		pos_cov[7] = msg.covariance[7];
		pos_cov[8] = msg.covariance[11];

		vel_cov[0] = msg.covariance[15];
		vel_cov[1] = msg.covariance[16];
		vel_cov[2] = msg.covariance[17];
		vel_cov[3] = msg.covariance[16];
		vel_cov[4] = msg.covariance[18];
		vel_cov[5] = msg.covariance[19];
		vel_cov[6] = msg.covariance[17];
		vel_cov[7] = msg.covariance[19];
		vel_cov[8] = msg.covariance[20];
	}

	// Full matrix 6x6.
	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
	//  0  1  2
	//  6  7  8
	// 12 13 14
	//          21 22 23
	//          27 28 29
	//          33 34 35
	case 36: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[1] = msg.covariance[1];
		pos_cov[2] = msg.covariance[2];
		pos_cov[3] = msg.covariance[6];
		pos_cov[4] = msg.covariance[7];
		pos_cov[5] = msg.covariance[8];
		pos_cov[6] = msg.covariance[12];
		pos_cov[7] = msg.covariance[13];
		pos_cov[8] = msg.covariance[14];

		vel_cov[0] = msg.covariance[21];
		vel_cov[1] = msg.covariance[22];
		vel_cov[2] = msg.covariance[23];
		vel_cov[3] = msg.covariance[27];
		vel_cov[4] = msg.covariance[28];
		vel_cov[5] = msg.covariance[29];
		vel_cov[6] = msg.covariance[33];
		vel_cov[7] = msg.covariance[34];
		vel_cov[8] = msg.covariance[35];
	}

	// Either empty or invalid sized, interpret as zero matrix
	default: {
		valid_covariances = false;
		break;	// Nothing to do
	}
	}

	process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
}

template <typename FixType>
void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
                                    const float (&pos_cov)[9],
                                    const float (&vel_cov)[9],
                                    const bool valid_pos_cov,
                                    const bool valid_vel_cov)
{
	// This bridge does not support redundant GNSS receivers yet.
	if (_receiver_node_id < 0) {
		_receiver_node_id = msg.getSrcNodeID().get();
		warnx("GNSS receiver node ID: %d", _receiver_node_id);

	} else {
		if (_receiver_node_id != msg.getSrcNodeID().get()) {
			return;  // This GNSS receiver is the redundant one, ignore it.
		}
	}

	auto report = ::vehicle_gps_position_s();

	/*
	 * FIXME HACK
	 * There used to be the following line of code:
	 * 	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
	 * to use an independent time source (based on hardware TIM5) instead of HRT.
	 * The proper solution is to be developed.
	 */
	report.timestamp = hrt_absolute_time();
	report.lat = msg.latitude_deg_1e8 / 10;
	report.lon = msg.longitude_deg_1e8 / 10;
	report.alt = msg.height_msl_mm;

	if (valid_pos_cov) {
		// Horizontal position uncertainty
		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
		report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;

		// Vertical position uncertainty
		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;

	} else {
		report.eph = -1.0F;
		report.epv = -1.0F;
	}

	if (valid_vel_cov) {
		report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);

		/* There is a nonlinear relationship between the velocity vector and the heading.
		 * Use Jacobian to transform velocity covariance to heading covariance
		 *
		 * Nonlinear equation:
		 * heading = atan2(vel_e_m_s, vel_n_m_s)
		 * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
		 *
		 * To calculate the variance of heading from the variance of velocity,
		 * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
		 */
		float vel_n = msg.ned_velocity[0];
		float vel_e = msg.ned_velocity[1];
		float vel_n_sq = vel_n * vel_n;
		float vel_e_sq = vel_e * vel_e;
		report.c_variance_rad =
			(vel_e_sq * vel_cov[0] +
			 -2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
			 vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));

	} else {
		report.s_variance_m_s = -1.0F;
		report.c_variance_rad = -1.0F;
	}

	report.fix_type = msg.status;

	report.vel_n_m_s = msg.ned_velocity[0];
	report.vel_e_m_s = msg.ned_velocity[1];
	report.vel_d_m_s = msg.ned_velocity[2];
	report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s +
	                       report.vel_e_m_s * report.vel_e_m_s +
	                       report.vel_d_m_s * report.vel_d_m_s);
	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
	report.vel_ned_valid = true;

	report.timestamp_time_relative = 0;
	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds

	report.satellites_used = msg.sats_used;

	// Using PDOP for HDOP and VDOP
	// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
	report.hdop = msg.pdop;
	report.vdop = msg.pdop;

	// Publish to a multi-topic
	int32_t gps_orb_instance;
	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
				 ORB_PRIO_HIGH);
}
<commit_msg>UAVCAN GNSS bridge status output shows whether support for the old Fix message is active<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
 *
 * 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 PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file gnss.cpp
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
 * @author Andrew Chambers <achamber@gmail.com>
 *
 */

#include "gnss.hpp"
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <mathlib/mathlib.h>

const char *const UavcanGnssBridge::NAME = "gnss";

UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
	_node(node),
	_sub_fix(node),
	_sub_fix2(node),
	_report_pub(nullptr)
{
}

int UavcanGnssBridge::init()
{
	int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
	if (res < 0) {
		warnx("GNSS fix sub failed %i", res);
		return res;
	}

	res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
	if (res < 0) {
		warnx("GNSS fix2 sub failed %i", res);
		return res;
	}

	return res;
}

unsigned UavcanGnssBridge::get_num_redundant_channels() const
{
	return (_receiver_node_id < 0) ? 0 : 1;
}

void UavcanGnssBridge::print_status() const
{
	printf("RX errors: %d, using old Fix: %d, receiver node id: ",
		_sub_fix.getFailureCount(), int(_old_fix_subscriber_active));

	if (_receiver_node_id < 0) {
		printf("N/A\n");

	} else {
		printf("%d\n", _receiver_node_id);
	}
}

void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
	const bool valid_pos_cov = !msg.position_covariance.empty();
	const bool valid_vel_cov = !msg.velocity_covariance.empty();

	float pos_cov[9];
	msg.position_covariance.unpackSquareMatrix(pos_cov);

	float vel_cov[9];
	msg.velocity_covariance.unpackSquareMatrix(vel_cov);

	process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
}

void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
{
	if (_old_fix_subscriber_active) {
		warnx("GNSS Fix2 message detected, disabling support for the old Fix message");
		_sub_fix.stop();
		_old_fix_subscriber_active = false;
		_receiver_node_id = -1;
	}

	float pos_cov[9]{};
	float vel_cov[9]{};
	bool valid_covariances = true;

	switch (msg.covariance.size()) {
	// Scalar matrix
	case 1: {
		const auto x = msg.covariance[0];

		pos_cov[0] = x;
		pos_cov[4] = x;
		pos_cov[8] = x;

		vel_cov[0] = x;
		vel_cov[4] = x;
		vel_cov[8] = x;
		break;
	}

	// Diagonal matrix (the most common case)
	case 6: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[4] = msg.covariance[1];
		pos_cov[8] = msg.covariance[2];

		vel_cov[0] = msg.covariance[3];
		vel_cov[4] = msg.covariance[4];
		vel_cov[8] = msg.covariance[5];
		break;
	}

	// Upper triangular matrix.
	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
	// 0  1  2
	// 1  6  7
	// 2  7 11
	//         15 16 17
	//         16 18 19
	//         17 19 20
	case 21: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[1] = msg.covariance[1];
		pos_cov[2] = msg.covariance[2];
		pos_cov[3] = msg.covariance[1];
		pos_cov[4] = msg.covariance[6];
		pos_cov[5] = msg.covariance[7];
		pos_cov[6] = msg.covariance[2];
		pos_cov[7] = msg.covariance[7];
		pos_cov[8] = msg.covariance[11];

		vel_cov[0] = msg.covariance[15];
		vel_cov[1] = msg.covariance[16];
		vel_cov[2] = msg.covariance[17];
		vel_cov[3] = msg.covariance[16];
		vel_cov[4] = msg.covariance[18];
		vel_cov[5] = msg.covariance[19];
		vel_cov[6] = msg.covariance[17];
		vel_cov[7] = msg.covariance[19];
		vel_cov[8] = msg.covariance[20];
	}

	// Full matrix 6x6.
	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
	//  0  1  2
	//  6  7  8
	// 12 13 14
	//          21 22 23
	//          27 28 29
	//          33 34 35
	case 36: {
		pos_cov[0] = msg.covariance[0];
		pos_cov[1] = msg.covariance[1];
		pos_cov[2] = msg.covariance[2];
		pos_cov[3] = msg.covariance[6];
		pos_cov[4] = msg.covariance[7];
		pos_cov[5] = msg.covariance[8];
		pos_cov[6] = msg.covariance[12];
		pos_cov[7] = msg.covariance[13];
		pos_cov[8] = msg.covariance[14];

		vel_cov[0] = msg.covariance[21];
		vel_cov[1] = msg.covariance[22];
		vel_cov[2] = msg.covariance[23];
		vel_cov[3] = msg.covariance[27];
		vel_cov[4] = msg.covariance[28];
		vel_cov[5] = msg.covariance[29];
		vel_cov[6] = msg.covariance[33];
		vel_cov[7] = msg.covariance[34];
		vel_cov[8] = msg.covariance[35];
	}

	// Either empty or invalid sized, interpret as zero matrix
	default: {
		valid_covariances = false;
		break;	// Nothing to do
	}
	}

	process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
}

template <typename FixType>
void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
                                    const float (&pos_cov)[9],
                                    const float (&vel_cov)[9],
                                    const bool valid_pos_cov,
                                    const bool valid_vel_cov)
{
	// This bridge does not support redundant GNSS receivers yet.
	if (_receiver_node_id < 0) {
		_receiver_node_id = msg.getSrcNodeID().get();
		warnx("GNSS receiver node ID: %d", _receiver_node_id);

	} else {
		if (_receiver_node_id != msg.getSrcNodeID().get()) {
			return;  // This GNSS receiver is the redundant one, ignore it.
		}
	}

	auto report = ::vehicle_gps_position_s();

	/*
	 * FIXME HACK
	 * There used to be the following line of code:
	 * 	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
	 * to use an independent time source (based on hardware TIM5) instead of HRT.
	 * The proper solution is to be developed.
	 */
	report.timestamp = hrt_absolute_time();
	report.lat = msg.latitude_deg_1e8 / 10;
	report.lon = msg.longitude_deg_1e8 / 10;
	report.alt = msg.height_msl_mm;

	if (valid_pos_cov) {
		// Horizontal position uncertainty
		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
		report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;

		// Vertical position uncertainty
		report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;

	} else {
		report.eph = -1.0F;
		report.epv = -1.0F;
	}

	if (valid_vel_cov) {
		report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);

		/* There is a nonlinear relationship between the velocity vector and the heading.
		 * Use Jacobian to transform velocity covariance to heading covariance
		 *
		 * Nonlinear equation:
		 * heading = atan2(vel_e_m_s, vel_n_m_s)
		 * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
		 *
		 * To calculate the variance of heading from the variance of velocity,
		 * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
		 */
		float vel_n = msg.ned_velocity[0];
		float vel_e = msg.ned_velocity[1];
		float vel_n_sq = vel_n * vel_n;
		float vel_e_sq = vel_e * vel_e;
		report.c_variance_rad =
			(vel_e_sq * vel_cov[0] +
			 -2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
			 vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));

	} else {
		report.s_variance_m_s = -1.0F;
		report.c_variance_rad = -1.0F;
	}

	report.fix_type = msg.status;

	report.vel_n_m_s = msg.ned_velocity[0];
	report.vel_e_m_s = msg.ned_velocity[1];
	report.vel_d_m_s = msg.ned_velocity[2];
	report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s +
	                       report.vel_e_m_s * report.vel_e_m_s +
	                       report.vel_d_m_s * report.vel_d_m_s);
	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
	report.vel_ned_valid = true;

	report.timestamp_time_relative = 0;
	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds

	report.satellites_used = msg.sats_used;

	// Using PDOP for HDOP and VDOP
	// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
	report.hdop = msg.pdop;
	report.vdop = msg.pdop;

	// Publish to a multi-topic
	int32_t gps_orb_instance;
	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
				 ORB_PRIO_HIGH);
}
<|endoftext|>


<commit_before>
#include "NavigationBar.h"

void NavigationBar::setup() {
	defaultPos_ = ofVec2f(450, 20);
	barPos_ = ofVec2f(0, 0);
	barSize_ = ofVec2f(1, 8);
	textPos_ = ofVec2f(defaultPos_);
	count_ = 0;
	textSwitch_ = 0;
	moveSwitch_ = 0;

	loadFiles();

	ofAddListener(ofEvents().update, this, &NavigationBar::moveText);
}

void NavigationBar::loadFiles() {
	loadXml();
	loadFont();

	mainBarImage_.load(mainBarFilename_);
	sideBarImage_.load(sideBarFilename_);
}

void NavigationBar::loadXml() {
	xml_.load("Select/NaviSettings.xml");
	xml_.pushTag("group");

	int textValue_ = xml_.getNumTags("Navi");
	for (int i = 0; i < textValue_; i++) {
		textList_.push_back(xml_.getValue("Navi", "none", i));
	}

	defaultPos_.x = xml_.getValue("TextStopPosX", 0);
	defaultPos_.y = xml_.getValue("TextStopPosY", 0);
	barPos_.x = xml_.getValue("BarPositionX", 0);
	barPos_.y = xml_.getValue("BarPositionY", 0);
	barSize_.x = xml_.getValue("BarSizeX", 0);
	barSize_.y = xml_.getValue("BarSizeY", 0);
	mainBarFilename_ = xml_.getValue("MainBar", "none");
	sideBarFilename_ = xml_.getValue("SideBar", "none");
	fontSize_ = xml_.getValue("FontSize", 0);
	fontScale_ = xml_.getValue("FontScale", 0);
	fontFilename_ = xml_.getValue("Font", "none");
	waitTime_ = xml_.getValue("TextWaitTime", 0);
	moveValue_ = xml_.getValue("TextMoveValue", 0);
	xml_.popTag();

}

void NavigationBar::loadFont() {
	font_.load(fontFilename_, fontSize_);
}

void NavigationBar::moveText(ofEventArgs &args) {
	switch (moveSwitch_) {
	case 0:
		frameOutTextStart();
		break;

	case 1:
		frameInTextStart();
		break;
	}
}

void NavigationBar::frameOutTextStart() {
	count_ += ofGetLastFrameTime() * 100;
	if (count_ >= waitTime_ * 60) {
		moveTextPos();
		if (textPos_.x <= -ofGetWindowWidth()) {
			frameOutTextEnd();
		}
	}
}

void NavigationBar::frameOutTextEnd() {
	textPos_.x = ofGetWindowWidth();
	count_ = 0;
	moveSwitch_ = 1;
	textSwitch_ += 1;
	if (textSwitch_ >= textList_.size()) { textSwitch_ = 0; }
}

void NavigationBar::frameInTextStart() {
	moveTextPos();
	if (textPos_.x <= ofVec2f(defaultPos_).x) {
		frameInTextEnd();
	}
}

void NavigationBar::frameInTextEnd() {
	textPos_ = ofVec2f(defaultPos_);
	moveSwitch_ = 0;
}

void NavigationBar::moveTextPos() {
	textPos_.x -= float(moveValue_)* (ofGetLastFrameTime() * 100);
}

void NavigationBar::draw() {
	mainBarImage_.draw(ofVec2f(barPos_), ofGetWindowWidth() / ofVec2f(barSize_).x, ofGetWindowHeight() / ofVec2f(barSize_).y);

	drawSwitchText();

	sideBarImage_.draw(ofVec2f(barPos_), ofGetWindowWidth() / ofVec2f(barSize_).x, ofGetWindowHeight() / ofVec2f(barSize_).y);
}

void NavigationBar::drawGui() {
	ImGui::Begin("NavigationBar");
	if (ImGui::Button("Save")) {
		saveXml();
	}
	if (ImGui::Button("Load")) {
		loadXml();
	}
	ImGui::DragFloat2("TextStopPos", defaultPos_.getPtr());
	ImGui::DragFloat("FontScale", &fontScale_, 1.0f, 500, 1000);
	ImGui::DragInt("TextWaitTime", &waitTime_, 1.0f, 1, 100);
	ImGui::DragFloat("TextMoveValue", &moveValue_, 1.0f, 15, 100);
	ImGui::DragFloat2("BarPosition", barPos_.getPtr());
	ImGui::DragFloat2("BarSize", barSize_.getPtr());
	ImGui::End();
}

void NavigationBar::saveXml() {
	xml_.pushTag("group");
	xml_.setValue("TextStopPosX", defaultPos_.x);
	xml_.setValue("TextStopPosY", defaultPos_.y);
	xml_.setValue("BarPositionX", barPos_.x);
	xml_.setValue("BarPositionY", barPos_.y);
	xml_.setValue("BarSizeX", barSize_.x);
	xml_.setValue("BarSizeY", barSize_.y);
	xml_.setValue("FontScale", fontScale_);
	xml_.setValue("TextWaitTime", waitTime_);
	xml_.setValue("TextMoveValue", moveValue_);
	xml_.popTag();
	xml_.saveFile("settings.xml");
}

void NavigationBar::drawSwitchText() {
	for (textSwitch_; textSwitch_ < textList_.size();) {
		ofSetColor(255, 255, 255);

		drawText(textList_[textSwitch_]);
		break;
	}
}

void NavigationBar::drawText(string& text) {
	ofPushMatrix();

	float fontWidth = font_.stringWidth(text);
	float fontHeight = font_.stringHeight(text);

	ofTranslate(textPos_);
	ofScale((ofGetWindowWidth() / (fontScale_)),
		(ofGetWindowHeight() / fontScale_), 1);
	font_.drawString(text, -fontWidth / 2, fontHeight);

	ofPopMatrix();
}
<commit_msg>update<commit_after>
#include "navigationBar.h"

void NavigationBar::setup() {
	defaultPos_ = ofVec2f(450, 20);
	barPos_ = ofVec2f(0, 0);
	barSize_ = ofVec2f(1, 8);
	textPos_ = ofVec2f(defaultPos_);
	count_ = 0;
	textSwitch_ = 0;
	moveSwitch_ = 0;

	loadFiles();

	ofAddListener(ofEvents().update, this, &NavigationBar::moveText);
}

void NavigationBar::loadFiles() {
	loadXml();
	loadFont();

	mainBarImage_.load(mainBarFilename_);
	sideBarImage_.load(sideBarFilename_);
}

void NavigationBar::loadXml() {
	xml_.load("Select/NaviSettings.xml");
	xml_.pushTag("group");

	int textValue_ = xml_.getNumTags("Navi");
	for (int i = 0; i < textValue_; i++) {
		textList_.push_back(xml_.getValue("Navi", "none", i));
	}

	defaultPos_.x = xml_.getValue("TextStopPosX", 0);
	defaultPos_.y = xml_.getValue("TextStopPosY", 0);
	barPos_.x = xml_.getValue("BarPositionX", 0);
	barPos_.y = xml_.getValue("BarPositionY", 0);
	barSize_.x = xml_.getValue("BarSizeX", 0);
	barSize_.y = xml_.getValue("BarSizeY", 0);
	mainBarFilename_ = xml_.getValue("MainBar", "none");
	sideBarFilename_ = xml_.getValue("SideBar", "none");
	fontSize_ = xml_.getValue("FontSize", 0);
	fontScale_ = xml_.getValue("FontScale", 0);
	fontFilename_ = xml_.getValue("Font", "none");
	waitTime_ = xml_.getValue("TextWaitTime", 0);
	moveValue_ = xml_.getValue("TextMoveValue", 0);
	xml_.popTag();

}

void NavigationBar::loadFont() {
	font_.load(fontFilename_, fontSize_);
}

void NavigationBar::moveText(ofEventArgs &args) {
	switch (moveSwitch_) {
	case 0:
		frameOutTextStart();
		break;

	case 1:
		frameInTextStart();
		break;
	}
}

void NavigationBar::frameOutTextStart() {
	count_ += ofGetLastFrameTime() * 100;
	if (count_ >= waitTime_ * 60) {
		moveTextPos();
		if (textPos_.x <= -ofGetWindowWidth()) {
			frameOutTextEnd();
		}
	}
}

void NavigationBar::frameOutTextEnd() {
	textPos_.x = ofGetWindowWidth();
	count_ = 0;
	moveSwitch_ = 1;
	textSwitch_ += 1;
	if (textSwitch_ >= textList_.size()) { textSwitch_ = 0; }
}

void NavigationBar::frameInTextStart() {
	moveTextPos();
	if (textPos_.x <= ofVec2f(defaultPos_).x) {
		frameInTextEnd();
	}
}

void NavigationBar::frameInTextEnd() {
	textPos_ = ofVec2f(defaultPos_);
	moveSwitch_ = 0;
}

void NavigationBar::moveTextPos() {
	textPos_.x -= float(moveValue_)* (ofGetLastFrameTime() * 100);
}

void NavigationBar::draw() {
	mainBarImage_.draw(ofVec2f(barPos_), ofGetWindowWidth() / ofVec2f(barSize_).x, ofGetWindowHeight() / ofVec2f(barSize_).y);

	drawSwitchText();

	sideBarImage_.draw(ofVec2f(barPos_), ofGetWindowWidth() / ofVec2f(barSize_).x, ofGetWindowHeight() / ofVec2f(barSize_).y);
}

void NavigationBar::drawGui() {
	ImGui::Begin("NavigationBar");
	if (ImGui::Button("Save")) {
		saveXml();
	}
	if (ImGui::Button("Load")) {
		loadXml();
	}
	ImGui::DragFloat2("TextStopPos", defaultPos_.getPtr());
	ImGui::DragFloat("FontScale", &fontScale_, 1.0f, 500, 1000);
	ImGui::DragInt("TextWaitTime", &waitTime_, 1.0f, 1, 100);
	ImGui::DragFloat("TextMoveValue", &moveValue_, 1.0f, 15, 100);
	ImGui::DragFloat2("BarPosition", barPos_.getPtr());
	ImGui::DragFloat2("BarSize", barSize_.getPtr());
	ImGui::End();
}

void NavigationBar::saveXml() {
	xml_.pushTag("group");
	xml_.setValue("TextStopPosX", defaultPos_.x);
	xml_.setValue("TextStopPosY", defaultPos_.y);
	xml_.setValue("BarPositionX", barPos_.x);
	xml_.setValue("BarPositionY", barPos_.y);
	xml_.setValue("BarSizeX", barSize_.x);
	xml_.setValue("BarSizeY", barSize_.y);
	xml_.setValue("FontScale", fontScale_);
	xml_.setValue("TextWaitTime", waitTime_);
	xml_.setValue("TextMoveValue", moveValue_);
	xml_.popTag();
	xml_.saveFile("settings.xml");
}

void NavigationBar::drawSwitchText() {
	for (textSwitch_; textSwitch_ < textList_.size();) {
		ofSetColor(255, 255, 255);

		drawText(textList_[textSwitch_]);
		break;
	}
}

void NavigationBar::drawText(string& text) {
	ofPushMatrix();

	float fontWidth = font_.stringWidth(text);
	float fontHeight = font_.stringHeight(text);

	ofTranslate(textPos_);
	ofScale((ofGetWindowWidth() / (fontScale_)),
		(ofGetWindowHeight() / fontScale_), 1);
	font_.drawString(text, -fontWidth / 2, fontHeight);

	ofPopMatrix();
}
<|endoftext|>


<commit_before>#define EIGEN_USE_MKL_ALL

#include <iostream> // for standard output
#include <vector> // vector objects
#include <eigen3/Eigen/Dense> // linear algebra library
#include <eigen3/unsupported/Eigen/KroneckerProduct> // provides tensor product
#include <eigen3/unsupported/Eigen/MatrixFunctions> // provides matrix functions

#include "constants.h"
#include "qp-math.h"

using namespace std;
using namespace Eigen;

// ---------------------------------------------------------------------------------------
// Matrix functions
// ---------------------------------------------------------------------------------------

// tensor product of many matrices
MatrixXcd tp(const initializer_list<MatrixXcd>& list) {
  MatrixXcd out = I1;
  for (MatrixXcd elem: list) {
    out = tp(out,elem);
  }
  return out;
}

// remove numerical artifacts from a matrix
MatrixXcd remove_artifacts(const MatrixXcd& A, const double threshold) {
  MatrixXcd B = MatrixXcd::Zero(A.rows(),A.cols());
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {
      B(m,n) += round(A(m,n).real()/threshold)*threshold;
      B(m,n) += round(A(m,n).imag()/threshold)*threshold*j;
    }
  }
  return B;
}

// get global phase of matrix
complex<double> get_phase(const MatrixXcd& A, const double threshold) {
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {
      if (abs(A(m,n)) > threshold) {
        const complex<double> phase = A(m,n)/abs(A(m,n));
        return phase;
      }
    }
  }
  return 1;
}

// ---------------------------------------------------------------------------------------
// Operator manipulation
// ---------------------------------------------------------------------------------------

// generate matrix B to act A on qbits qs_act out of qbits_new
MatrixXcd act(const MatrixXcd& A, const vector<uint>& qs_act, const uint qbits_new) {
  assert(A.rows() == A.cols()); // A should be square

  if (qs_act.size() == qbits_new) {
    bool do_nothing = true;
    for (uint i = 0; i < qbits_new; i++) {
      if (i != qs_act.at(i)) {
        do_nothing = false;
        break;
      }
    }
    if (do_nothing) return A;
  }

  // number of qbits A acted on
  const uint qbits_old = qs_act.size();
  assert(qbits_old == log2(A.rows()));

  // vector of qubits we are ignoring
  vector<uint> qs_ignore;
  for (uint i = 0; i < qbits_new; i++) {
    if (!in_vector(i,qs_act)) {
      qs_ignore.push_back(i);
    }
  }

  // initialize B (output) to the zero matrix
  MatrixXcd B = MatrixXcd::Zero(pow(2,qbits_new),pow(2,qbits_new));

  // loop over all entries A(m,n)
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {

      // get contribution of substates |m-><n-| to indices of B
      uint b_m = 0, b_n = 0;
      for (uint q = 0; q < qbits_old; q++) {
        if (qbit_state(q,qbits_old,m)) b_m += bit_int(qs_act.at(q),qbits_new);
        if (qbit_state(q,qbits_old,n)) b_n += bit_int(qs_act.at(q),qbits_new);
      }

      // loop over all elements of the form |ms><ns| in B
      for (uint s = 0; s < pow(2,qs_ignore.size()); s++) {
        uint b_out = b_m, b_in = b_n;
        for (uint q = 0; q < qs_ignore.size(); q++) {
          if (qbit_state(q,qs_ignore.size(),s)) {
            b_out += bit_int(qs_ignore.at(q),qbits_new);
            b_in += bit_int(qs_ignore.at(q),qbits_new);
          }
        }
        B(b_out,b_in) = A(m,n);
      }
    }
  }
  return B;
}

// perform a partial trace over qbits qs_trace
MatrixXcd ptrace(const MatrixXcd& A, const vector<uint>& qs_trace) {
  assert(A.rows() == A.cols()); // A should be square

  // number of qbits A acted on
  const uint qbits_old = log2(A.rows());
  const uint qbits_new = qbits_old - qs_trace.size();

  // vector of qubits we are keeping
  vector<uint> qs_keep;
  for (uint i = 0; i < qbits_old; i++) {
    if (!in_vector(i,qs_trace)) qs_keep.push_back(i);
  }
  assert(qbits_new == qs_keep.size());

  // initialize B (output) to the zero matrix
  MatrixXcd B = MatrixXcd::Zero(pow(2,qbits_new),pow(2,qbits_new));

  // loop over all entries B(m,n)
  for (uint m = 0; m < B.rows(); m++) {
    for (uint n = 0; n < B.cols(); n++) {

      // get contribution of substates |m-><n-| to indices of A
      uint a_m = 0, a_n = 0;
      for (uint q = 0; q < qbits_new; q++) {
        if (qbit_state(q,qbits_new,m)) a_m += bit_int(qs_keep.at(q),qbits_old);
        if (qbit_state(q,qbits_new,n)) a_n += bit_int(qs_keep.at(q),qbits_old);
      }

      // loop over all elements of the form |ms><ns| in A
      for (uint s = 0; s < pow(2,qs_trace.size()); s++) {
        uint a_out = a_m, a_in = a_n;
        for (uint q = 0; q < qs_trace.size(); q++) {
          if (qbit_state(q,qs_trace.size(),s)) {
            a_out += bit_int(qs_trace.at(q),qbits_old);
            a_in += bit_int(qs_trace.at(q),qbits_old);
          }
        }
        B(m,n) += A(a_out,a_in);
      }
    }
  }
  return B;
}

// ---------------------------------------------------------------------------------------
// Gate decomposition and fidelity
// ---------------------------------------------------------------------------------------

// returns basis element p for an operator acting on a system with N spins
MatrixXcd U_basis_element(const uint p, const uint N) {
  const vector<MatrixXcd> spins = {I2, sx, sy, sz};
  MatrixXcd b_p = I1;
  for (uint n = 0; n < N; n++) {
    b_p = tp(b_p,spins.at(int_bit(p,2*n)+2*int_bit(p,2*n+1)));
  }
  return b_p;
}

// returns element p of a basis for operators acting on a system with N qubits
string U_basis_element_text(const uint p, const uint N) {
  const vector<char> spins = {'I','X','Y','Z'};
  stringstream stream;
  for (uint n = 0; n < N; n++) {
    stream << spins.at(int_bit(p,2*n)+2*int_bit(p,2*n+1));
  }
  return stream.str();
}

// returns matrix whose columns are basis Hamiltonians for a system of N spins
MatrixXcd U_basis_matrix(const uint N) {
  MatrixXcd out = MatrixXcd::Zero(pow(4,N),pow(4,N));
  for (int p = 0; p < pow(4,N); p++) {
    out.col(p) = flatten(U_basis_element(p,N));
  }
  return out;
}

// decompose an operator into its basis elements
VectorXcd U_decompose(const MatrixXcd& U, const bool fast) {
  const uint N = log2(U.rows());
  if (fast) return U_basis_matrix(N).householderQr().solve(flatten(U));
  else return U_basis_matrix(N).fullPivLu().solve(flatten(U));
}

// compute mean fidelity of gate U with respect to G, i.e. how well U approximates G
double gate_fidelity(const MatrixXcd&  U, const MatrixXcd& G) {
  assert(U.size() == G.size());

  const MatrixXcd Uc = remove_phase(U);
  const MatrixXcd Gc = remove_phase(G);

  const uint D = U.rows();
  const uint N = log2(D);
  double fidelity = D*D;

  for (uint i = 0; i < D*D; i++) {
    const MatrixXcd B_i = U_basis_element(i,N);
    fidelity += real(trace(Uc*B_i.adjoint()*Uc.adjoint()*Gc*B_i*Gc.adjoint()));
  }
  fidelity /= D*D*(D+1);

  return fidelity;
}

// compute mean fidelity of propagator acting on system_qubits
double gate_fidelity(const MatrixXcd& U, const MatrixXcd& G, const vector<uint>& nuclei) {
  assert(U.size() == G.size());

  const uint spins = log2(G.rows());
  vector<uint> system_qubits = {0};
  for (uint n: nuclei) system_qubits.push_back(n+1);

  const MatrixXcd U_err = G.adjoint() * U;
  const VectorXcd H_err_vec = U_decompose(j*log(U_err));

  MatrixXcd H_env = MatrixXcd::Zero(pow(2,spins),pow(2,spins));
  for (uint h = 0; h < H_err_vec.size(); h++) {
    bool add_this_element = true;
    for (uint q: system_qubits) {
      if (int_bit(h,2*q) + int_bit(h,2*q+1)) {
        add_this_element = false;
        break;
      }
    }
    if (add_this_element) H_env += H_err_vec(h)*U_basis_element(h,spins);
  }
  return gate_fidelity(U*exp(j*H_env),G);
}

// ---------------------------------------------------------------------------------------
// Matrix vectors
// ---------------------------------------------------------------------------------------

mvec operator*(const MatrixXcd& G, const mvec& v) {
  vector<MatrixXcd> out;
  for (uint i = 0; i < v.size(); i++) {
    out.push_back(G*v.at(i));
  }
  return mvec(out);
}

// ---------------------------------------------------------------------------------------
// Printing methods
// ---------------------------------------------------------------------------------------

// print operator in human-readable form
void U_print(const MatrixXcd& U, const double threshold) {
  const int N = log2(U.rows());
  const VectorXcd hs = remove_artifacts(U_decompose(U), threshold);
  for (int p = 0; p < pow(4,N); p++) {
    if (abs(hs(p)) != 0) {
      cout << U_basis_element_text(p,N) << ": " << hs(p) << endl;
    }
  }
  cout << endl;
}

// print state vector in human readable form
void state_print(const MatrixXcd& psi) {
  const uint N = psi.size();
  const uint qbits = log2(N);
  for (uint n = 0; n < N; n++) {
    if (abs(psi(n)) != 0) {
      cout << "|";
      for (uint q = 0; q < qbits; q++) {
        cout << (qbit_state(q,qbits,n)?"d":"u");
      }
      cout << "> " << psi(n) << endl;
    }
  }
  cout << endl;
}

// print matrix in human readable form
void matrix_print(const MatrixXcd& M) {
  const uint qbits = log2(M.rows());
  for (uint m = 0; m < M.rows(); m++) {
    for (uint n = 0; n < M.cols(); n++) {
      if (abs(M(m,n)) != 0) {
        cout << "|";
        for (uint q = 0; q < qbits; q++) {
          cout << (qbit_state(q,qbits,m)?"d":"u");
        }
        cout << "><";
        for (uint q = 0; q < qbits; q++) {
          cout << (qbit_state(q,qbits,n)?"d":"u");
        }
        cout << "| " << M(m,n) << endl;
      }
    }
  }
  cout << endl;
}
<commit_msg>changed int to uint<commit_after>#define EIGEN_USE_MKL_ALL

#include <iostream> // for standard output
#include <vector> // vector objects
#include <eigen3/Eigen/Dense> // linear algebra library
#include <eigen3/unsupported/Eigen/KroneckerProduct> // provides tensor product
#include <eigen3/unsupported/Eigen/MatrixFunctions> // provides matrix functions

#include "constants.h"
#include "qp-math.h"

using namespace std;
using namespace Eigen;

// ---------------------------------------------------------------------------------------
// Matrix functions
// ---------------------------------------------------------------------------------------

// tensor product of many matrices
MatrixXcd tp(const initializer_list<MatrixXcd>& list) {
  MatrixXcd out = I1;
  for (MatrixXcd elem: list) {
    out = tp(out,elem);
  }
  return out;
}

// remove numerical artifacts from a matrix
MatrixXcd remove_artifacts(const MatrixXcd& A, const double threshold) {
  MatrixXcd B = MatrixXcd::Zero(A.rows(),A.cols());
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {
      B(m,n) += round(A(m,n).real()/threshold)*threshold;
      B(m,n) += round(A(m,n).imag()/threshold)*threshold*j;
    }
  }
  return B;
}

// get global phase of matrix
complex<double> get_phase(const MatrixXcd& A, const double threshold) {
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {
      if (abs(A(m,n)) > threshold) {
        const complex<double> phase = A(m,n)/abs(A(m,n));
        return phase;
      }
    }
  }
  return 1;
}

// ---------------------------------------------------------------------------------------
// Operator manipulation
// ---------------------------------------------------------------------------------------

// generate matrix B to act A on qbits qs_act out of qbits_new
MatrixXcd act(const MatrixXcd& A, const vector<uint>& qs_act, const uint qbits_new) {
  assert(A.rows() == A.cols()); // A should be square

  if (qs_act.size() == qbits_new) {
    bool do_nothing = true;
    for (uint i = 0; i < qbits_new; i++) {
      if (i != qs_act.at(i)) {
        do_nothing = false;
        break;
      }
    }
    if (do_nothing) return A;
  }

  // number of qbits A acted on
  const uint qbits_old = qs_act.size();
  assert(qbits_old == log2(A.rows()));

  // vector of qubits we are ignoring
  vector<uint> qs_ignore;
  for (uint i = 0; i < qbits_new; i++) {
    if (!in_vector(i,qs_act)) {
      qs_ignore.push_back(i);
    }
  }

  // initialize B (output) to the zero matrix
  MatrixXcd B = MatrixXcd::Zero(pow(2,qbits_new),pow(2,qbits_new));

  // loop over all entries A(m,n)
  for (uint m = 0; m < A.rows(); m++) {
    for (uint n = 0; n < A.cols(); n++) {

      // get contribution of substates |m-><n-| to indices of B
      uint b_m = 0, b_n = 0;
      for (uint q = 0; q < qbits_old; q++) {
        if (qbit_state(q,qbits_old,m)) b_m += bit_int(qs_act.at(q),qbits_new);
        if (qbit_state(q,qbits_old,n)) b_n += bit_int(qs_act.at(q),qbits_new);
      }

      // loop over all elements of the form |ms><ns| in B
      for (uint s = 0; s < pow(2,qs_ignore.size()); s++) {
        uint b_out = b_m, b_in = b_n;
        for (uint q = 0; q < qs_ignore.size(); q++) {
          if (qbit_state(q,qs_ignore.size(),s)) {
            b_out += bit_int(qs_ignore.at(q),qbits_new);
            b_in += bit_int(qs_ignore.at(q),qbits_new);
          }
        }
        B(b_out,b_in) = A(m,n);
      }
    }
  }
  return B;
}

// perform a partial trace over qbits qs_trace
MatrixXcd ptrace(const MatrixXcd& A, const vector<uint>& qs_trace) {
  assert(A.rows() == A.cols()); // A should be square

  // number of qbits A acted on
  const uint qbits_old = log2(A.rows());
  const uint qbits_new = qbits_old - qs_trace.size();

  // vector of qubits we are keeping
  vector<uint> qs_keep;
  for (uint i = 0; i < qbits_old; i++) {
    if (!in_vector(i,qs_trace)) qs_keep.push_back(i);
  }
  assert(qbits_new == qs_keep.size());

  // initialize B (output) to the zero matrix
  MatrixXcd B = MatrixXcd::Zero(pow(2,qbits_new),pow(2,qbits_new));

  // loop over all entries B(m,n)
  for (uint m = 0; m < B.rows(); m++) {
    for (uint n = 0; n < B.cols(); n++) {

      // get contribution of substates |m-><n-| to indices of A
      uint a_m = 0, a_n = 0;
      for (uint q = 0; q < qbits_new; q++) {
        if (qbit_state(q,qbits_new,m)) a_m += bit_int(qs_keep.at(q),qbits_old);
        if (qbit_state(q,qbits_new,n)) a_n += bit_int(qs_keep.at(q),qbits_old);
      }

      // loop over all elements of the form |ms><ns| in A
      for (uint s = 0; s < pow(2,qs_trace.size()); s++) {
        uint a_out = a_m, a_in = a_n;
        for (uint q = 0; q < qs_trace.size(); q++) {
          if (qbit_state(q,qs_trace.size(),s)) {
            a_out += bit_int(qs_trace.at(q),qbits_old);
            a_in += bit_int(qs_trace.at(q),qbits_old);
          }
        }
        B(m,n) += A(a_out,a_in);
      }
    }
  }
  return B;
}

// ---------------------------------------------------------------------------------------
// Gate decomposition and fidelity
// ---------------------------------------------------------------------------------------

// returns basis element p for an operator acting on a system with N spins
MatrixXcd U_basis_element(const uint p, const uint N) {
  const vector<MatrixXcd> spins = {I2, sx, sy, sz};
  MatrixXcd b_p = I1;
  for (uint n = 0; n < N; n++) {
    b_p = tp(b_p,spins.at(int_bit(p,2*n)+2*int_bit(p,2*n+1)));
  }
  return b_p;
}

// returns element p of a basis for operators acting on a system with N qubits
string U_basis_element_text(const uint p, const uint N) {
  const vector<char> spins = {'I','X','Y','Z'};
  stringstream stream;
  for (uint n = 0; n < N; n++) {
    stream << spins.at(int_bit(p,2*n)+2*int_bit(p,2*n+1));
  }
  return stream.str();
}

// returns matrix whose columns are basis Hamiltonians for a system of N spins
MatrixXcd U_basis_matrix(const uint N) {
  MatrixXcd out = MatrixXcd::Zero(pow(4,N),pow(4,N));
  for (uint p = 0; p < pow(4,N); p++) {
    out.col(p) = flatten(U_basis_element(p,N));
  }
  return out;
}

// decompose an operator into its basis elements
VectorXcd U_decompose(const MatrixXcd& U, const bool fast) {
  const uint N = log2(U.rows());
  if (fast) return U_basis_matrix(N).householderQr().solve(flatten(U));
  else return U_basis_matrix(N).fullPivLu().solve(flatten(U));
}

// compute mean fidelity of gate U with respect to G, i.e. how well U approximates G
double gate_fidelity(const MatrixXcd&  U, const MatrixXcd& G) {
  assert(U.size() == G.size());

  const MatrixXcd Uc = remove_phase(U);
  const MatrixXcd Gc = remove_phase(G);

  const uint D = U.rows();
  const uint N = log2(D);
  double fidelity = D*D;

  for (uint i = 0; i < D*D; i++) {
    const MatrixXcd B_i = U_basis_element(i,N);
    fidelity += real(trace(Uc*B_i.adjoint()*Uc.adjoint()*Gc*B_i*Gc.adjoint()));
  }
  fidelity /= D*D*(D+1);

  return fidelity;
}

// compute mean fidelity of propagator acting on system_qubits
double gate_fidelity(const MatrixXcd& U, const MatrixXcd& G, const vector<uint>& nuclei) {
  assert(U.size() == G.size());

  const uint spins = log2(G.rows());
  vector<uint> system_qubits = {0};
  for (uint n: nuclei) system_qubits.push_back(n+1);

  const MatrixXcd U_err = G.adjoint() * U;
  const VectorXcd H_err_vec = U_decompose(j*log(U_err));

  MatrixXcd H_env = MatrixXcd::Zero(pow(2,spins),pow(2,spins));
  for (uint h = 0; h < H_err_vec.size(); h++) {
    bool add_this_element = true;
    for (uint q: system_qubits) {
      if (int_bit(h,2*q) + int_bit(h,2*q+1)) {
        add_this_element = false;
        break;
      }
    }
    if (add_this_element) H_env += H_err_vec(h)*U_basis_element(h,spins);
  }
  return gate_fidelity(U*exp(j*H_env),G);
}

// ---------------------------------------------------------------------------------------
// Matrix vectors
// ---------------------------------------------------------------------------------------

mvec operator*(const MatrixXcd& G, const mvec& v) {
  vector<MatrixXcd> out;
  for (uint i = 0; i < v.size(); i++) {
    out.push_back(G*v.at(i));
  }
  return mvec(out);
}

// ---------------------------------------------------------------------------------------
// Printing methods
// ---------------------------------------------------------------------------------------

// print operator in human-readable form
void U_print(const MatrixXcd& U, const double threshold) {
  const int N = log2(U.rows());
  const VectorXcd hs = remove_artifacts(U_decompose(U), threshold);
  for (int p = 0; p < pow(4,N); p++) {
    if (abs(hs(p)) != 0) {
      cout << U_basis_element_text(p,N) << ": " << hs(p) << endl;
    }
  }
  cout << endl;
}

// print state vector in human readable form
void state_print(const MatrixXcd& psi) {
  const uint N = psi.size();
  const uint qbits = log2(N);
  for (uint n = 0; n < N; n++) {
    if (abs(psi(n)) != 0) {
      cout << "|";
      for (uint q = 0; q < qbits; q++) {
        cout << (qbit_state(q,qbits,n)?"d":"u");
      }
      cout << "> " << psi(n) << endl;
    }
  }
  cout << endl;
}

// print matrix in human readable form
void matrix_print(const MatrixXcd& M) {
  const uint qbits = log2(M.rows());
  for (uint m = 0; m < M.rows(); m++) {
    for (uint n = 0; n < M.cols(); n++) {
      if (abs(M(m,n)) != 0) {
        cout << "|";
        for (uint q = 0; q < qbits; q++) {
          cout << (qbit_state(q,qbits,m)?"d":"u");
        }
        cout << "><";
        for (uint q = 0; q < qbits; q++) {
          cout << (qbit_state(q,qbits,n)?"d":"u");
        }
        cout << "| " << M(m,n) << endl;
      }
    }
  }
  cout << endl;
}
<|endoftext|>


<commit_before>#include "command.hh"
#include "store-api.hh"
#include "references.hh"

using namespace nix;

struct CmdMakeContentAddressable : StorePathsCommand
{
    CmdMakeContentAddressable()
    {
        realiseMode = Build;
    }

    std::string name() override
    {
        return "make-content-addressable";
    }

    std::string description() override
    {
        return "rewrite a path or closure to content-addressable form";
    }

    Examples examples() override
    {
        return {
            Example{
                "To create a content-addressable representation of GNU Hello (but not its dependencies):",
                "nix make-content-addressable nixpkgs.hello"
            },
            Example{
                "To compute a content-addressable representation of the current NixOS system closure:",
                "nix make-content-addressable -r /run/current-system"
            },
        };
    }
    void run(ref<Store> store, Paths storePaths) override
    {
        auto paths = store->topoSortPaths(PathSet(storePaths.begin(), storePaths.end()));

        paths.reverse();

        std::map<Path, Path> remappings;

        for (auto & path : paths) {
            auto oldInfo = store->queryPathInfo(path);
            auto oldHashPart = storePathToHash(path);
            auto name = storePathToName(path);

            StringSink sink;
            store->narFromPath(path, sink);

            StringMap rewrites;

            ValidPathInfo info;
            for (auto & ref : oldInfo->references) {
                if (ref == path)
                    info.references.insert("self");
                else {
                    auto replacement = get(remappings, ref, ref);
                    // FIXME: warn about unremapped paths?
                    info.references.insert(replacement);
                    if (replacement != ref)
                        rewrites[storePathToHash(ref)] = storePathToHash(replacement);
                }
            }

            *sink.s = rewriteStrings(*sink.s, rewrites);

            HashModuloSink hashModuloSink(htSHA256, oldHashPart);
            hashModuloSink((unsigned char *) sink.s->data(), sink.s->size());

            info.narHash = hashModuloSink.finish().first;
            info.narSize = sink.s->size();
            replaceInSet(info.references, path, std::string("self"));
            info.path = store->makeFixedOutputPath(true, info.narHash, name, info.references);
            replaceInSet(info.references, std::string("self"), info.path);
            info.ca = makeFixedOutputCA(true, info.narHash);

            printError("rewrote '%s' to '%s'", path, info.path);

            auto source = sinkToSource([&](Sink & nextSink) {
                RewritingSink rsink2(oldHashPart, storePathToHash(info.path), nextSink);
                rsink2((unsigned char *) sink.s->data(), sink.s->size());
                rsink2.flush();
            });

            store->addToStore(info, *source);

            remappings[path] = info.path;
        }
    }
};

static RegisterCommand r1(make_ref<CmdMakeContentAddressable>());
<commit_msg>Revert "nix make-content-addressable: Add examples"<commit_after>#include "command.hh"
#include "store-api.hh"
#include "references.hh"

using namespace nix;

struct CmdMakeContentAddressable : StorePathsCommand
{
    CmdMakeContentAddressable()
    {
        realiseMode = Build;
    }

    std::string name() override
    {
        return "make-content-addressable";
    }

    std::string description() override
    {
        return "test";
    }

    void run(ref<Store> store, Paths storePaths) override
    {
        auto paths = store->topoSortPaths(PathSet(storePaths.begin(), storePaths.end()));

        paths.reverse();

        std::map<Path, Path> remappings;

        for (auto & path : paths) {
            auto oldInfo = store->queryPathInfo(path);
            auto oldHashPart = storePathToHash(path);
            auto name = storePathToName(path);

            StringSink sink;
            store->narFromPath(path, sink);

            StringMap rewrites;

            ValidPathInfo info;
            for (auto & ref : oldInfo->references) {
                if (ref == path)
                    info.references.insert("self");
                else {
                    auto replacement = get(remappings, ref, ref);
                    // FIXME: warn about unremapped paths?
                    info.references.insert(replacement);
                    if (replacement != ref)
                        rewrites[storePathToHash(ref)] = storePathToHash(replacement);
                }
            }

            *sink.s = rewriteStrings(*sink.s, rewrites);

            HashModuloSink hashModuloSink(htSHA256, oldHashPart);
            hashModuloSink((unsigned char *) sink.s->data(), sink.s->size());

            info.narHash = hashModuloSink.finish().first;
            info.narSize = sink.s->size();
            replaceInSet(info.references, path, std::string("self"));
            info.path = store->makeFixedOutputPath(true, info.narHash, name, info.references);
            replaceInSet(info.references, std::string("self"), info.path);
            info.ca = makeFixedOutputCA(true, info.narHash);

            printError("rewrote '%s' to '%s'", path, info.path);

            auto source = sinkToSource([&](Sink & nextSink) {
                RewritingSink rsink2(oldHashPart, storePathToHash(info.path), nextSink);
                rsink2((unsigned char *) sink.s->data(), sink.s->size());
                rsink2.flush();
            });

            store->addToStore(info, *source);

            remappings[path] = info.path;
        }
    }
};

static RegisterCommand r1(make_ref<CmdMakeContentAddressable>());
<|endoftext|>


<commit_before><commit_msg>adjustment implementation<commit_after><|endoftext|>


<commit_before>// (C) Copyright 2017, Google Inc.
// 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 <iostream>             // for cout

#include "include_gunit.h"
#include "scanutils.h"

namespace {

class ScanutilsTest : public ::testing::Test {
 protected:
  void SetUp() override {
  }
};

TEST_F(ScanutilsTest, DoesScanf) {
  // This test verifies that tfscanf does Scanf the same as stdio fscanf.
  // There are probably a gazillion more test cases that could be added, but
  // these brought the tesseract and unittest test results in line.
  std::string filename = file::JoinPath(TESTDATA_DIR, "scanftest.txt");
  FILE* fp1 = fopen(filename.c_str(), "r");
  if (fp1 == nullptr) {
    std::cout << "Failed to open file " << filename << '\n';
    GTEST_SKIP();
    return;
  }
  FILE* fp2 = fopen(filename.c_str(), "r");
  if (fp2 == nullptr) {
    std::cout << "Failed to open file " << filename << '\n';
    GTEST_SKIP();
    fclose(fp1);
    return;
  }
  // The file contains this:
  // 42.5 17 0.001000 -0.001000
  // 0 1 123 -123 0x100
  // abcdefghijklmnopqrstuvwxyz
  // abcdefghijklmnopqrstuvwxyz
  // MF 25 6.25e-2 0.5e5 -1e+4
  // 42 MF 25 6.25e-2 0.5
  // 24
  const int kNumFloats = 4;
  float f1[kNumFloats], f2[kNumFloats];
  int r1 = fscanf(fp1, "%f %f %f %f", &f1[0], &f1[1], &f1[2], &f1[3]);
  int r2 = tfscanf(fp2, "%f %f %f %f", &f2[0], &f2[1], &f2[2], &f2[3]);
  EXPECT_EQ(r1, kNumFloats);
  EXPECT_EQ(r2, kNumFloats);
  if (r1 == r2) {
    for (int i = 0; i < r1; ++i) {
      EXPECT_FLOAT_EQ(f1[i], f2[i]);
    }
  }
  const int kNumInts = 5;
  int i1[kNumInts], i2[kNumInts];
  r1 = fscanf(fp1, "%d %d %d %d %i", &i1[0], &i1[1], &i1[2], &i1[3], &i1[4]);
  r2 = tfscanf(fp2, "%d %d %d %d %i", &i2[0], &i2[1], &i2[2], &i2[3], &i2[4]);
  EXPECT_EQ(r1, kNumInts);
  EXPECT_EQ(r2, kNumInts);
  if (r1 == r2) {
    for (int i = 0; i < kNumInts; ++i) {
      EXPECT_EQ(i1[i], i2[i]);
    }
  }
  const int kStrLen = 1024;
  char s1[kStrLen];
  char s2[kStrLen];
  r1 = fscanf(fp1, "%1023s", s1);
  r2 = tfscanf(fp2, "%1023s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(26, strlen(s2));
  r1 = fscanf(fp1, "%20s", s1);
  r2 = tfscanf(fp2, "%20s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(20, strlen(s2));
  // Now read the rest of the alphabet.
  r1 = fscanf(fp1, "%s", s1);
  r2 = tfscanf(fp2, "%s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(6, strlen(s2));
  r1 = fscanf(fp1, "%s", s1);
  r2 = tfscanf(fp2, "%s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(2, strlen(s2));
  r1 = fscanf(fp1, "%f %f %f %f", &f1[0], &f1[1], &f1[2], &f1[3]);
  r2 = tfscanf(fp2, "%f %f %f %f", &f2[0], &f2[1], &f2[2], &f2[3]);
  EXPECT_EQ(r1, r2);
  for (int i = 0; i < kNumFloats; ++i) EXPECT_FLOAT_EQ(f1[i], f2[i]);
  // Test the * for field suppression.
  r1 = fscanf(fp1, "%d %*s %*d %*f %*f", &i1[0]);
  r2 = tfscanf(fp2, "%d %*s %*d %*f %*f", &i2[0]);
  EXPECT_EQ(r1, r2);
  EXPECT_EQ(i1[0], i2[0]);
  // We should still see the next value and no phantoms.
  r1 = fscanf(fp1, "%d %s", &i1[0], s1);
  r2 = tfscanf(fp2, "%d %s", &i2[0], s2);
  EXPECT_EQ(r1, r2);
  EXPECT_EQ(1, r2);
  EXPECT_EQ(i1[0], i2[0]);
  fclose(fp2);
  fclose(fp1);
}

}  // namespace
<commit_msg>unittest: Add missing precision specifiers (CID 1402752)<commit_after>// (C) Copyright 2017, Google Inc.
// 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 <iostream>             // for cout

#include "include_gunit.h"
#include "scanutils.h"

namespace {

class ScanutilsTest : public ::testing::Test {
 protected:
  void SetUp() override {
  }
};

TEST_F(ScanutilsTest, DoesScanf) {
  // This test verifies that tfscanf does Scanf the same as stdio fscanf.
  // There are probably a gazillion more test cases that could be added, but
  // these brought the tesseract and unittest test results in line.
  std::string filename = file::JoinPath(TESTDATA_DIR, "scanftest.txt");
  FILE* fp1 = fopen(filename.c_str(), "r");
  if (fp1 == nullptr) {
    std::cout << "Failed to open file " << filename << '\n';
    GTEST_SKIP();
    return;
  }
  FILE* fp2 = fopen(filename.c_str(), "r");
  if (fp2 == nullptr) {
    std::cout << "Failed to open file " << filename << '\n';
    GTEST_SKIP();
    fclose(fp1);
    return;
  }
  // The file contains this:
  // 42.5 17 0.001000 -0.001000
  // 0 1 123 -123 0x100
  // abcdefghijklmnopqrstuvwxyz
  // abcdefghijklmnopqrstuvwxyz
  // MF 25 6.25e-2 0.5e5 -1e+4
  // 42 MF 25 6.25e-2 0.5
  // 24
  const int kNumFloats = 4;
  float f1[kNumFloats], f2[kNumFloats];
  int r1 = fscanf(fp1, "%f %f %f %f", &f1[0], &f1[1], &f1[2], &f1[3]);
  int r2 = tfscanf(fp2, "%f %f %f %f", &f2[0], &f2[1], &f2[2], &f2[3]);
  EXPECT_EQ(r1, kNumFloats);
  EXPECT_EQ(r2, kNumFloats);
  if (r1 == r2) {
    for (int i = 0; i < r1; ++i) {
      EXPECT_FLOAT_EQ(f1[i], f2[i]);
    }
  }
  const int kNumInts = 5;
  int i1[kNumInts], i2[kNumInts];
  r1 = fscanf(fp1, "%d %d %d %d %i", &i1[0], &i1[1], &i1[2], &i1[3], &i1[4]);
  r2 = tfscanf(fp2, "%d %d %d %d %i", &i2[0], &i2[1], &i2[2], &i2[3], &i2[4]);
  EXPECT_EQ(r1, kNumInts);
  EXPECT_EQ(r2, kNumInts);
  if (r1 == r2) {
    for (int i = 0; i < kNumInts; ++i) {
      EXPECT_EQ(i1[i], i2[i]);
    }
  }
  const int kStrLen = 1024;
  char s1[kStrLen];
  char s2[kStrLen];
  r1 = fscanf(fp1, "%1023s", s1);
  r2 = tfscanf(fp2, "%1023s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(26, strlen(s2));
  r1 = fscanf(fp1, "%20s", s1);
  r2 = tfscanf(fp2, "%20s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(20, strlen(s2));
  // Now read the rest of the alphabet.
  r1 = fscanf(fp1, "%1023s", s1);
  r2 = tfscanf(fp2, "%1023s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(6, strlen(s2));
  r1 = fscanf(fp1, "%1023s", s1);
  r2 = tfscanf(fp2, "%1023s", s2);
  EXPECT_EQ(r1, r2);
  EXPECT_STREQ(s1, s2);
  EXPECT_EQ(2, strlen(s2));
  r1 = fscanf(fp1, "%f %f %f %f", &f1[0], &f1[1], &f1[2], &f1[3]);
  r2 = tfscanf(fp2, "%f %f %f %f", &f2[0], &f2[1], &f2[2], &f2[3]);
  EXPECT_EQ(r1, r2);
  for (int i = 0; i < kNumFloats; ++i) EXPECT_FLOAT_EQ(f1[i], f2[i]);
  // Test the * for field suppression.
  r1 = fscanf(fp1, "%d %*s %*d %*f %*f", &i1[0]);
  r2 = tfscanf(fp2, "%d %*s %*d %*f %*f", &i2[0]);
  EXPECT_EQ(r1, r2);
  EXPECT_EQ(i1[0], i2[0]);
  // We should still see the next value and no phantoms.
  r1 = fscanf(fp1, "%d %1023s", &i1[0], s1);
  r2 = tfscanf(fp2, "%d %1023s", &i2[0], s2);
  EXPECT_EQ(r1, r2);
  EXPECT_EQ(1, r2);
  EXPECT_EQ(i1[0], i2[0]);
  fclose(fp2);
  fclose(fp1);
}

}  // namespace
<|endoftext|>


<commit_before><commit_msg>vcldemo: include new text demo<commit_after><|endoftext|>