text:
#include <chrono>
#include <numeric>
#include <random>
#include <gtest/gtest.h>
#include "sock/networkmock.hpp"
#include "sock/networkcommon.hpp"
#include <boost/asio.hpp>
#include <boost/asio/ssl.hpp>
#include <qi/messaging/sock/accept.hpp>
#include "src/messaging/tcpmessagesocket.hpp"
#include "src/messaging/transportserver.hpp"
#include "tests/qi/testutils/testutils.hpp"
#include "ka/scoped.hpp"

static const qi::MilliSeconds defaultTimeout{ 2000 };

////////////////////////////////////////////////////////////////////////////////
// Connected tests
////////////////////////////////////////////////////////////////////////////////

namespace mock
{
  template<typename H> // NetHandler H
  void readHeader(N::_mutable_buffer_sequence buf, H h, qi::uint32_t magic = qi::Message::Header::magicCookie, qi::uint32_t size = 10u, N::error_code_type error = {})
  {
    qi::Message::Header header;
    header.magic = magic;
    header.size = size;
    assert(std::distance(buf.begin, buf.end) >= static_cast<std::ptrdiff_t>(sizeof(header)));
    auto* p = reinterpret_cast<decltype(buf.begin)>(&header);
    std::copy(p, p + sizeof(header), buf.begin);
    h(error, sizeof(header));
  }

  /// A read handler that alternatively reads the message header, then the message
  /// data.
  struct AsyncReadNextLayerHeaderThenData
  {
    qi::uint32_t _magic = qi::Message::Header::magicCookie;
    qi::uint32_t _headerSize = 10u;
    N::error_code_type _headerError = {};
    N::error_code_type _dataError = {};
    int _callCount = 0;
    void operator()(N::ssl_socket_type::next_layer_type&, N::_mutable_buffer_sequence buf, N::_anyTransferHandler h)
    {
      ++_callCount;
      if (_callCount % 2 == 1)
      {
        readHeader(buf, h, _magic, _headerSize, _headerError);
      }
      else
      {
        // read data
        h(_dataError, 0);
      }
    }
  };
} // namespace mock

qi::Message makeMessage(const qi::MessageAddress& address)
{
  using namespace qi;
  Message msg{Message::Type_Call, address};
  Buffer bufSend;
  {
    std::vector<int> data(10000);
    std::iota(data.begin(), data.end(), 42);
    bufSend.write(&data[0], data.size() * sizeof(data[0]));
    msg.setBuffer(bufSend);
  }
  return msg;
}

template<typename N>
struct NoMoreMessage
{
  boost::optional<qi::Message*> operator()(qi::sock::ErrorCode<N> e, qi::Message*) {
    if (e) throw std::runtime_error("error sending message");
    return boost::optional<qi::Message*>{};
  }
};

// TODO: Replace this by Message == when the associated patch is available.
bool messageEqual(const qi::Message& m0, const qi::Message& m1)
{
  if (m0.address() != m1.address()) return false;
  if (m0.buffer().size() != m1.buffer().size()) return false;
  auto b0 = static_cast<const char*>(m0.buffer().data());
  return std::equal(b0, b0 + m0.buffer().size(), (char*)m1.buffer().data());
}

template<typename... Args>
struct DisconnectSignal
{
  qi::SignalLink link = 0;
  qi::Signal<Args...>* signal = nullptr;

  DisconnectSignal(qi::SignalLink link, qi::Signal<Args...>* signal)
    : link{link}
    , signal{signal}
  {}

  void operator()()
  {
    if (signal && link != 0)
      signal->disconnect(link);
  }
};

struct SignalPromises
{
  qi::Promise<void> _connectedReceived;
  qi::Promise<std::string> _disconnectedReceived;

  qi::Future<void> connectedReceived()
  {
    return _connectedReceived.future();
  }
  qi::Future<std::string> disconnectedReceived()
  {
    return _disconnectedReceived.future();
  }
};

struct SignalConnection
{
  SignalPromises promises;
  std::function<void()> disconnect = ka::constant_function();

  ~SignalConnection()
  {
    disconnect();
  }
};

using SignalConnectionPtr = std::unique_ptr<SignalConnection>;

template<typename SocketPtr>
SignalConnectionPtr connectSignals(const SocketPtr& socket)
{
  SignalConnectionPtr c { new SignalConnection };
  const auto cptr = c.get();

  const qi::SignalLink linkConnected =
      socket->connected.connect([=]() { cptr->promises._connectedReceived.setValue(0); });
  DisconnectSignal<> disconnectConnected{ linkConnected, &socket->connected };

  const qi::SignalLink linkDisconnected = socket->disconnected.connect(
      [=](const std::string& msg) { cptr->promises._disconnectedReceived.setValue(msg); });
  DisconnectSignal<std::string> disconnectDisconnected{ linkDisconnected, &socket->disconnected };

  const auto lifetimeTransfo = ka::data_bound_transfo(socket);
  c->disconnect = lifetimeTransfo([=]() mutable {
    disconnectConnected();
    disconnectDisconnected();
  });

  return c;
}

namespace {

  struct SchemeTcp
  {
  // Function<std::string ()>:
    std::string operator()() const { return "tcp"; }
  };

  struct SchemeTcpSSL
  {
  // Function<std::string ()>:
    std::string operator()() const { return "tcps"; }
  };

  template<class SchemeType>
  class NetMessageSocket : public ::testing::Test
  {
  public:
    static SchemeType scheme;

    static qi::Url defaultListenURL()
    {
      static const qi::Url listenUrl = scheme() + "://127.0.0.1:0";
      return listenUrl;
    }

    struct ListenResult
    {
      qi::Promise<qi::MessageSocketPtr> promiseConnectedSocket;
      qi::Url url;
    };

    static ListenResult listen(qi::TransportServer& server
      , const qi::Url& url = defaultListenURL()
      , bool connectNewConnectionSignal = true)
    {
      using namespace qi;
      const auto key = qi::path::findData("qi", "server.key");
      const auto crt = qi::path::findData("qi", "server.crt");
      server.setIdentity(key, crt);
      Promise<MessageSocketPtr> promiseConnectedSocket;
      if (connectNewConnectionSignal)
      {
        server.newConnection.connect([=](const std::pair<MessageSocketPtr, Url>& p) mutable {
          promiseConnectedSocket.setValue(p.first);
        });
      }
      else
      {
        promiseConnectedSocket.setValue({});
      }
      server.listen(url);

      return { promiseConnectedSocket, server.endpoints().front() };
    }

  };

  template<class SchemeType>
  SchemeType NetMessageSocket<SchemeType>::scheme;

  template<class SchemeType>
  class NetMessageSocketAsio : public NetMessageSocket<SchemeType> {};

  using SchemeTypes = ::testing::Types< SchemeTcp
                                        , SchemeTcpSSL
                                        >;

  TYPED_TEST_CASE(NetMessageSocket, SchemeTypes);
  TYPED_TEST_CASE(NetMessageSocketAsio, SchemeTypes);

}

TYPED_TEST(NetMessageSocket, DestroyNotConnectedAsio)
{
  using namespace qi;
  using namespace qi::sock;

  SignalConnectionPtr signalConnection;
  {
    auto clientSideSocket = makeMessageSocket(this->scheme());
    const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
    signalConnection = connectSignals(clientSideSocket);

    // Check that signal were not emitted.
    ASSERT_TRUE(signalConnection->promises.connectedReceived().isRunning());
    ASSERT_TRUE(signalConnection->promises.disconnectedReceived().isRunning());
  }

  // Socket is destroyed : disconnect must not be emitted since it was not
  // connected in the first place.
  ASSERT_TRUE(signalConnection->promises.disconnectedReceived().isRunning());
}

TYPED_TEST(NetMessageSocket, ConnectAndDisconnectAsio)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto url = this->listen(server).url;

  // We also want to check that signals are emitted.
  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  auto signalConnection = connectSignals(clientSideSocket);

  Future<void> fut0 = clientSideSocket->connect(url);
  ASSERT_EQ(FutureState_FinishedWithValue, fut0.wait(defaultTimeout));
  ASSERT_EQ(FutureState_FinishedWithValue,
            signalConnection->promises.connectedReceived().wait(defaultTimeout));
  ASSERT_TRUE(signalConnection->promises.disconnectedReceived().isRunning());

  Future<void> fut1 = clientSideSocket->disconnect();
  ASSERT_EQ(FutureState_FinishedWithValue, fut1.wait(defaultTimeout));
  ASSERT_EQ(FutureState_FinishedWithValue,
            signalConnection->promises.disconnectedReceived().wait(defaultTimeout));
}

TYPED_TEST(NetMessageSocket, ConnectAndDestroyAsio)
{
  using namespace qi;
  using namespace qi::sock;

  // We also want to check that signals are emitted.
  SignalConnectionPtr signalConnection;

  {
    // Start a server and get the server side socket.
    TransportServer server;
    const auto url = this->listen(server).url;
    auto clientSideSocket = makeMessageSocket(this->scheme());
    const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
    signalConnection = connectSignals(clientSideSocket);

    Future<void> fut0 = clientSideSocket->connect(url);
    ASSERT_EQ(FutureState_FinishedWithValue, fut0.wait(defaultTimeout));
    ASSERT_EQ(FutureState_FinishedWithValue,
              signalConnection->promises.connectedReceived().wait(defaultTimeout));
    ASSERT_TRUE(signalConnection->promises.disconnectedReceived().isRunning());
  }

  // disconnected must be emitted on destruction.
  ASSERT_EQ(FutureState_FinishedWithValue,
            signalConnection->promises.disconnectedReceived().wait(defaultTimeout));
}

TYPED_TEST(NetMessageSocket, SendWhileNotConnectedAsio)
{
  using namespace qi;
  using namespace qi::sock;

  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  auto signalConnection = connectSignals(clientSideSocket);

  MessageAddress address{1234, 5, 9876, 107};
  ASSERT_FALSE(clientSideSocket->send(makeMessage(address)));

  // Check that signal were not emitted.
  ASSERT_TRUE(signalConnection->promises.connectedReceived().isRunning());
  ASSERT_TRUE(signalConnection->promises.disconnectedReceived().isRunning());
}

TYPED_TEST(NetMessageSocket, SendAfterDisconnectedAsio)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto url = this->listen(server).url;

  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  Future<void> fut0 = clientSideSocket->connect(url);
  ASSERT_EQ(FutureState_FinishedWithValue, fut0.wait(defaultTimeout));

  Future<void> fut1 = clientSideSocket->disconnect();
  ASSERT_EQ(FutureState_FinishedWithValue, fut1.wait(defaultTimeout));

  MessageAddress address{1234, 5, 9876, 107};
  ASSERT_FALSE(clientSideSocket->send(makeMessage(address)));
}

TYPED_TEST(NetMessageSocket, DisconnectWhileNotConnectedAsio)
{
  using namespace qi;
  using namespace qi::sock;

  auto socket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ socket->disconnect().wait(defaultTimeout); });
  ASSERT_FALSE(socket->disconnect().hasError());
}

TYPED_TEST(NetMessageSocket, ReceiveOneMessageAsio)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto listenRes = this->listen(server);
  auto& promiseServerSideSocket = listenRes.promiseConnectedSocket;
  const auto url = listenRes.url;

  // Connect the client.
  auto msgSent = makeMessage(MessageAddress{1234, 5, 9876, 107});

  Promise<void> promiseReceivedMessage;
  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  clientSideSocket->messageReady.connect([&](const Message& msgReceived) mutable {
    if (!messageEqual(msgReceived, msgSent)) throw std::runtime_error("messages are not equal.");
    promiseReceivedMessage.setValue(0);
  });
  Future<void> fut = clientSideSocket->connect(url);
  ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));

  // The server sends a message.
  ASSERT_TRUE(promiseServerSideSocket.future().hasValue());
  ASSERT_TRUE(promiseServerSideSocket.future().value()->ensureReading());
  ASSERT_TRUE(promiseServerSideSocket.future().value()->send(msgSent));

  // Wait for the client to receive it.
  ASSERT_EQ(FutureState_FinishedWithValue, promiseReceivedMessage.future().wait(defaultTimeout));
}

TYPED_TEST(NetMessageSocketAsio, ReceiveManyMessages)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto listenRes = this->listen(server);
  auto& promiseServerSideSocket = listenRes.promiseConnectedSocket;
  const auto url = listenRes.url;

  // Connect the client.
  Promise<void> promiseAllMessageReceived;
  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });

  const int messageCount = 100;
  MessageAddress address{1234, 5, 9876, 107};
  std::atomic<int> i { 0 };
  clientSideSocket->messageReady.connect([=, &i](const Message& msg) mutable {
    const auto oldi = i++;
    if (oldi >= messageCount) promiseAllMessageReceived.setError("Too many messages received.");
    auto addr = address;
    addr.messageId = address.messageId + oldi;
    auto msg2 = makeMessage(addr);
    if (!messageEqual(msg, msg2))
    {
      promiseAllMessageReceived.setError("message not equal.");
    }
    if (oldi + 1 == messageCount) promiseAllMessageReceived.setValue(0);
  });
  Future<void> fut = clientSideSocket->connect(url);
  fut.wait();
  ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));

  // The server sends a message.
  ASSERT_TRUE(promiseServerSideSocket.future().hasValue());
  auto serverSideSocket = promiseServerSideSocket.future().value();
  serverSideSocket->ensureReading();
  for (int i = 0; i != messageCount; ++i)
  {
    serverSideSocket->send(makeMessage(address));
    ++address.messageId;
  }

  // Wait for the client to receive it.
  ASSERT_EQ(FutureState_FinishedWithValue, promiseAllMessageReceived.future().wait());
  qiLogInfo("") << "fin\n";
}

// The test ends while a socket connection or a server accept may be pending.
// The destruction of the corresponding objects must be fine.
// This test must typically be launched a great number of times to be meaningful.
TYPED_TEST(NetMessageSocket, PrematureDestroy)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto url = this->listen(server).url;

  // Connect the client.
  auto msgSent = makeMessage(MessageAddress{1234, 5, 9876, 107});

  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  clientSideSocket->connect(url);
}

TYPED_TEST(NetMessageSocket, DisconnectWhileConnecting)
{
  using namespace qi;
  using namespace qi::sock;
  using N = mock::Network;

  std::thread threadResolve;
  auto scopedResolve = ka::scoped_set_and_restore(
    Resolver<N>::async_resolve,
    [&](Resolver<N>::query q, Resolver<N>::_anyResolveHandler h) {
        threadResolve = std::thread{[=] {
          mock::defaultAsyncResolve(q, h);
        }};
    }
  );
  auto r = ka::scoped([&] {
    threadResolve.join();
  });

  Promise<void> promiseAsyncConnectStarted;
  Promise<ErrorCode<N>> promiseAsyncConnectResult;

  // The async connect waits for a promise to be set before finishing.
  std::thread threadConnect;
  auto scopedConnect = ka::scoped_set_and_restore(
    Lowest<SslSocket<N>>::async_connect,
    [&](N::_resolver_entry, N::_anyHandler h) mutable {
      threadConnect = std::thread{[=]() mutable {
        promiseAsyncConnectStarted.setValue(0);
        auto err = promiseAsyncConnectResult.future().value();
        h(err);
      }};
    }
  );
  auto c = ka::scoped([&] {
    threadConnect.join();
  });

  using LowestLayer = Lowest<SslSocket<N>>;
  auto scopedShutdown = ka::scoped_set_and_restore(
    LowestLayer::_shutdown,
    [=](LowestLayer::shutdown_type, ErrorCode<N>) mutable {
      auto err = operationAborted<ErrorCode<N>>();
      promiseAsyncConnectResult.setValue(err);
    }
  );

  auto socket = boost::make_shared<TcpMessageSocket<N>>();
  Future<void> futConnect = socket->connect(Url{"tcp://10.11.12.13:1234"});

  // We wait for the async connect to start.
  promiseAsyncConnectStarted.future().wait();
  Future<void> futDisconnect = socket->disconnect();

  // Now the async connect (therefore the connecting part) can finish.
  ASSERT_EQ(FutureState_FinishedWithValue, futDisconnect.wait(defaultTimeout));
  ASSERT_EQ(FutureState_FinishedWithError, futConnect.wait(defaultTimeout));
  ASSERT_EQ("Connect abort: disconnection requested while connecting", futConnect.error());
}

TYPED_TEST(NetMessageSocket, DisconnectWhileDisconnecting)
{
  using namespace qi;
  using namespace qi::sock;
  using N = mock::Network;

  Promise<ErrorCode<N>> promiseAsyncReadWrite;

  Resolver<N>::async_resolve = mock::defaultAsyncResolve;
  Lowest<SslSocket<N>>::async_connect = mock::defaultAsyncConnect;
  std::vector<std::thread> readThreads, writeThreads;

  // Make sure async read and async write block until shutdown happens.
  auto scopedRead = ka::scoped_set_and_restore(
    N::_async_read_next_layer,
    [=, &readThreads](SslSocket<N>::next_layer_type&, N::_mutable_buffer_sequence, N::_anyTransferHandler h) {
      readThreads.push_back(std::thread{[=] {
        h(promiseAsyncReadWrite.future().value(), 0);
      }});
    }
  );
  auto scopedWrite = ka::scoped_set_and_restore(
    N::_async_write_next_layer,
    [=, &writeThreads](SslSocket<N>::next_layer_type&, const std::vector<N::_const_buffer_sequence>&, N::_anyTransferHandler h) {
      writeThreads.push_back(std::thread{[=] {
        h(promiseAsyncReadWrite.future().value(), 0);
      }});
    }
  );

  using LowestLayer = Lowest<SslSocket<N>>;
  auto scopedShutdown = ka::scoped_set_and_restore(
    LowestLayer::_shutdown,
    [=](LowestLayer::shutdown_type, ErrorCode<N>&) mutable {
      if (promiseAsyncReadWrite.future().isRunning())
      {
        promiseAsyncReadWrite.setValue(shutdown<ErrorCode<N>>());
      }
    }
  );

  Promise<void> promiseCloseStarted;
  Promise<void> promiseCanFinishClose;
  auto scopedClose = ka::scoped_set_and_restore(
    LowestLayer::close,
    [=](ErrorCode<N>&) mutable {
      promiseCloseStarted.setValue(0);
      promiseCanFinishClose.future().wait(); // block
    }
  );

  auto socket = boost::make_shared<TcpMessageSocket<N>>();
  socket->connect(Url{"tcp://10.11.12.13:1234"});

  Future<void> futDisconnect0 = socket->disconnect();
  ASSERT_TRUE(futDisconnect0.isRunning());

  // Wait for entering close().
  ASSERT_EQ(FutureState_FinishedWithValue, promiseCloseStarted.future().wait(defaultTimeout));

  // Subsequent disconnect() cannot complete because close() is blocking.
  Future<void> futDisconnect1 = socket->disconnect();
  ASSERT_TRUE(futDisconnect1.isRunning()) << "finished: " << futDisconnect1.isFinished();
  Future<void> futDisconnect2 = socket->disconnect();
  ASSERT_TRUE(futDisconnect2.isRunning());

  // Now, unblock close() so that disconnecting state and later
  // disconnected state can complete.
  promiseCanFinishClose.setValue(0);

  // All future must complete.
  ASSERT_EQ(FutureState_FinishedWithValue, futDisconnect0.wait(defaultTimeout));
  ASSERT_EQ(FutureState_FinishedWithValue, futDisconnect1.wait(defaultTimeout));
  ASSERT_EQ(FutureState_FinishedWithValue, futDisconnect2.wait(defaultTimeout));
  ASSERT_FALSE(socket->isConnected());
  ASSERT_EQ(MessageSocket::Status::Disconnected, socket->status());

  for (auto& t: readThreads) t.join();
  for (auto& t: writeThreads) t.join();
}

// Stress test for disconnections.
// A lot of threads try to disconnect the same socket at the same time.
// This test should be launched a lot of times to be meaningful.
TYPED_TEST(NetMessageSocketAsio, DisconnectBurst)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server.
  TransportServer server;
  const auto url = this->listen(server).url;

  // Connect the client.
  auto socket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ socket->disconnect().wait(defaultTimeout); });
  Future<void> fut = socket->connect(url);
  ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));

  // Launch threads waiting to disconnect the socket.
  Promise<void> promiseRun;
  std::array<std::thread, 100> threads;
  for (auto& t: threads)
  {
    t = std::thread{[&] {
      promiseRun.future().wait();
      socket->disconnect().wait(defaultTimeout);
    }};
  }

  // Trigger the concurrent disconnections from all threads.
  promiseRun.setValue(nullptr);
  for (auto& t: threads) t.join();
}

TYPED_TEST(NetMessageSocketAsio, SendReceiveManyMessages)
{
  using namespace qi;
  using namespace qi::sock;

  // Start a server and get the server side socket.
  TransportServer server;
  const auto listenRes = this->listen(server);
  auto& promiseServerSideSocket = listenRes.promiseConnectedSocket;
  const auto url = listenRes.url;

  // Connect the client.
  Promise<void> promiseAllMessageReceived;
  auto clientSideSocket = makeMessageSocket(this->scheme());
  const auto _ = ka::scoped([=]{ clientSideSocket->disconnect().wait(defaultTimeout); });
  const unsigned sendThreadCount = 100u;
  const unsigned perSendThreadMessageCount = 200u;
  const unsigned messageCount = sendThreadCount * perSendThreadMessageCount;
  MessageAddress address{1234, 5, 9876, 107};
  const auto msg0 = makeMessage(address);
  std::atomic<unsigned> i{0u};
  clientSideSocket->messageReady.connect([=, &i](const Message& msg) mutable {
    if (i > messageCount) promiseAllMessageReceived.setError("Too many messages received.");
    if (!messageEqual(msg, msg0))
    {
      promiseAllMessageReceived.setError("message not equal.");
    }
    const auto iVal = ++i;
    if (iVal == messageCount) promiseAllMessageReceived.setValue(0);
  });
  Future<void> fut = clientSideSocket->connect(url);
  ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));

  // The server sends a message.
  ASSERT_TRUE(promiseServerSideSocket.future().hasValue());
  auto serverSideSocket = promiseServerSideSocket.future().value();
  serverSideSocket->ensureReading();

  std::mutex randMutex;
  std::random_device rd;
  std::mt19937 gen{rd()};
  std::uniform_int_distribution<> rand{1, 20};
  std::vector<std::thread> sendThreads;
  for (auto i = sendThreadCount; i != 0u; --i)
  {
    sendThreads.push_back(std::thread{
      [&] {
        for (auto j = perSendThreadMessageCount; j != 0u; --j)
        {
          serverSideSocket->send(msg0);
          std::this_thread::sleep_for(std::chrono::milliseconds{syncRand(rand, gen, randMutex)});
        }
      }
    });
  }

  // Wait for the client to receive it.
  ASSERT_EQ(FutureState_FinishedWithValue, promiseAllMessageReceived.future().wait());
  for (auto& t: sendThreads) t.join();
}

TEST(NetMessageSocketAsio, DisconnectToDistantWhileConnected)
{
  using namespace qi;
  using namespace qi::sock;

  const std::string remoteServiceOwnerPath = path::findBin("remoteserviceowner");
  const std::string scheme{"tcp"};
  const Url url{scheme + "://127.0.0.1:54321"};
  test::ScopedProcess _{
    remoteServiceOwnerPath, {"--qi-standalone", "--qi-listen-url=" + url.str()}
  };
  auto socket = makeMessageSocket(scheme);
  const auto _2 = ka::scoped([=]{ socket->disconnect().wait(defaultTimeout); });
  Future<void> futCo = test::attemptConnect(*socket, url);
  ASSERT_EQ(FutureState_FinishedWithValue, futCo.wait(defaultTimeout));
  Future<void> futDisco = socket->disconnect().async();
  ASSERT_EQ(FutureState_FinishedWithValue, futDisco.wait(defaultTimeout));
}

// Connect to another process and make it brutally crash to check that the
// disconnection is quick and does not last until a system timeout expires.
TEST(NetMessageSocketAsio, DistantCrashWhileConnected)
{
  using namespace qi;
  using namespace qi::sock;

  const std::string remoteServiceOwnerPath = path::findBin("remoteserviceowner");
  const std::string protocol{"tcp"};
  const Url url{protocol + "://127.0.0.1:54321"};
  MessageSocketPtr socket;
  const auto _ = ka::scoped([=]{ if (socket) socket->disconnect().wait(defaultTimeout); });
  {
    test::ScopedProcess _{
      remoteServiceOwnerPath, {"--qi-standalone", "--qi-listen-url=" + url.str()}
    };
    socket = makeMessageSocket(protocol);
    Future<void> fut = test::attemptConnect(*socket, url);
    ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));
  }
  std::this_thread::sleep_for(std::chrono::milliseconds{500});
  ASSERT_EQ(MessageSocket::Status::Disconnected, socket->status());
  Future<void> fut = socket->disconnect();
  ASSERT_EQ(FutureState_FinishedWithValue, fut.wait(defaultTimeout));
}

max_stars_repo_name: yumilceh/libqi

max_issues_repo_name: yumilceh/libqi

max_forks_repo_name: yumilceh/libqi


text:
//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include <boost/format.hpp>
#include <soci/postgresql/soci-postgresql.h>
#include <soci/soci.h>
#include "DB_Connection.hpp"
#include "util/LangHelpers.hpp"

using namespace sim_mob::db;
using std::string;
using soci::postgresql;
using soci::session;

namespace
{
const std::string PGSQL_CONNSTR_FORMAT = "host=%1% "
                                         "port=%2% "
                                         "user=%3% "
                                         "password=%4% "
                                         "dbname=%5%";

/**
 * Class that holds the session object.
 *
 * \author Pedro Gandola
 */
template<typename T>
class DB_Session
{
public:

    DB_Session()
    {
    }

    virtual ~DB_Session()
    {
    }

    T& getSession()
    {
        return session;
    }
    T session;
};

/**
 * Soci session holder.
 */
typedef DB_Session<soci::session> SociSessionImpl;
}

DB_Connection::DB_Connection(BackendType type, const DB_Config& config) :
        currentSession(nullptr), type(type), connected(false)
{
    switch (type)
    {
    case POSTGRES:
    {
        const std::string PGSQL_CONNSTR_FORMAT2 = "host=%1% port=%2% user=%3% password=%4% dbname=%5%";
        boost::format fmtr = boost::format(PGSQL_CONNSTR_FORMAT2);
        fmtr % config.getHost() % config.getPort() % config.getUsername() % config.getPassword() % config.getDatabaseName();
        connectionStr = fmtr.str();
        break;
    }
    default:
        break;
    }
}

DB_Connection::~DB_Connection()
{
    disconnect();
}

bool DB_Connection::connect()
{
    if (!connected)
    {
        switch (type)
        {
        case POSTGRES:
        {
            currentSession = new SociSessionImpl();
            getSession<soci::session>().open(postgresql, connectionStr);
            connected = true;
            break;
        }
        default:
            break;
        }
    }
    return connected;
}

bool DB_Connection::disconnect()
{
    if (connected)
    {
        switch (type)
        {
        case POSTGRES:
        {
            getSession<soci::session>().close();
            delete (SociSessionImpl*) currentSession;
            break;
        }
        default:
            break;
        }
        connected = false;
    }
    return !connected;
}

bool DB_Connection::isConnected() const
{
    return connected;
}

template<typename T> T& DB_Connection::getSession()
{
    return ((DB_Session<T>*) (currentSession))->getSession();
}

std::string DB_Connection::getSchema()
{
    return schema;
}

void DB_Connection::setSchema(std::string _schema)
{
    schema = _schema;
}

std::string DB_Connection::getConnectionStr()
{
    return this->connectionStr;
}

template soci::session& DB_Connection::getSession<soci::session>();

ext: cpp


text:
// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4 (projects.h)

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PROJECTS_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PROJECTS_HPP

#include <cstring>
#include <string>
#include <vector>

#include <boost/math/constants/constants.hpp>

namespace boost { namespace geometry { namespace projections
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

/* some useful constants */
static const double FORTPI = boost::math::constants::pi<double>() / 4.0;

static const int PJD_UNKNOWN =0;
static const int PJD_3PARAM = 1;
static const int PJD_7PARAM = 2;
static const int PJD_GRIDSHIFT = 3;
static const int PJD_WGS84 = 4;   /* WGS84 (or anything considered equivelent) */

struct pvalue
{
    std::string param;
    int used;

    int i;
    double f;
    std::string s;
};

struct pj_const_pod
{
    int over;   /* over-range flag */
    int geoc;   /* geocentric latitude flag */
    int is_latlong; /* proj=latlong ... not really a projection at all */
    int is_geocent; /* proj=geocent ... not really a projection at all */
    double
        a,  /* major axis or radius if es==0 */
        a_orig, /* major axis before any +proj related adjustment */
        es, /* e ^ 2 */
        es_orig, /* es before any +proj related adjustment */
        e,  /* eccentricity */
        ra, /* 1/A */
        one_es, /* 1 - e^2 */
        rone_es, /* 1/one_es */
        lam0, phi0, /* central longitude, latitude */
        x0, y0, /* easting and northing */
        k0,    /* general scaling factor */
        to_meter, fr_meter; /* cartesian scaling */

    int datum_type; /* PJD_UNKNOWN/3PARAM/7PARAM/GRIDSHIFT/WGS84 */
    double  datum_params[7];
    double  from_greenwich; /* prime meridian offset (in radians) */
    double  long_wrap_center; /* 0.0 for -180 to 180, actually in radians*/

    // Initialize all variables to zero
    pj_const_pod()
    {
        std::memset(this, 0, sizeof(pj_const_pod));
    }
};

// PROJ4 complex. Might be replaced with std::complex
struct COMPLEX { double r, i; };

struct PJ_ELLPS
{
    std::string id;    /* ellipse keyword name */
    std::string major;    /* a= value */
    std::string ell;    /* elliptical parameter */
    std::string name;    /* comments */
};

struct PJ_DATUMS
{
    std::string id;     /* datum keyword */
    std::string defn;   /* ie. "to_wgs84=..." */
    std::string ellipse_id; /* ie from ellipse table */
    std::string comments; /* EPSG code, etc */
};

struct PJ_PRIME_MERIDIANS
{
    std::string id;     /* prime meridian keyword */
    std::string defn;   /* offset from greenwich in DMS format. */
};

struct PJ_UNITS
{
    std::string id;    /* units keyword */
    std::string to_meter;    /* multiply by value to get meters */
    std::string name;    /* comments */
};

struct DERIVS
{
    double x_l, x_p; /* derivatives of x for lambda-phi */
    double y_l, y_p; /* derivatives of y for lambda-phi */
};

struct FACTORS
{
    struct DERIVS der;
    double h, k;    /* meridinal, parallel scales */
    double omega, thetap;    /* angular distortion, theta prime */
    double conv;    /* convergence */
    double s;        /* areal scale factor */
    double a, b;    /* max-min scale error */
    int code;        /* info as to analytics, see following */
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief parameters, projection parameters
    \details This structure initializes all projections
    \ingroup projection
*/
struct parameters : public detail::pj_const_pod
{
    std::string name;
    std::vector<detail::pvalue> params;
};

// TODO: derived from boost::exception / make more for forward/inverse/init/setup
class proj_exception
{
public:

    proj_exception(int code = 0)
        : m_code(code)
    {
    }
    int code() const { return m_code; }
private :
    int m_code;
};

}}} // namespace boost::geometry::projections
#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PROJECTS_HPP

ext: hpp

max_stars_repo_name: bowlofstew/omim

max_issues_repo_name: bowlofstew/omim

max_forks_repo_name: bowlofstew/omim


text:
#include "ConnectionConfiguration.h"

#include <boost/filesystem.hpp>
namespace bfs = boost::filesystem;

namespace mc_rtc_rviz
{

ConnectionConfiguration::ConnectionConfiguration()
: protocol_(Protocol::IPC), host_((bfs::temp_directory_path() / "mc_rtc").string()), sub_suffix_("_pub.ipc"),
  push_suffix_("_rep.ipc")
{
}

ConnectionConfiguration::ConnectionConfiguration(const std::string & host) : ConnectionConfiguration()
{
  host_ = host;
}

namespace
{
std::string n2s(unsigned int i)
{
  std::stringstream ss;
  ss << i;
  return ss.str();
}
} // namespace

ConnectionConfiguration::ConnectionConfiguration(const std::string & host, unsigned int s, unsigned int p)
: ConnectionConfiguration(Protocol::TCP, host, n2s(s), n2s(p))
{
}

ConnectionConfiguration::ConnectionConfiguration(Protocol proto,
                                                 const std::string & host,
                                                 const std::string & sub,
                                                 const std::string & push)
: protocol_(proto), host_(host), sub_suffix_(sub), push_suffix_(push)
{
}

std::string ConnectionConfiguration::toText() const
{
  if(protocol_ == Protocol::IPC)
  {
    return "IPC | " + host_;
  }
  return "TCP | " + host_ + ":" + sub_suffix_ + "|" + push_suffix_;
}

std::string ConnectionConfiguration::sub_uri() const
{
  if(protocol_ == Protocol::IPC)
  {
    return "ipc://" + host_ + sub_suffix_;
  }
  return "tcp://" + host_ + ":" + sub_suffix_;
}

std::string ConnectionConfiguration::push_uri() const
{
  if(protocol_ == Protocol::IPC)
  {
    return "ipc://" + host_ + push_suffix_;
  }
  return "tcp://" + host_ + ":" + push_suffix_;
}

} // namespace mc_rtc_rviz

namespace mc_rtc
{

using ConnectionConfiguration = mc_rtc_rviz::ConnectionConfiguration;
using Protocol = ConnectionConfiguration::Protocol;

ConnectionConfiguration ConfigurationLoader<ConnectionConfiguration>::load(const Configuration & cfg)
{
  Protocol proto = static_cast<std::string>(cfg("type")) == "IPC" ? Protocol::IPC : Protocol::TCP;
  if(proto == Protocol::IPC)
  {
    return ConnectionConfiguration(static_cast<std::string>(cfg("host")));
  }
  else
  {
    return ConnectionConfiguration(cfg("host"), cfg("sub_suffix"), cfg("push_suffix"));
  }
}

Configuration ConfigurationLoader<ConnectionConfiguration>::save(const ConnectionConfiguration & config)
{
  mc_rtc::Configuration cfg;
  cfg.add("type", config.protocol() == Protocol::IPC ? "IPC" : "TCP");
  cfg.add("host", config.host());
  cfg.add("sub_suffix", config.sub_suffix());
  cfg.add("push_suffix", config.push_suffix());
  return cfg;
}

} // namespace mc_rtc


text:
//
// Created by plter on 2019/9/18.
//

#include "../include/Request.h"
#include <boost/beast.hpp>
#include <boost/asio/use_awaitable.hpp>

#include <utility>
#include <iostream>
#include "../include/Server.h"
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
#include <vector>

namespace beast = boost::beast;
namespace http = beast::http;

um::Request::Request(um::Server *server, um::TcpStreamSPtr stream) :
        _stream(std::move(stream)), _method(http::verb::unknown) {
    _server = server;
}

boost::asio::awaitable<void> um::Request::asyncInit() {
    beast::flat_buffer buffer;
    _beastRequest = BeastHttpStringBodyRequest();

    http::request_parser<http::string_body> parser;
    parser.body_limit(_server->getRequestBodyLimit());
    co_await http::async_read(*_stream, buffer, parser, boost::asio::use_awaitable);
    _beastRequest = parser.get();

    _target = _beastRequest.target().to_string();
    _method = _beastRequest.method();

    auto questionMarkPosition = _target.find('?');
    if (questionMarkPosition != std::string::npos) {
        _requestPath = _target.substr(0, questionMarkPosition);
        _query = _target.substr(questionMarkPosition + 1);
    } else {
        _requestPath = _target;
    }
    _body = _beastRequest.body();
    _contentType = _beastRequest[boost::beast::http::field::content_type].to_string();

    //Parse the body
    _getVars = um::URLParameterDecoder::decode(_query);
    if (_method == boost::beast::http::verb::post) {
        if (_contentType == "application/x-www-form-urlencoded") {
            _postVars = um::URLParameterDecoder::decode(_body);
        } else {
            const static boost::regex r("^multipart/form-data; boundary=(.*)$");
            boost::smatch matcher;
            if (boost::regex_match(_contentType, matcher, r)) {
                auto boundary = matcher[1];
                _formData = FormDataDecoder::decode(_body, boundary);
            }
        }
    }
}

const um::TcpStreamSPtr &um::Request::getStream() const {
    return _stream;
}

const um::BeastHttpStringBodyRequest &um::Request::getBeastRequest() const {
    return _beastRequest;
}

const std::string &um::Request::getTarget() const {
    return _target;
}

const std::string &um::Request::getRequestPath() const {
    return _requestPath;
}

const std::string &um::Request::getQuery() const {
    return _query;
}

http::verb um::Request::getMethod() const {
    return _method;
}

const std::string &um::Request::getBody() const {
    return _body;
}

const um::URLParameterPairs &um::Request::getPostVars() const {
    return _postVars;
}

const um::URLParameterPairs &um::Request::getGetVars() const {
    return _getVars;
}

const std::map<std::string, std::string> &um::Request::getCookie() const {
    return _cookie;
}

void um::Request::setCookie(const std::map<std::string, std::string> &cookie) {
    _cookie = cookie;
}

um::Server *um::Request::getServer() const {
    return _server;
}

const std::string &um::Request::getSessionId() const {
    return _sessionId;
}

void um::Request::setSessionId(const std::string &sessionId) {
    _sessionId = sessionId;
}

std::map<std::string, std::string> &um::Request::getSession() {
    return getServer()->getSessionStorage()->getSession(getSessionId());
}

std::string um::Request::getSessionValue(const std::string &key) {
    return getSessionStorage()->getSessionValue(getSessionId(), key);
}

void um::Request::setSessionValue(const std::string &key, std::string value) {
    getSessionStorage()->setSessionValue(getSessionId(), key, std::move(value));
}

void um::Request::setSessionValue(const std::string &key, int value) {
    setSessionValue(key, std::to_string(value));
}

std::string um::Request::getSessionValue(const std::string &key, std::string defaultValue) {
    auto v = getSessionValue(key);
    if (!v.empty()) {
        return v;
    } else {
        return defaultValue;
    }
}

int um::Request::getSessionValueAsInt(const std::string &key) {
    return std::stoi(getSessionValue(key));
}

int um::Request::getSessionValueAsInt(const std::string &key, int defaultValue) {
    auto v = getSessionValue(key);
    if (!v.empty()) {
        return std::stoi(v);
    } else {
        return defaultValue;
    }
}

um::AbstractSessionStorageSPtr um::Request::getSessionStorage() const {
    return getServer()->getSessionStorage();
}

const std::string &um::Request::getControllerName() const {
    return _controllerName;
}

void um::Request::setControllerName(const std::string &controllerName) {
    _controllerName = controllerName;
}

const std::string &um::Request::getActionName() const {
    return _actionName;
}

void um::Request::setActionName(const std::string &actionName) {
    _actionName = actionName;
}

void um::Request::addArg(const std::string &arg) {
    _args.emplace_back(arg);
}

std::string um::Request::arg(int index) {
    return _args[index];
}

const std::vector<std::string> &um::Request::getArgs() const {
    return _args;
}

const um::FormData &um::Request::getFormData() const {
    return _formData;
}

max_stars_repo_name: plter/UnderMountain

max_issues_repo_name: plter/UnderMountain

max_forks_repo_name: plter/UnderMountain


text:
#include "json/reader.h"
#include "ReadFixture.h"
#include "JsonOps.h"
#include <boost/test/unit_test.hpp>

class ObjectReadFixture : public ReadFixture
{
public:
   virtual void ObjParse()
   {
      RunParse();
      o = root;
   }
   // Putting a value in a const UnknownElement first causes an exception if there is a bad conversion
   const json::UnknownElement GetConstElement(std::string key)
   {
      const json::UnknownElement u = o[key];
      return u;
   }
   void KeyInObject(std::string key)
   {
      BOOST_TEST((o.Find(key) != o.End()), "Could not find key: " + key);
   }
   json::Object o;
};

BOOST_AUTO_TEST_SUITE(ObjectRead)

BOOST_FIXTURE_TEST_CASE(EmptyObject, ObjectReadFixture)
{
   input << "{}";

   ObjParse();

   BOOST_TEST((o.Begin() == o.End()));
}

BOOST_FIXTURE_TEST_CASE(SingleItem, ObjectReadFixture)
{
   input << "{\n\t\"Null\" : null\n}";
   ObjParse();

   BOOST_TEST(o.Begin()->name == "Null");
   const json::Null n = GetConstElement("Null");
}

BOOST_FIXTURE_TEST_CASE(ObjectWithBasicTypes, ObjectReadFixture)
{
   input << "{\n\t\"String\" : \"TEST_STRING\",\n\t\"Number\" : 42,\n\t\"Boolean\" : true,\n\t\"Null\" : null\n}";
   ObjParse();

   KeyInObject("String");
   const json::String s = GetConstElement("String");
   BOOST_TEST("TEST_STRING" == s.Value());

   KeyInObject("Number");
   const json::Number n = GetConstElement("Number");
   BOOST_TEST(42 == n.Value());

   KeyInObject("Boolean");
   const json::Boolean b = GetConstElement("Boolean");
   BOOST_TEST(true == b.Value());

   KeyInObject("Null");
   const json::Null nu = GetConstElement("Null");
}

BOOST_FIXTURE_TEST_CASE(ObjectWithObject, ObjectReadFixture)
{
   input << "{\n\t\"NestedObject\" : {\n\t\t\"Null2\" : null\n\t}\n}";
   ObjParse();

   KeyInObject("NestedObject");
   json::Object o2 = GetConstElement("NestedObject");
   BOOST_TEST((o2.Find("Null2") != o2.End()), "No sub object");
}

BOOST_FIXTURE_TEST_CASE(ObjectWithArray, ObjectReadFixture)
{
   input << "{\n\t\"NestedArray\" : [\n\t\tnull,\n\t\tnull\n\t]\n}";
   ObjParse();

   KeyInObject("NestedArray");
   json::Array a = GetConstElement("NestedArray");
   BOOST_TEST(2 == a.Size());
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: cajun-jsonapi/cajun-jsonapi

max_issues_repo_name: cajun-jsonapi/cajun-jsonapi

max_forks_repo_name: cajun-jsonapi/cajun-jsonapi


text:
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019,  Intel Corporation.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * 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 Willow Garage 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.
 *********************************************************************/

/* Author: Yu Yan */

#include <moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h>
#include <moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h>

#include <rviz/window_manager_interface.h>

#include <Eigen/Geometry>
#include <cmath>

#include <iostream>

namespace moveit_rviz_plugin
{
HandEyeCalibrationDisplay::HandEyeCalibrationDisplay(QWidget* parent) : Display()
{
  move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "",
                                                     "The name of the ROS namespace in "
                                                     "which the move_group node is running",
                                                     this, SLOT(fillPlanningGroupNameComboBox()), this);
  planning_scene_topic_property_ =
      new rviz::RosTopicProperty("Planning Scene Topic", "move_group/monitored_planning_scene",
                                 ros::message_traits::datatype<moveit_msgs::PlanningScene>(),
                                 "The topic on which the moveit_msgs::PlanningScene messages are received", this,
                                 SLOT(fillPlanningGroupNameComboBox()), this);

  fov_marker_enabled_property_ = new rviz::BoolProperty(
      "Camera FOV Marker", true, "Enable marker showing camera field of view", this, SLOT(updateMarkers()), this);
  fov_marker_alpha_property_ =
      new rviz::FloatProperty("Marker Alpha", 0.3f, "Specifies the alpha (transparency) for the rendered marker",
                              fov_marker_enabled_property_, SLOT(updateMarkers()), this);
  fov_marker_size_property_ =
      new rviz::FloatProperty("Marker Size", 1.5f, "Specifies the size (depth in meters) for the rendered marker",
                              fov_marker_enabled_property_, SLOT(updateMarkers()), this);
}

HandEyeCalibrationDisplay::~HandEyeCalibrationDisplay()
{
  if (frame_dock_)
    delete frame_dock_;
}

void HandEyeCalibrationDisplay::onInitialize()
{
  Display::onInitialize();

  rviz::WindowManagerInterface* window_context = context_->getWindowManager();
  frame_ = new HandEyeCalibrationFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr);

  if (window_context)
  {
    frame_dock_ = window_context->addPane("HandEye Calibration", frame_);
  }
}

void HandEyeCalibrationDisplay::save(rviz::Config config) const
{
  Display::save(config);
  if (frame_)
  {
    frame_->saveWidget(config);
  }
}

// Load all configuration data for this panel from the given Config object.
void HandEyeCalibrationDisplay::load(const rviz::Config& config)
{
  Display::load(config);
  if (frame_)
  {
    frame_->loadWidget(config);
  }
}

void HandEyeCalibrationDisplay::fillPlanningGroupNameComboBox()
{
  if (frame_ && frame_->tab_control_)
  {
    frame_->tab_control_->fillPlanningGroupNameComboBox();
  }
}

void HandEyeCalibrationDisplay::updateMarkers()
{
  if (frame_ && frame_->tab_context_)
  {
    frame_->tab_context_->updateAllMarkers();
  }
}

}  // namespace moveit_rviz_plugin


text:
// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MCWF_TrajectoryLogger.h"

#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/numeric.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include <iostream>
#include <cmath>

using namespace std;

quantumtrajectory::mcwf::Logger::Logger(int logLevel, size_t nLindblads)
  : logLevel_(logLevel), nLindblads_(nLindblads),
    nMCWF_steps_(), nOvershot_(), nToleranceOvershot_(),
    dpMaxOvershoot_(), dpToleranceMaxOvershoot_(), normMaxDeviation_(),
    traj_()
{}

ostream& quantumtrajectory::mcwf::Logger::onEnd(ostream& os) const
{
  if (logLevel_) {
    os<<"\nTotal number of MCWF steps: "<<nMCWF_steps_<<endl
      <<"\ndpLimit overshot: "<<nOvershot_<<" times, maximal overshoot: "<<dpMaxOvershoot_
      <<"\ndpTolerance overshot: "<<nToleranceOvershot_<<" times, maximal overshoot: "<<dpToleranceMaxOvershoot_<<endl
      <<"\nMaximal deviation of norm from 1: "<<normMaxDeviation_<<endl
      <<"\nMCWF Trajectory:\n";

    for (auto i : traj_) os<<i.first<<"\t"<<i.second<<std::endl;
    os<<endl;
  }
  return os;
}

void quantumtrajectory::mcwf::Logger::step() {++nMCWF_steps_;}

void quantumtrajectory::mcwf::Logger::stepBack(ostream& os, double dp, double dtDid, double newDtTry, double t, bool logControl)
{
  ++nToleranceOvershot_;
  if (logControl) dpToleranceMaxOvershoot_=max(dpToleranceMaxOvershoot_,dp);
  if (logLevel_>2)
    os<<"dpTolerance overshot: "<<dp<<" stepping back to "<<t<<" timestep decreased: "<<dtDid<<" => "<<newDtTry<<endl;
}

void quantumtrajectory::mcwf::Logger::overshot(ostream& os, double dp, double oldDtTry, double newDtTry, bool logControl)
{
  ++nOvershot_;
  if (logControl) dpMaxOvershoot_=max(dpMaxOvershoot_,dp);
  if (logLevel_>2)
    os<<"dpLimit overshot: "<<dp<<" timestep decreased: "<<oldDtTry<<" => "<<newDtTry<<endl;
}

void quantumtrajectory::mcwf::Logger::processNorm(double norm)
{
  normMaxDeviation_=max(normMaxDeviation_,fabs(1-norm));
  // NEEDS_WORK this should be somehow weighed by the timestep
}

void quantumtrajectory::mcwf::Logger::jumpOccured(ostream& os, double t, size_t lindbladNo)
{
  traj_.push_back({t,lindbladNo});
  if (logLevel_>1)
    os<<"Jump No. "<<lindbladNo<<" at time "<<t<<endl;
}

ostream& quantumtrajectory::ensemble::streamLog(ostream& os, const LoggerList& loggerList, size_t nBins, size_t nJumpsPerBin)
{
  using namespace boost;

#define AVERAGE_function(f) accumulate(loggerList, 0., [] (double init, const Logger& l) {return init + l.f;})/loggerList.size()
#define MAX_function(f) max_element(loggerList, [] (const Logger& a, const Logger& b) {return a.f<b.f;})->f

  os<<"\nAverage number of total steps: "<<AVERAGE_function(nMCWF_steps_)<<endl
    <<"\nOn average, dpLimit overshot: "<<AVERAGE_function(nOvershot_)<<" times, maximal overshoot: "<<MAX_function(dpMaxOvershoot_)
    <<"\nOn average, dpTolerance overshot: "<<AVERAGE_function(nToleranceOvershot_)<<" times, maximal overshoot: "<<MAX_function(dpToleranceMaxOvershoot_)<<endl
    <<"\nMaximal deviation of norm from 1: "<<MAX_function(normMaxDeviation_)<<endl;
/* NEEDS_WORK: this was before factoring out logging to Evolved/Adaptive, so it could be restored on some more fundamental level:  if (loggerList.front().isHamiltonian_) os<<"\nAverage number of failed ODE steps: "<<AVERAGE_function(nFailedSteps_)<<"\nAverage number of Hamiltonian calls:"<<AVERAGE_function(nHamiltonianCalls_)<<endl;*/

#undef  MAX_function
#undef  AVERAGE_function

  using namespace accumulators;

  size_t nTotalJumps=accumulate(loggerList, 0, [] (double init, const Logger& l) {return init + l.traj_.size();});

  // Heuristic: if nBins is not given (0), then the number of bins is determined such that the bins contain nJumpsPerBin samples on average
  if (!nBins && nTotalJumps<2*nJumpsPerBin) {
    os<<"\nToo few jumps for a histogram\n";
  }
  else {
    size_t actualNumberOfBins=nBins ? nBins : nTotalJumps/nJumpsPerBin;
    accumulator_set<double, features<tag::density> > acc( tag::density::num_bins = actualNumberOfBins , tag::density::cache_size = nTotalJumps );

    // fill accumulator
    for (auto i : loggerList) for (auto j : i.traj_) acc(j.first);

    const iterator_range<std::vector<std::pair<double, double> >::iterator> histogram=density(acc);

    os<<"\nHistogram of jumps. Number of bins="<<actualNumberOfBins+2<<endl;
    for (auto i : histogram)
      os<<i.first<<"\t"<<i.second<<endl;
  }

  os<<"\nAverage number of total jumps: "<<nTotalJumps/double(loggerList.size())<<endl;

  return os;
}

ext: cc

max_stars_repo_name: bartoszek/cppqed

max_issues_repo_name: bartoszek/cppqed

max_forks_repo_name: bartoszek/cppqed


text:
//
// Copyright 2017 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//

#include <boost/python.hpp>
#include <boost/python/class.hpp>

#include "pxr/pxr.h"
#include "pxr/base/tf/makePyConstructor.h"
#include "pxr/base/tf/pyContainerConversions.h"
#include "pxr/base/tf/pyPtrHelpers.h"
#include "pxr/base/tf/pyResultConversions.h"

#include "pxr/usd/usd/pyConversions.h"

#include "attributeProxy.h"
#include "displayGroupProxy.h"
#include "metadataProxy.h"
#include "objectProxy.h"
#include "primProxy.h"
#include "proxyBase.h"
#include "relationshipProxy.h"
#include "variantSetsProxy.h"

using namespace boost::python;

PXR_NAMESPACE_USING_DIRECTIVE

static VtValue _GetValueMetadata(UsdQt_MetadataProxyPtr proxy) {
    VtValue result;
    if (!proxy->GetValue(&result)) {
        // TODO: Throw error.
        return VtValue();
    }
    return result;
}

static VtValue _GetValueMetadataDictKey(UsdQt_MetadataDictKeyProxyPtr proxy) {
    VtValue result;
    if (!proxy->GetValue(&result)) {
        // TODO: Throw error.
        return VtValue();
    }
    return result;
}

static VtValue _GetValueAttribute(UsdQt_AttributeProxyPtr proxy,
                                  UsdTimeCode time) {
    VtValue result;
    if (!proxy->Get(&result, time)) {
        // TODO: Throw error.
        return VtValue();
    }
    return result;
}

static SdfPathVector _GetTargets(UsdQt_RelationshipProxyPtr proxy) {
    SdfPathVector result;
    if (!proxy->GetTargets(&result)) {
        // TODO: Throw error.
        return SdfPathVector();
    }
    return result;
}

static bool _SetValueAttribute(UsdQt_AttributeProxyPtr proxy, object value,
                               UsdTimeCode time) {
    SdfValueTypeName typeName = proxy->GetTypeName();
    if (!typeName) {
        // Should this be a hard rule?  Should this be moved into the C++
        // API?
        TF_CODING_ERROR("Cannot set value on attributes with mixed types.");
        return false;
    }
    return proxy->Set(UsdPythonToSdfType(value, typeName), time);
}

static bool _SetValueMetadata(UsdQt_MetadataProxyPtr proxy, object value) {
    VtValue result;
    if (!UsdPythonToMetadataValue(proxy->GetName(), TfToken(), value,
                                  &result)) {
        TF_CODING_ERROR("Cannot convert python value for metadata set.");
        return false;
    }

    return proxy->SetValue(result);
}

static bool _SetValueMetadataDictKey(UsdQt_MetadataDictKeyProxyPtr proxy,
                                     VtValue value) {
    return proxy->SetValue(value);
}

static std::string _VariantSetsRepr(UsdQt_VariantSetsProxyPtr proxy) {
    return TfStringPrintf("UsdQt_VariantSetsProxy(%zu objects)",
                          proxy->GetSize());
}

static std::string _VariantSetRepr(UsdQt_VariantSetProxyPtr proxy) {
    return TfStringPrintf("UsdQt_VariantSetProxy(%zu objects)",
                          proxy->GetSize());
}

static std::string _PrimRepr(UsdQt_PrimProxyPtr proxy) {
    return TfStringPrintf("UsdQt_PrimProxy(%zu objects)", proxy->GetSize());
}

static std::string _AttributeRepr(UsdQt_AttributeProxyPtr proxy) {
    return TfStringPrintf("UsdQt_AttributeProxy('%s' for '%zu' objects)",
                          proxy->GetName().GetText(), proxy->GetSize());
}

static std::string _RelationshipRepr(UsdQt_RelationshipProxyPtr proxy) {
    return TfStringPrintf("UsdQt_RelationshipProxy('%s' for '%zu' objects)",
                          proxy->GetName().GetText(), proxy->GetSize());
}

static std::string _MetadataRepr(UsdQt_MetadataProxyPtr proxy) {
    return TfStringPrintf("UsdQt_MetadataProxy('%s' for '%zu' objects)",
                          proxy->GetName().GetText(), proxy->GetSize());
}

static std::string _MetadataDictKeyRepr(UsdQt_MetadataDictKeyProxyPtr proxy) {
    return TfStringPrintf(
        "UsdQt_MetadataDictKeyProxy('%s/%s' for '%zu' objects)",
        proxy->GetDictName().GetText(), proxy->GetEntryName().GetText(),
        proxy->GetSize());
}

static std::string _DisplayGroupRepr(UsdQt_DisplayGroupProxyPtr proxy) {
    return TfStringPrintf("UsdQt_DisplayGroupProxy('%s')",
                          proxy->GetName().GetText());
}

TF_REFPTR_CONST_VOLATILE_GET(UsdQt_ProxyBase)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_ObjectProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_VariantSetsProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_VariantSetProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_MetadataProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_MetadataDictKeyProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_AttributeProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_RelationshipProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_PrimProxy)
TF_REFPTR_CONST_VOLATILE_GET(UsdQt_DisplayGroupProxy)

void wrapOpinionProxy() {
    {
        typedef UsdQt_ProxyBase This;
        typedef UsdQt_ProxyBasePtr ThisPtr;
        class_<This, ThisPtr, boost::noncopyable>("_ProxyBase", no_init);
    }
    {
        typedef UsdQt_ObjectProxy This;
        typedef UsdQt_ObjectProxyPtr ThisPtr;
        class_<This, ThisPtr, boost::noncopyable,
               bases<UsdQt_ProxyBase>>("_ObjectProxy", no_init)
            .def("ContainsPathOrDescendent", &This::ContainsPathOrDescendent)
            .def("ContainsPath", &This::ContainsPath);
    }
    {
        typedef UsdQt_VariantSetsProxy This;
        typedef UsdQt_VariantSetsProxyPtr ThisPtr;
        std::vector<std::string> (This::*ThisGetNames)() const =
            &This::GetNames;
        class_<This, ThisPtr, boost::noncopyable, bases<UsdQt_ProxyBase>>(
            "_VariantSetsProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetNames", ThisGetNames,
                 return_value_policy<TfPySequenceToList>())
            .def("IsValid", &This::IsValid)
            .def("CreateVariantSetProxy", &This::CreateVariantSetProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("__repr__", &::_VariantSetsRepr);
        TfPyRegisterStlSequencesFromPython<UsdVariantSets>();
    }
    {
        typedef UsdQt_VariantSetProxy This;
        typedef UsdQt_VariantSetProxyPtr ThisPtr;

        class_<This, ThisPtr, boost::noncopyable,
               bases<UsdQt_ProxyBase>>("_VariantSetProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetName", &This::GetName)
            .def("GetVariantNames", &This::GetVariantNames,
                 return_value_policy<TfPySequenceToList>())
            .def("GetVariantSelection", &This::GetVariantSelection)
            .def("SetVariantSelection", &This::SetVariantSelection)
            .def("ClearVariantSelection", &This::ClearVariantSelection)
            .def("__repr__", &::_VariantSetRepr);
        TfPyRegisterStlSequencesFromPython<UsdVariantSet>();
    }
    {
        typedef UsdQt_MetadataProxy This;
        typedef UsdQt_MetadataProxyPtr ThisPtr;

        class_<This, ThisPtr, boost::noncopyable,
               bases<UsdQt_ProxyBase>>("_MetadataProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetValue", &::_GetValueMetadata)
            .def("SetValue", &::_SetValueMetadata)
            .def("ClearValue", &This::ClearValue)
            .def("GetName", &This::GetName)
            .def("GetType", &This::GetType)
            .def("GetDictKeys", &This::GetDictKeys)
            .def("GetObjects", &This::GetObjects,
                 return_value_policy<TfPySequenceToList>())
            .def("CreateMetadataDictKeyProxy",
                 &This::CreateMetadataDictKeyProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("GetSize", &This::GetSize)
            .def("__repr__", &_MetadataRepr);
    }
    {
        typedef UsdQt_MetadataDictKeyProxy This;
        typedef UsdQt_MetadataDictKeyProxyPtr ThisPtr;
        class_<This, ThisPtr, boost::noncopyable,
               bases<UsdQt_ProxyBase>>("_MetadataDictKeyProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetValue", &::_GetValueMetadataDictKey)
            .def("GetType", &This::GetType)
            .def("SetValue", &::_SetValueMetadataDictKey)
            .def("ClearValue", &This::ClearValue)
            .def("GetDictName", &This::GetDictName)
            .def("GetEntryName", &This::GetEntryName)
            .def("__repr__", &_MetadataDictKeyRepr);
    }
    {
        typedef UsdQt_AttributeProxy This;
        typedef UsdQt_AttributeProxyPtr ThisPtr;
        class_<This, ThisPtr, boost::noncopyable, bases<UsdQt_ObjectProxy>>(
            "_AttributeProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetName", &This::GetName)
            .def("Get", &::_GetValueAttribute)
            .def("Set", &::_SetValueAttribute)
            .def("Clear", &This::Clear)
            .def("ClearAtTime", &This::ClearAtTime)
            .def("Block", &This::Block)
            .def("GetTypeName", &This::GetTypeName)
            .def("GetVariability", &This::GetVariability)
            .def("GetAllowedTokens", &This::GetAllowedTokens)
            .def("GetDocumentation", &This::GetDocumentation)
            .def("GetMetadataFields", &This::GetMetadataFields)
            .def("CreateMetadataProxy", &This::CreateMetadataProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("GetSize", &This::GetSize)
            .def("GetAttributes", &This::GetAttributes,
                 return_value_policy<TfPySequenceToList>())
            .def("IsDefined", &This::IsDefined)
            .def("IsAuthored", &This::IsAuthored)
            .def("IsAuthoredAt", &This::IsAuthoredAt)
            .def("__repr__", &::_AttributeRepr);
    }
    {
        typedef UsdQt_RelationshipProxy This;
        typedef UsdQt_RelationshipProxyPtr ThisPtr;

        class_<This, ThisPtr, boost::noncopyable, bases<UsdQt_ObjectProxy>>(
            "_RelationshipProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetName", &This::GetName)
            .def("GetTargets", &::_GetTargets)
            .def("ClearTargets", &This::ClearTargets)
            .def("BlockTargets", &This::BlockTargets)
            .def("GetDocumentation", &This::GetDocumentation)
            .def("GetMetadataFields", &This::GetMetadataFields)
            .def("CreateMetadataProxy", &This::CreateMetadataProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("IsDefined", &This::IsDefined)
            .def("IsAuthored", &This::IsAuthored)
            .def("IsAuthoredAt", &This::IsAuthoredAt)
            .def("__repr__", &::_RelationshipRepr);
    }
    {
        typedef UsdQt_PrimProxy This;
        typedef UsdQt_PrimProxyPtr ThisPtr;

        class_<This, ThisPtr, boost::noncopyable, bases<UsdQt_ObjectProxy>>(
            "_PrimProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetNames", &This::GetNames,
                 return_value_policy<TfPySequenceToList>())
            .def("GetPrims", &This::GetPrims,
                 return_value_policy<TfPySequenceToList>())
            .def("GetAttributeNames", &This::GetAttributeNames)
            .def("GetRelationshipNames", &This::GetRelationshipNames)
            .def("GetMetadataFields", &This::GetMetadataFields)
            .def("CreateAttributeProxy", &This::CreateAttributeProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("CreateRelationshipProxy", &This::CreateRelationshipProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("CreateMetadataProxy", &This::CreateMetadataProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("CreateVariantSetsProxy", &This::CreateVariantSetsProxy,
                 return_value_policy<TfPyRefPtrFactory<>>())
            .def("ClearExpired", &This::ClearExpired)
            .def("__repr__", &::_PrimRepr);
    }
    {
        typedef UsdQt_DisplayGroupProxy This;
        typedef UsdQt_DisplayGroupProxyPtr ThisPtr;

        class_<This, ThisPtr, boost::noncopyable,
               bases<UsdQt_ProxyBase>>("_DisplayGroupProxy", no_init)
            .def(TfPyRefAndWeakPtr())
            .def(TfMakePyConstructor(&This::New))
            .def("GetName", &This::GetName)
            .def("__repr__", &::_DisplayGroupRepr);
    }
}

max_stars_repo_name: chunkified/usd-qt

max_issues_repo_name: chunkified/usd-qt

max_forks_repo_name: chunkified/usd-qt


text:
// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * 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 The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/io/read_1dsfm.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <gflags/gflags.h>

#include <algorithm>
#include <fstream>  // NOLINT
#include <string>
#include <utility>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/sfm/view.h"
#include "theia/sfm/track.h"
#include "theia/sfm/types.h"
#include "theia/util/filesystem.h"
#include "theia/util/map_util.h"

namespace theia {

class Input1DSFM {
 public:
  Input1DSFM(const std::string& dataset_directory,
             Reconstruction* reconstruction,
             ViewGraph* view_graph)
      : dataset_directory_(dataset_directory),
        reconstruction_(reconstruction),
        view_graph_(view_graph) {}

  bool ReadCC(std::unordered_set<int>* valid_image_index);
  bool ReadListsFile(const std::unordered_set<int>& valid_image_index);
  bool ReadTracks();
  bool ReadCoords();
  bool ReadEGs();

 private:
  bool ReadCoordsHeaderLine(const std::string& line,
                            ViewId* image_index,
                            int* num_keys);

  const std::string& dataset_directory_;
  Reconstruction* reconstruction_;
  ViewGraph* view_graph_;

  // Maps image id -> [ feature id -> feature coordinate].
  std::unordered_map<ViewId, std::vector<Feature> > feature_coordinates_;
  // Maps image id -> [ feature id -> feature color].
  std::unordered_map<ViewId, std::vector<Eigen::Matrix<uint8_t, 3, 1> > >
      feature_colors_;
};

// Reads the connected components file.
bool Input1DSFM::ReadCC(std::unordered_set<int>* valid_image_index) {
  const std::string cc_filename = dataset_directory_ + "/cc.txt";

  std::ifstream ifs(cc_filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    LOG(ERROR) << "Cannot read the cc file from " << cc_filename;
    return false;
  }

  while (!ifs.eof()) {
    int img_index;
    ifs >> img_index;
    valid_image_index->insert(img_index);
  }

  return true;
}

// Reads the list file and ignores the focal length since it can be recovered
// from the coords file.
bool Input1DSFM::ReadListsFile(
    const std::unordered_set<int>& valid_image_index) {
  const std::string list_filename = dataset_directory_ + "/list.txt";
  std::ifstream ifs(list_filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    LOG(ERROR) << "Cannot read the list file from " << list_filename;
    return false;
  }

  const char space = static_cast<char>(' ');
  while (!ifs.eof()) {
    // Read in the filename.
    std::string filename, truncated_filename;

    ifs >> filename;
    if (filename.length() == 0) {
      break;
    }
    CHECK(theia::GetFilenameFromFilepath(filename, true, &truncated_filename));
    const ViewId view_id = reconstruction_->AddView(truncated_filename);
    CHECK_NE(view_id, kInvalidViewId);

    // Check to see if the exif focal length is given.
    double focal_length = 0;
    if (ifs.peek() == space) {
      int temp;
      ifs >> temp;
      ifs >> focal_length;
    }

    // If the view is not in the connected component remove it. Adding it first
    // then removing it allows for our ViewIds to stay in sync with the index of
    // the image in the lists file. This will simplify the indexing throughout
    // the entire program.
    if (!ContainsKey(valid_image_index, view_id)) {
      reconstruction_->RemoveView(view_id);
      continue;
    }

    if (focal_length != 0) {
      reconstruction_->MutableView(view_id)
          ->MutableCameraIntrinsicsPrior()
          ->focal_length.value = focal_length;
      reconstruction_->MutableView(view_id)
          ->MutableCameraIntrinsicsPrior()
          ->focal_length.is_set = true;
      LOG(INFO) << "Adding image " << truncated_filename
                << " with focal length: " << focal_length;
    } else {
      LOG(INFO) << "Adding image " << truncated_filename
                << " with focal length: UNKNOWN";
    }
  }
  return true;
}

bool Input1DSFM::ReadCoordsHeaderLine(const std::string& line,
                                      ViewId* view_id,
                                      int* num_keys) {
  float principal_point_x, principal_point_y, focal_length;
  char name[256];
  sscanf(line.c_str(),
         "#index = %d, name = %s keys = %d, px = %f, py = %f, focal = %f",
         view_id,
         name,
         num_keys,
         &principal_point_x,
         &principal_point_y,
         &focal_length);

  View* view = reconstruction_->MutableView(*view_id);
  if (view == nullptr) {
    return false;
  }

  // Set the metadata.
  CameraIntrinsicsPrior* prior = view->MutableCameraIntrinsicsPrior();
  prior->image_width = principal_point_x * 2.0;
  prior->image_height = principal_point_y * 2.0;
  prior->principal_point[0].value = principal_point_x;
  prior->principal_point[0].is_set = true;
  prior->principal_point[1].value = principal_point_y;
  prior->principal_point[1].is_set = true;
  return true;
}

// Reads the coords file. Only the coords with a valid track in the connected
// component are kept.
bool Input1DSFM::ReadCoords() {
  const std::string coords_filename = dataset_directory_ + "/coords.txt";
  std::ifstream ifs(coords_filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    LOG(ERROR) << "Cannot read the coords file from " << coords_filename;
    return false;
  }

  feature_coordinates_.reserve(reconstruction_->NumViews());
  feature_colors_.reserve(reconstruction_->NumViews());
  while (!ifs.eof()) {
    std::string line;
    std::getline(ifs, line);
    if (ifs.eof()) {
      break;
    }

    int num_keys;
    ViewId view_id;
    // If the image is not in the connected component then do not read it.
    if (!ReadCoordsHeaderLine(line, &view_id, &num_keys)) {
      for (int i = 0; i < num_keys; i++) {
        std::getline(ifs, line);
      }
      continue;
    }

    auto& features = feature_coordinates_[view_id];
    features.reserve(num_keys);
    auto& colors = feature_colors_[view_id];
    colors.reserve(num_keys);

    Eigen::Vector2d keypoint;
    Eigen::Vector3i color;
    for (int i = 0; i < num_keys; i++) {
      std::getline(ifs, line);
      sscanf(line.c_str(), "%*d %lf %lf 0 0 %d %d %d", &keypoint[0],
             &keypoint[1], &color[0], &color[1], &color[2]);
      features.emplace_back(keypoint);
      colors.emplace_back(color.cast<uint8_t>());
    }
  }

  return true;
}

bool Input1DSFM::ReadTracks() {
  const std::string tracks_filename = dataset_directory_ + "/tracks.txt";
  std::ifstream ifs(tracks_filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    LOG(ERROR) << "Cannot read the coords file from " << tracks_filename;
    return false;
  }

  // Read number of tracks.
  int num_tracks;
  ifs >> num_tracks;

  for (int i = 0; i < num_tracks; i++) {
    int num_features;
    ifs >> num_features;

    std::vector<std::pair<ViewId, Feature> > track;
    track.reserve(num_features);
    int feature_id;
    ViewId view_id;
    Eigen::Vector3f color = Eigen::Vector3f::Zero();
    for (int j = 0; j < num_features; j++) {
      ifs >> view_id;
      ifs >> feature_id;

      // Aggregate the features that form this track.
      const auto& features = FindOrDie(feature_coordinates_, view_id);
      const Feature& feature = features[feature_id];
      track.emplace_back(view_id, feature);

      // Add the color of the feature to form the mean color of the point.
      const auto& colors = FindOrDie(feature_colors_, view_id);
      color += colors[feature_id].cast<float>();
    }
    // Add the track to the reconstruction.
    const TrackId track_id = reconstruction_->AddTrack(track);
    CHECK_NE(track_id, kInvalidTrackId);

    // Set the color of the track.
    color /= static_cast<float>(track.size());
    *reconstruction_->MutableTrack(track_id)->MutableColor() =
        color.cast<uint8_t>();
  }

  return true;
}

// Reads the epipolar geometry files.
bool Input1DSFM::ReadEGs() {
  const std::string eg_filename = dataset_directory_ + "/EGs.txt";
  std::ifstream ifs(eg_filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    LOG(ERROR) << "Cannot read the EG file from " << eg_filename;
    return false;
  }

  const Eigen::Matrix3d bundler_to_theia =
      Eigen::Vector3d(1.0, -1.0, -1.0).asDiagonal();
  while (!ifs.eof()) {
    TwoViewInfo info;
    ViewId view_id1, view_id2;
    ifs >> view_id1;
    ifs >> view_id2;

    // The rotation defines the camera 2 to camera 1 transformation in row-major
    // order). We want a camera 1 to camera 2 transformation so we read in the
    // transpose (i.e., column-major order).
    Eigen::Matrix3d rotation;
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        ifs >> rotation(i, j);
      }
    }

    rotation = bundler_to_theia * rotation.transpose() * bundler_to_theia;

    // Convert to angle axis.
    ceres::RotationMatrixToAngleAxis(rotation.data(), info.rotation_2.data());

    // Assign the position.
    ifs >> info.position_2[0];
    ifs >> info.position_2[1];
    ifs >> info.position_2[2];

    info.position_2 = bundler_to_theia * info.position_2;

    // Add the focal lengths. If they are known from EXIF, add that value
    // otherwise add a focal length guess correspdonding to a median viewing
    // angle.
    const CameraIntrinsicsPrior prior1 =
        reconstruction_->View(view_id1)->CameraIntrinsicsPrior();
    const CameraIntrinsicsPrior prior2 =
        reconstruction_->View(view_id2)->CameraIntrinsicsPrior();
    if (prior1.focal_length.is_set) {
      info.focal_length_1 = prior1.focal_length.value;
    } else {
      info.focal_length_1 = 1.2 * prior1.principal_point[0].value;
    }

    if (prior2.focal_length.is_set) {
      info.focal_length_2 = prior2.focal_length.value;
    } else {
      info.focal_length_2 = 1.2 * prior2.principal_point[0].value;
    }

    // Add the number of inliers.
    const std::vector<ViewId> views = {view_id1, view_id2};
    const std::vector<TrackId> common_tracks =
        FindCommonTracksInViews(*reconstruction_, views);
    info.num_verified_matches = common_tracks.size();

    // Add the match to the output.
    if (reconstruction_->View(view_id1) != nullptr &&
        reconstruction_->View(view_id2) != nullptr) {
      view_graph_->AddEdge(view_id1, view_id2, info);
    }
  }
  return true;
}

bool Read1DSFM(const std::string& dataset_directory,
               Reconstruction* reconstruction,
               ViewGraph* view_graph) {
  CHECK_NOTNULL(reconstruction);
  CHECK_NOTNULL(view_graph);

  Input1DSFM input_reader(dataset_directory, reconstruction, view_graph);

  LOG(INFO) << "Reading connected components.";
  std::unordered_set<int> valid_images;
  if (!input_reader.ReadCC(&valid_images)) {
    return false;
  }

  LOG(INFO) << "Readings lists file.";
  if (!input_reader.ReadListsFile(valid_images)) {
    return false;
  }

  LOG(INFO) << "Reading feature coordinates.";
  if (!input_reader.ReadCoords()) {
    return false;
  }

  LOG(INFO) << "Reading tracks.";
  if (!input_reader.ReadTracks()) {
    return false;
  }

  LOG(INFO) << "Reading epipolar geometries.";
  if (!input_reader.ReadEGs()) {
    return false;
  }

  return true;
}

}  // namespace theia

max_stars_repo_name: hunter-packages/TheiaSfM

max_issues_repo_name: hunter-packages/TheiaSfM

max_forks_repo_name: hunter-packages/TheiaSfM