<commit_before>/*====================================================================
Copyright(c) 2016 Adam Rankin

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.
====================================================================*/

// Local includes
#include "pch.h"
#include "IGTLinkClient.h"
#include "IGTCommon.h"
#include "TrackedFrameMessage.h"

// IGT includes
#include "igtlCommandMessage.h"
#include "igtlCommon.h"
#include "igtlMessageHeader.h"
#include "igtlOSUtil.h"
#include "igtlStatusMessage.h"

// STD includes
#include <chrono>
#include <regex>

// Windows includes
#include <collection.h>
#include <pplawait.h>
#include <ppltasks.h>
#include <robuffer.h>

using namespace Concurrency;
using namespace Windows::Foundation;
using namespace Platform::Collections;
using namespace Windows::Storage::Streams;
using namespace Windows::UI::Xaml::Controls;
using namespace Windows::UI::Xaml::Media;
using namespace Windows::Data::Xml::Dom;

namespace UWPOpenIGTLink
{
  const int IGTLinkClient::CLIENT_SOCKET_TIMEOUT_MSEC = 500;
  const uint32 IGTLinkClient::MESSAGE_LIST_MAX_SIZE = 200;

  //----------------------------------------------------------------------------
  IGTLinkClient::IGTLinkClient()
    : m_igtlMessageFactory(igtl::MessageFactory::New())
    , m_clientSocket(igtl::ClientSocket::New())
  {
    m_igtlMessageFactory->AddMessageType("TRACKEDFRAME", (igtl::MessageFactory::PointerToMessageBaseNew)&igtl::TrackedFrameMessage::New);
    ServerHost = L"127.0.0.1";
    ServerPort = 18944;
    ServerIGTLVersion = IGTL_HEADER_VERSION_2;

    m_frameSize.assign(3, 0);
  }

  //----------------------------------------------------------------------------
  IGTLinkClient::~IGTLinkClient()
  {
    Disconnect();
    auto disconnectTask = concurrency::create_task([this]()
    {
      while (Connected)
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(33));
      }
    });
    disconnectTask.wait();
  }

  //----------------------------------------------------------------------------
  IAsyncOperation<bool>^ IGTLinkClient::ConnectAsync(double timeoutSec)
  {
    Disconnect();

    m_receiverPumpTokenSource = cancellation_token_source();

    return create_async([ = ]() -> bool
    {
      const int retryDelaySec = 1.0;
      int errorCode = 1;
      auto start = std::chrono::high_resolution_clock::now();
      while (errorCode != 0)
      {
        std::wstring wideStr(ServerHost->Begin());
        std::string str(wideStr.begin(), wideStr.end());
        errorCode = m_clientSocket->ConnectToServer(str.c_str(), ServerPort);
        if (errorCode == 0)
        {
          break;
        }
        std::chrono::duration<double, std::milli> timeDiff = std::chrono::high_resolution_clock::now() - start;
        if (timeDiff.count() > timeoutSec * 1000)
        {
          // time is up
          break;
        }
        std::this_thread::sleep_for(std::chrono::seconds(retryDelaySec));
      }

      if (errorCode != 0)
      {
        return false;
      }

      m_clientSocket->SetTimeout(CLIENT_SOCKET_TIMEOUT_MSEC);
      m_clientSocket->SetReceiveBlocking(true);

      // We're connected, start the data receiver thread
      create_task([this]()
      {
        DataReceiverPump(this, m_receiverPumpTokenSource.get_token());
      });

      return true;
    });
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::Disconnect()
  {
    m_receiverPumpTokenSource.cancel();

    {
      std::lock_guard<std::mutex> guard(m_socketMutex);
      m_clientSocket->CloseSocket();
    }
  }

  //----------------------------------------------------------------------------
  TrackedFrame^ IGTLinkClient::GetTrackedFrame(double lastKnownTimestamp)
  {
    igtl::TrackedFrameMessage::Pointer trackedFrameMsg = nullptr;
    {
      // Retrieve the next available tracked frame reply
      std::lock_guard<std::mutex> guard(m_messageListMutex);
      for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
      {
        if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
        {
          trackedFrameMsg = dynamic_cast<igtl::TrackedFrameMessage*>((*replyIter).GetPointer());
          break;
        }
      }

      if (trackedFrameMsg == nullptr)
      {
        // No message found
        return nullptr;
      }
    }

    auto ts = igtl::TimeStamp::New();
    trackedFrameMsg->GetTimeStamp(ts);
    if (ts->GetTimeStamp() <= lastKnownTimestamp)
    {
      // No new messages since requested timestamp
      if (m_receivedUWPMessages.find(lastKnownTimestamp) != m_receivedUWPMessages.end())
      {
        return m_receivedUWPMessages.at(lastKnownTimestamp);
      }
      return nullptr;
    }

    auto frame = ref new TrackedFrame();
    m_receivedUWPMessages[ts->GetTimeStamp()] = frame;

    // Tracking/other related fields
    for (auto& pair : trackedFrameMsg->GetMetaData())
    {
      std::wstring keyWideStr(pair.first.begin(), pair.first.end());
      std::wstring valueWideStr(pair.second.begin(), pair.second.end());
      frame->SetCustomFrameField(keyWideStr, valueWideStr);
    }

    for (auto& pair : trackedFrameMsg->GetCustomFrameFields())
    {
      frame->SetCustomFrameField(pair.first, pair.second);
    }

    // Image related fields
    frame->SetFrameSize(trackedFrameMsg->GetFrameSize());
    frame->Timestamp = ts->GetTimeStamp();
    frame->ImageSizeBytes = trackedFrameMsg->GetImageSizeInBytes();
    frame->SetImageData(trackedFrameMsg->GetImage());
    frame->NumberOfComponents = trackedFrameMsg->GetNumberOfComponents();
    frame->ScalarType = trackedFrameMsg->GetScalarType();
    frame->SetEmbeddedImageTransform(trackedFrameMsg->GetEmbeddedImageTransform());
    frame->ImageType = (uint16)trackedFrameMsg->GetImageType();
    frame->ImageOrientation = (uint16)trackedFrameMsg->GetImageOrientation();
    frame->SetFrameTransformsInternal(trackedFrameMsg->GetFrameTransforms());

    return frame;
  }

  //----------------------------------------------------------------------------
  Command^ IGTLinkClient::GetCommand(double lastKnownTimestamp)
  {
    igtl::MessageBase::Pointer igtMessage = nullptr;
    {
      // Retrieve the next available tracked frame reply
      std::lock_guard<std::mutex> guard(m_messageListMutex);
      for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
      {
        if (typeid(*(*replyIter)) == typeid(igtl::RTSCommandMessage))
        {
          igtMessage = *replyIter;
          break;
        }
      }

      if (igtMessage == nullptr)
      {
        return nullptr;
      }
    }

    auto ts = igtl::TimeStamp::New();
    igtMessage->GetTimeStamp(ts);
    if (ts->GetTimeStamp() <= lastKnownTimestamp)
    {
      return nullptr;
    }

    auto cmd = ref new Command();
    auto cmdMsg = dynamic_cast<igtl::RTSCommandMessage*>(igtMessage.GetPointer());

    cmd->CommandContent = ref new Platform::String(std::wstring(cmdMsg->GetCommandContent().begin(), cmdMsg->GetCommandContent().end()).c_str());
    cmd->CommandName = ref new Platform::String(std::wstring(cmdMsg->GetCommandName().begin(), cmdMsg->GetCommandName().end()).c_str());
    cmd->OriginalCommandId = cmdMsg->GetCommandId();
    cmd->Timestamp = lastKnownTimestamp;

    XmlDocument^ doc = ref new XmlDocument();
    doc->LoadXml(cmd->CommandContent);

    for (IXmlNode^ node : doc->ChildNodes)
    {
      if (dynamic_cast<Platform::String^>(node->NodeName) == L"Result")
      {
        cmd->Result = (dynamic_cast<Platform::String^>(node->NodeValue) == L"true");
        break;
      }
    }

    if (!cmd->Result)
    {
      bool found(false);
      // Parse for the error string
      for (IXmlNode^ node : doc->ChildNodes)
      {
        if (dynamic_cast<Platform::String^>(node->NodeName) == L"Error")
        {
          cmd->ErrorString = dynamic_cast<Platform::String^>(node->NodeValue);
          found = true;
          break;
        }
      }

      if (!found)
      {
        // TODO : quiet error reporting
      }
    }

    for (auto& pair : cmdMsg->GetMetaData())
    {
      std::wstring keyWideStr(pair.first.begin(), pair.first.end());
      std::wstring valueWideStr(pair.second.begin(), pair.second.end());
      cmd->Parameters->Insert(ref new Platform::String(keyWideStr.c_str()), ref new Platform::String(valueWideStr.c_str()));
    }

    return cmd;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::SendMessage(igtl::MessageBase::Pointer packedMessage)
  {
    int success = 0;
    {
      std::lock_guard<std::mutex> guard(m_socketMutex);
      success = m_clientSocket->Send(packedMessage->GetBufferPointer(), packedMessage->GetBufferSize());
    }
    if (!success)
    {
      std::cerr << "OpenIGTLink client couldn't send message to server." << std::endl;
      return false;
    }
    return true;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::SendMessage(MessageBasePointerPtr messageBasePointerPtr)
  {
    igtl::MessageBase::Pointer* messageBasePointer = (igtl::MessageBase::Pointer*)(messageBasePointerPtr);
    return SendMessage(*messageBasePointer);
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::DataReceiverPump(IGTLinkClient^ self, concurrency::cancellation_token token)
  {
    LOG_TRACE(L"IGTLinkClient::DataReceiverPump");

    while (!token.is_canceled())
    {
      auto headerMsg = self->m_igtlMessageFactory->CreateHeaderMessage(IGTL_HEADER_VERSION_1);

      // Receive generic header from the socket
      int numOfBytesReceived = 0;
      {
        std::lock_guard<std::mutex> guard(self->m_socketMutex);
        if (!self->m_clientSocket->GetConnected())
        {
          // We've become disconnected while waiting for the socket, we're done here!
          return;
        }
        numOfBytesReceived = self->m_clientSocket->Receive(headerMsg->GetBufferPointer(), headerMsg->GetBufferSize());
      }
      if (numOfBytesReceived == 0  // No message received
          || numOfBytesReceived != headerMsg->GetBufferSize() // Received data is not as we expected
         )
      {
        // Failed to receive data, maybe the socket is disconnected
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        continue;
      }

      int c = headerMsg->Unpack(1);
      if (!(c & igtl::MessageHeader::UNPACK_HEADER))
      {
        std::cerr << "Failed to receive message (invalid header)" << std::endl;
        continue;
      }

      igtl::MessageBase::Pointer bodyMsg = nullptr;
      try
      {
        bodyMsg = self->m_igtlMessageFactory->CreateReceiveMessage(headerMsg);
      }
      catch (const std::exception&)
      {
        // Message header was not correct, skip this message
        // Will be impossible to tell if the body of this message is in the socket... this is a pretty bad corruption.
        // Force re-connect?
        LOG_TRACE("Corruption in the message header. Serious error.");
        continue;
      }

      if (bodyMsg.IsNull())
      {
        LOG_TRACE("Unable to create message of type: " << headerMsg->GetMessageType());
        continue;
      }

      // Accept all messages but status messages, they are used as a keep alive mechanism
      if (typeid(*bodyMsg) != typeid(igtl::StatusMessage))
      {
        {
          std::lock_guard<std::mutex> guard(self->m_socketMutex);
          if (!self->m_clientSocket->GetConnected())
          {
            // We've become disconnected while waiting for the socket, we're done here!
            return;
          }
          self->m_clientSocket->Receive(bodyMsg->GetBufferBodyPointer(), bodyMsg->GetBufferBodySize());
        }

        int c = bodyMsg->Unpack(1);
        if (!(c & igtl::MessageHeader::UNPACK_BODY))
        {
          LOG_TRACE("Failed to receive reply (invalid body)");
          continue;
        }

        {
          // save reply
          std::lock_guard<std::mutex> guard(self->m_messageListMutex);
          self->m_receivedMessages.push_back(bodyMsg);
        }
      }
      else
      {
        std::lock_guard<std::mutex> guard(self->m_socketMutex);
        self->m_clientSocket->Skip(headerMsg->GetBodySizeToRead(), 0);
      }

      if (self->m_receivedMessages.size() > MESSAGE_LIST_MAX_SIZE)
      {
        std::lock_guard<std::mutex> guard(self->m_messageListMutex);

        // erase the front N results
        uint32 toErase = self->m_receivedMessages.size() - MESSAGE_LIST_MAX_SIZE;
        self->m_receivedMessages.erase(self->m_receivedMessages.begin(), self->m_receivedMessages.begin() + toErase);
      }

      auto oldestTimestamp = self->GetOldestTrackedFrameTimestamp();
      if (oldestTimestamp > 0)
      {
        for (auto it = self->m_receivedUWPMessages.begin(); it != self->m_receivedUWPMessages.end();)
        {
          if (it->first < oldestTimestamp)
          {
            it = self->m_receivedUWPMessages.erase(it);
          }
          else
          {
            ++it;
          }
        }
      }
    }

    return;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::SocketReceive(void* data, int length)
  {
    std::lock_guard<std::mutex> guard(m_socketMutex);
    return m_clientSocket->Receive(data, length);
  }

  //----------------------------------------------------------------------------
  double IGTLinkClient::GetLatestTrackedFrameTimestamp()
  {
    // Retrieve the next available tracked frame reply
    std::lock_guard<std::mutex> guard(m_messageListMutex);
    for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
    {
      if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
      {
        igtl::TimeStamp::Pointer ts = igtl::TimeStamp::New();
        (*replyIter)->GetTimeStamp(ts);
        return ts->GetTimeStamp();
      }
    }
    return -1.0;
  }

  //----------------------------------------------------------------------------
  double IGTLinkClient::GetOldestTrackedFrameTimestamp()
  {
    // Retrieve the next available tracked frame reply
    std::lock_guard<std::mutex> guard(m_messageListMutex);
    for (auto replyIter = m_receivedMessages.begin(); replyIter != m_receivedMessages.end(); ++replyIter)
    {
      if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
      {
        igtl::TimeStamp::Pointer ts = igtl::TimeStamp::New();
        (*replyIter)->GetTimeStamp(ts);
        return ts->GetTimeStamp();
      }
    }
    return -1.0;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::ServerPort::get()
  {
    return m_serverPort;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerPort::set(int arg)
  {
    m_serverPort = arg;
  }

  //----------------------------------------------------------------------------
  Platform::String^ IGTLinkClient::ServerHost::get()
  {
    return m_serverHost;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerHost::set(Platform::String^ arg)
  {
    m_serverHost = arg;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::ServerIGTLVersion::get()
  {
    return m_serverIGTLVersion;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerIGTLVersion::set(int arg)
  {
    m_serverIGTLVersion = arg;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::Connected::get()
  {
    return m_clientSocket->GetConnected();
  }
}<commit_msg>Adding quiet reporting<commit_after>/*====================================================================
Copyright(c) 2016 Adam Rankin

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.
====================================================================*/

// Local includes
#include "pch.h"
#include "IGTLinkClient.h"
#include "IGTCommon.h"
#include "TrackedFrameMessage.h"

// IGT includes
#include "igtlCommandMessage.h"
#include "igtlCommon.h"
#include "igtlMessageHeader.h"
#include "igtlOSUtil.h"
#include "igtlStatusMessage.h"

// STD includes
#include <chrono>
#include <regex>

// Windows includes
#include <collection.h>
#include <pplawait.h>
#include <ppltasks.h>
#include <robuffer.h>

using namespace Concurrency;
using namespace Windows::Foundation;
using namespace Platform::Collections;
using namespace Windows::Storage::Streams;
using namespace Windows::UI::Xaml::Controls;
using namespace Windows::UI::Xaml::Media;
using namespace Windows::Data::Xml::Dom;

namespace UWPOpenIGTLink
{
  const int IGTLinkClient::CLIENT_SOCKET_TIMEOUT_MSEC = 500;
  const uint32 IGTLinkClient::MESSAGE_LIST_MAX_SIZE = 200;

  //----------------------------------------------------------------------------
  IGTLinkClient::IGTLinkClient()
    : m_igtlMessageFactory(igtl::MessageFactory::New())
    , m_clientSocket(igtl::ClientSocket::New())
  {
    m_igtlMessageFactory->AddMessageType("TRACKEDFRAME", (igtl::MessageFactory::PointerToMessageBaseNew)&igtl::TrackedFrameMessage::New);
    ServerHost = L"127.0.0.1";
    ServerPort = 18944;
    ServerIGTLVersion = IGTL_HEADER_VERSION_2;

    m_frameSize.assign(3, 0);
  }

  //----------------------------------------------------------------------------
  IGTLinkClient::~IGTLinkClient()
  {
    Disconnect();
    auto disconnectTask = concurrency::create_task([this]()
    {
      while (Connected)
      {
        std::this_thread::sleep_for(std::chrono::milliseconds(33));
      }
    });
    disconnectTask.wait();
  }

  //----------------------------------------------------------------------------
  IAsyncOperation<bool>^ IGTLinkClient::ConnectAsync(double timeoutSec)
  {
    Disconnect();

    m_receiverPumpTokenSource = cancellation_token_source();

    return create_async([ = ]() -> bool
    {
      const int retryDelaySec = 1.0;
      int errorCode = 1;
      auto start = std::chrono::high_resolution_clock::now();
      while (errorCode != 0)
      {
        std::wstring wideStr(ServerHost->Begin());
        std::string str(wideStr.begin(), wideStr.end());
        errorCode = m_clientSocket->ConnectToServer(str.c_str(), ServerPort);
        if (errorCode == 0)
        {
          break;
        }
        std::chrono::duration<double, std::milli> timeDiff = std::chrono::high_resolution_clock::now() - start;
        if (timeDiff.count() > timeoutSec * 1000)
        {
          // time is up
          break;
        }
        std::this_thread::sleep_for(std::chrono::seconds(retryDelaySec));
      }

      if (errorCode != 0)
      {
        return false;
      }

      m_clientSocket->SetTimeout(CLIENT_SOCKET_TIMEOUT_MSEC);
      m_clientSocket->SetReceiveBlocking(true);

      // We're connected, start the data receiver thread
      create_task([this]()
      {
        DataReceiverPump(this, m_receiverPumpTokenSource.get_token());
      });

      return true;
    });
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::Disconnect()
  {
    m_receiverPumpTokenSource.cancel();

    {
      std::lock_guard<std::mutex> guard(m_socketMutex);
      m_clientSocket->CloseSocket();
    }
  }

  //----------------------------------------------------------------------------
  TrackedFrame^ IGTLinkClient::GetTrackedFrame(double lastKnownTimestamp)
  {
    igtl::TrackedFrameMessage::Pointer trackedFrameMsg = nullptr;
    {
      // Retrieve the next available tracked frame reply
      std::lock_guard<std::mutex> guard(m_messageListMutex);
      for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
      {
        if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
        {
          trackedFrameMsg = dynamic_cast<igtl::TrackedFrameMessage*>((*replyIter).GetPointer());
          break;
        }
      }

      if (trackedFrameMsg == nullptr)
      {
        // No message found
        return nullptr;
      }
    }

    auto ts = igtl::TimeStamp::New();
    trackedFrameMsg->GetTimeStamp(ts);
    if (ts->GetTimeStamp() <= lastKnownTimestamp)
    {
      // No new messages since requested timestamp
      if (m_receivedUWPMessages.find(lastKnownTimestamp) != m_receivedUWPMessages.end())
      {
        return m_receivedUWPMessages.at(lastKnownTimestamp);
      }
      return nullptr;
    }

    auto frame = ref new TrackedFrame();
    m_receivedUWPMessages[ts->GetTimeStamp()] = frame;

    // Tracking/other related fields
    for (auto& pair : trackedFrameMsg->GetMetaData())
    {
      std::wstring keyWideStr(pair.first.begin(), pair.first.end());
      std::wstring valueWideStr(pair.second.begin(), pair.second.end());
      frame->SetCustomFrameField(keyWideStr, valueWideStr);
    }

    for (auto& pair : trackedFrameMsg->GetCustomFrameFields())
    {
      frame->SetCustomFrameField(pair.first, pair.second);
    }

    // Image related fields
    frame->SetFrameSize(trackedFrameMsg->GetFrameSize());
    frame->Timestamp = ts->GetTimeStamp();
    frame->ImageSizeBytes = trackedFrameMsg->GetImageSizeInBytes();
    frame->SetImageData(trackedFrameMsg->GetImage());
    frame->NumberOfComponents = trackedFrameMsg->GetNumberOfComponents();
    frame->ScalarType = trackedFrameMsg->GetScalarType();
    frame->SetEmbeddedImageTransform(trackedFrameMsg->GetEmbeddedImageTransform());
    frame->ImageType = (uint16)trackedFrameMsg->GetImageType();
    frame->ImageOrientation = (uint16)trackedFrameMsg->GetImageOrientation();
    frame->SetFrameTransformsInternal(trackedFrameMsg->GetFrameTransforms());

    return frame;
  }

  //----------------------------------------------------------------------------
  Command^ IGTLinkClient::GetCommand(double lastKnownTimestamp)
  {
    igtl::MessageBase::Pointer igtMessage = nullptr;
    {
      // Retrieve the next available tracked frame reply
      std::lock_guard<std::mutex> guard(m_messageListMutex);
      for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
      {
        if (typeid(*(*replyIter)) == typeid(igtl::RTSCommandMessage))
        {
          igtMessage = *replyIter;
          break;
        }
      }

      if (igtMessage == nullptr)
      {
        return nullptr;
      }
    }

    auto ts = igtl::TimeStamp::New();
    igtMessage->GetTimeStamp(ts);
    if (ts->GetTimeStamp() <= lastKnownTimestamp)
    {
      return nullptr;
    }

    auto cmd = ref new Command();
    auto cmdMsg = dynamic_cast<igtl::RTSCommandMessage*>(igtMessage.GetPointer());

    cmd->CommandContent = ref new Platform::String(std::wstring(cmdMsg->GetCommandContent().begin(), cmdMsg->GetCommandContent().end()).c_str());
    cmd->CommandName = ref new Platform::String(std::wstring(cmdMsg->GetCommandName().begin(), cmdMsg->GetCommandName().end()).c_str());
    cmd->OriginalCommandId = cmdMsg->GetCommandId();
    cmd->Timestamp = lastKnownTimestamp;

    XmlDocument^ doc = ref new XmlDocument();
    doc->LoadXml(cmd->CommandContent);

    for (IXmlNode^ node : doc->ChildNodes)
    {
      if (dynamic_cast<Platform::String^>(node->NodeName) == L"Result")
      {
        cmd->Result = (dynamic_cast<Platform::String^>(node->NodeValue) == L"true");
        break;
      }
    }

    if (!cmd->Result)
    {
      bool found(false);
      // Parse for the error string
      for (IXmlNode^ node : doc->ChildNodes)
      {
        if (dynamic_cast<Platform::String^>(node->NodeName) == L"Error")
        {
          cmd->ErrorString = dynamic_cast<Platform::String^>(node->NodeValue);
          found = true;
          break;
        }
      }

      if (!found)
      {
        OutputDebugStringA("Error string not found even though error was reported. Check IGT server implementation.");
      }
    }

    for (auto& pair : cmdMsg->GetMetaData())
    {
      std::wstring keyWideStr(pair.first.begin(), pair.first.end());
      std::wstring valueWideStr(pair.second.begin(), pair.second.end());
      cmd->Parameters->Insert(ref new Platform::String(keyWideStr.c_str()), ref new Platform::String(valueWideStr.c_str()));
    }

    return cmd;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::SendMessage(igtl::MessageBase::Pointer packedMessage)
  {
    int success = 0;
    {
      std::lock_guard<std::mutex> guard(m_socketMutex);
      success = m_clientSocket->Send(packedMessage->GetBufferPointer(), packedMessage->GetBufferSize());
    }
    if (!success)
    {
      std::cerr << "OpenIGTLink client couldn't send message to server." << std::endl;
      return false;
    }
    return true;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::SendMessage(MessageBasePointerPtr messageBasePointerPtr)
  {
    igtl::MessageBase::Pointer* messageBasePointer = (igtl::MessageBase::Pointer*)(messageBasePointerPtr);
    return SendMessage(*messageBasePointer);
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::DataReceiverPump(IGTLinkClient^ self, concurrency::cancellation_token token)
  {
    LOG_TRACE(L"IGTLinkClient::DataReceiverPump");

    while (!token.is_canceled())
    {
      auto headerMsg = self->m_igtlMessageFactory->CreateHeaderMessage(IGTL_HEADER_VERSION_1);

      // Receive generic header from the socket
      int numOfBytesReceived = 0;
      {
        std::lock_guard<std::mutex> guard(self->m_socketMutex);
        if (!self->m_clientSocket->GetConnected())
        {
          // We've become disconnected while waiting for the socket, we're done here!
          return;
        }
        numOfBytesReceived = self->m_clientSocket->Receive(headerMsg->GetBufferPointer(), headerMsg->GetBufferSize());
      }
      if (numOfBytesReceived == 0  // No message received
          || numOfBytesReceived != headerMsg->GetBufferSize() // Received data is not as we expected
         )
      {
        // Failed to receive data, maybe the socket is disconnected
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        continue;
      }

      int c = headerMsg->Unpack(1);
      if (!(c & igtl::MessageHeader::UNPACK_HEADER))
      {
        std::cerr << "Failed to receive message (invalid header)" << std::endl;
        continue;
      }

      igtl::MessageBase::Pointer bodyMsg = nullptr;
      try
      {
        bodyMsg = self->m_igtlMessageFactory->CreateReceiveMessage(headerMsg);
      }
      catch (const std::exception&)
      {
        // Message header was not correct, skip this message
        // Will be impossible to tell if the body of this message is in the socket... this is a pretty bad corruption.
        // Force re-connect?
        LOG_TRACE("Corruption in the message header. Serious error.");
        continue;
      }

      if (bodyMsg.IsNull())
      {
        LOG_TRACE("Unable to create message of type: " << headerMsg->GetMessageType());
        continue;
      }

      // Accept all messages but status messages, they are used as a keep alive mechanism
      if (typeid(*bodyMsg) != typeid(igtl::StatusMessage))
      {
        {
          std::lock_guard<std::mutex> guard(self->m_socketMutex);
          if (!self->m_clientSocket->GetConnected())
          {
            // We've become disconnected while waiting for the socket, we're done here!
            return;
          }
          self->m_clientSocket->Receive(bodyMsg->GetBufferBodyPointer(), bodyMsg->GetBufferBodySize());
        }

        int c = bodyMsg->Unpack(1);
        if (!(c & igtl::MessageHeader::UNPACK_BODY))
        {
          LOG_TRACE("Failed to receive reply (invalid body)");
          continue;
        }

        {
          // save reply
          std::lock_guard<std::mutex> guard(self->m_messageListMutex);
          self->m_receivedMessages.push_back(bodyMsg);
        }
      }
      else
      {
        std::lock_guard<std::mutex> guard(self->m_socketMutex);
        self->m_clientSocket->Skip(headerMsg->GetBodySizeToRead(), 0);
      }

      if (self->m_receivedMessages.size() > MESSAGE_LIST_MAX_SIZE)
      {
        std::lock_guard<std::mutex> guard(self->m_messageListMutex);

        // erase the front N results
        uint32 toErase = self->m_receivedMessages.size() - MESSAGE_LIST_MAX_SIZE;
        self->m_receivedMessages.erase(self->m_receivedMessages.begin(), self->m_receivedMessages.begin() + toErase);
      }

      auto oldestTimestamp = self->GetOldestTrackedFrameTimestamp();
      if (oldestTimestamp > 0)
      {
        for (auto it = self->m_receivedUWPMessages.begin(); it != self->m_receivedUWPMessages.end();)
        {
          if (it->first < oldestTimestamp)
          {
            it = self->m_receivedUWPMessages.erase(it);
          }
          else
          {
            ++it;
          }
        }
      }
    }

    return;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::SocketReceive(void* data, int length)
  {
    std::lock_guard<std::mutex> guard(m_socketMutex);
    return m_clientSocket->Receive(data, length);
  }

  //----------------------------------------------------------------------------
  double IGTLinkClient::GetLatestTrackedFrameTimestamp()
  {
    // Retrieve the next available tracked frame reply
    std::lock_guard<std::mutex> guard(m_messageListMutex);
    for (auto replyIter = m_receivedMessages.rbegin(); replyIter != m_receivedMessages.rend(); ++replyIter)
    {
      if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
      {
        igtl::TimeStamp::Pointer ts = igtl::TimeStamp::New();
        (*replyIter)->GetTimeStamp(ts);
        return ts->GetTimeStamp();
      }
    }
    return -1.0;
  }

  //----------------------------------------------------------------------------
  double IGTLinkClient::GetOldestTrackedFrameTimestamp()
  {
    // Retrieve the next available tracked frame reply
    std::lock_guard<std::mutex> guard(m_messageListMutex);
    for (auto replyIter = m_receivedMessages.begin(); replyIter != m_receivedMessages.end(); ++replyIter)
    {
      if (typeid(*(*replyIter)) == typeid(igtl::TrackedFrameMessage))
      {
        igtl::TimeStamp::Pointer ts = igtl::TimeStamp::New();
        (*replyIter)->GetTimeStamp(ts);
        return ts->GetTimeStamp();
      }
    }
    return -1.0;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::ServerPort::get()
  {
    return m_serverPort;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerPort::set(int arg)
  {
    m_serverPort = arg;
  }

  //----------------------------------------------------------------------------
  Platform::String^ IGTLinkClient::ServerHost::get()
  {
    return m_serverHost;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerHost::set(Platform::String^ arg)
  {
    m_serverHost = arg;
  }

  //----------------------------------------------------------------------------
  int IGTLinkClient::ServerIGTLVersion::get()
  {
    return m_serverIGTLVersion;
  }

  //----------------------------------------------------------------------------
  void IGTLinkClient::ServerIGTLVersion::set(int arg)
  {
    m_serverIGTLVersion = arg;
  }

  //----------------------------------------------------------------------------
  bool IGTLinkClient::Connected::get()
  {
    return m_clientSocket->GetConnected();
  }
}<|endoftext|>


<commit_before>//
//  DiskController.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/07/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "DiskController.hpp"
#include "UnformattedTrack.hpp"
#include "../../NumberTheory/Factors.hpp"
#include <cassert>

using namespace Storage::Disk;

Controller::Controller(Cycles clock_rate, int clock_rate_multiplier, int revolutions_per_minute) :
		clock_rate_(clock_rate.as_int() * clock_rate_multiplier),
		clock_rate_multiplier_(clock_rate_multiplier),
		rotational_multiplier_(60, revolutions_per_minute),

		cycles_since_index_hole_(0),
		motor_is_on_(false),

		is_reading_(true),

		TimedEventLoop((unsigned int)(clock_rate.as_int() * clock_rate_multiplier)) {
	// seed this class with a PLL, any PLL, so that it's safe to assume non-nullptr later
	Time one(1);
	set_expected_bit_length(one);
}

void Controller::setup_track() {
	track_ = drive_->get_track();
	if(!track_) {
		track_.reset(new UnformattedTrack);
	}

	Time offset;
	Time track_time_now = get_time_into_track();
	assert(track_time_now >= Time(0) && current_event_.length <= Time(1));

	Time time_found = track_->seek_to(track_time_now);
	assert(time_found >= Time(0) && time_found <= track_time_now);
	offset = track_time_now - time_found;

	get_next_event(offset);
}

void Controller::run_for(const Cycles cycles) {
	Time zero(0);

	if(drive_ && drive_->has_disk() && motor_is_on_) {
		if(!track_) setup_track();

		int number_of_cycles = clock_rate_multiplier_ * cycles.as_int();
		while(number_of_cycles) {
			int cycles_until_next_event = (int)get_cycles_until_next_event();
			int cycles_to_run_for = std::min(cycles_until_next_event, number_of_cycles);
			if(!is_reading_ && cycles_until_bits_written_ > zero) {
				int write_cycles_target = (int)cycles_until_bits_written_.get_unsigned_int();
				if(cycles_until_bits_written_.length % cycles_until_bits_written_.clock_rate) write_cycles_target++;
				cycles_to_run_for = std::min(cycles_to_run_for, write_cycles_target);
			}

			cycles_since_index_hole_ += (unsigned int)cycles_to_run_for;

			number_of_cycles -= cycles_to_run_for;
			if(is_reading_) {
				pll_->run_for(Cycles(cycles_to_run_for));
			} else {
				if(cycles_until_bits_written_ > zero) {
					Storage::Time cycles_to_run_for_time(cycles_to_run_for);
					if(cycles_until_bits_written_ <= cycles_to_run_for_time) {
						process_write_completed();
						if(cycles_until_bits_written_ <= cycles_to_run_for_time)
							cycles_until_bits_written_.set_zero();
						else
							cycles_until_bits_written_ -= cycles_to_run_for_time;
					} else {
						cycles_until_bits_written_ -= cycles_to_run_for_time;
					}
				}
			}
			TimedEventLoop::run_for(Cycles(cycles_to_run_for));
		}
	}
}

#pragma mark - Track timed event loop

void Controller::get_next_event(const Time &duration_already_passed) {
	if(track_) {
		current_event_ = track_->get_next_event();
	} else {
		current_event_.length.length = 1;
		current_event_.length.clock_rate = 1;
		current_event_.type = Track::Event::IndexHole;
	}

	// divide interval, which is in terms of a single rotation of the disk, by rotation speed to
	// convert it into revolutions per second; this is achieved by multiplying by rotational_multiplier_
	assert(current_event_.length <= Time(1) && current_event_.length >= Time(0));
	Time interval = (current_event_.length - duration_already_passed) * rotational_multiplier_;
	set_next_event_time_interval(interval);
}

void Controller::process_next_event()
{
	switch(current_event_.type) {
		case Track::Event::FluxTransition:
			if(is_reading_) pll_->add_pulse();
		break;
		case Track::Event::IndexHole:
//			printf("%p %d [/%d = %d]\n", this, cycles_since_index_hole_, clock_rate_multiplier_, cycles_since_index_hole_ / clock_rate_multiplier_);
			cycles_since_index_hole_ = 0;
			process_index_hole();
		break;
	}
	get_next_event(Time(0));
}

Storage::Time Controller::get_time_into_track() {
	// this is proportion of a second
	Time result(cycles_since_index_hole_, 8000000 * clock_rate_multiplier_);
	result /= rotational_multiplier_;
	result.simplify();
	return result;
}

#pragma mark - Writing

void Controller::begin_writing(bool clamp_to_index_hole) {
	is_reading_ = false;
	clamp_writing_to_index_hole_ = clamp_to_index_hole;

	write_segment_.length_of_a_bit = bit_length_ / rotational_multiplier_;
	write_segment_.data.clear();
	write_segment_.number_of_bits = 0;

	write_start_time_ = get_time_into_track();
}

void Controller::write_bit(bool value) {
	bool needs_new_byte = !(write_segment_.number_of_bits&7);
	if(needs_new_byte) write_segment_.data.push_back(0);
	if(value) write_segment_.data[write_segment_.number_of_bits >> 3] |= 0x80 >> (write_segment_.number_of_bits & 7);
	write_segment_.number_of_bits++;

	cycles_until_bits_written_ += cycles_per_bit_;
}

void Controller::end_writing() {
	is_reading_ = true;

	if(!patched_track_) {
		// Avoid creating a new patched track if this one is already patched
		patched_track_ = std::dynamic_pointer_cast<PCMPatchedTrack>(track_);
		if(!patched_track_) {
			patched_track_.reset(new PCMPatchedTrack(track_));
		} else {
			printf("");
		}
	} else {
		printf("");
	}
	patched_track_->add_segment(write_start_time_, write_segment_, clamp_writing_to_index_hole_);
	cycles_since_index_hole_ %= 8000000 * clock_rate_multiplier_;
	invalidate_track();	// TEMPORARY: to force a seek
}

#pragma mark - PLL control and delegate

void Controller::set_expected_bit_length(Time bit_length) {
	bit_length_ = bit_length;
	bit_length_.simplify();

	cycles_per_bit_ = Storage::Time(clock_rate_) * bit_length;
	cycles_per_bit_.simplify();

	// this conversion doesn't need to be exact because there's a lot of variation to be taken
	// account of in rotation speed, air turbulence, etc, so a direct conversion will do
	int clocks_per_bit = (int)cycles_per_bit_.get_unsigned_int();
	pll_.reset(new DigitalPhaseLockedLoop(clocks_per_bit, 3));
	pll_->set_delegate(this);
}

void Controller::digital_phase_locked_loop_output_bit(int value) {
	process_input_bit(value, (unsigned int)cycles_since_index_hole_);
}

#pragma mark - Drive actions

bool Controller::get_is_track_zero() {
	if(!drive_) return false;
	return drive_->get_is_track_zero();
}

bool Controller::get_drive_is_ready() {
	if(!drive_) return false;
	return drive_->has_disk();
}

bool Controller::get_drive_is_read_only() {
	if(!drive_) return false;
	return drive_->get_is_read_only();
}

void Controller::step(int direction) {
	invalidate_track();
	if(drive_) drive_->step(direction);
}

void Controller::set_motor_on(bool motor_on) {
	motor_is_on_ = motor_on;
}

bool Controller::get_motor_on() {
	return motor_is_on_;
}

void Controller::set_drive(std::shared_ptr<Drive> drive) {
	if(drive_ != drive) {
		invalidate_track();
		drive_ = drive;
	}
}

void Controller::invalidate_track() {
	track_ = nullptr;
	if(patched_track_) {
		drive_->set_track(patched_track_);
		patched_track_ = nullptr;
	}
}

void Controller::process_write_completed() {}
<commit_msg>Removed debugging nonsense.<commit_after>//
//  DiskController.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/07/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "DiskController.hpp"
#include "UnformattedTrack.hpp"
#include "../../NumberTheory/Factors.hpp"
#include <cassert>

using namespace Storage::Disk;

Controller::Controller(Cycles clock_rate, int clock_rate_multiplier, int revolutions_per_minute) :
		clock_rate_(clock_rate.as_int() * clock_rate_multiplier),
		clock_rate_multiplier_(clock_rate_multiplier),
		rotational_multiplier_(60, revolutions_per_minute),

		cycles_since_index_hole_(0),
		motor_is_on_(false),

		is_reading_(true),

		TimedEventLoop((unsigned int)(clock_rate.as_int() * clock_rate_multiplier)) {
	// seed this class with a PLL, any PLL, so that it's safe to assume non-nullptr later
	Time one(1);
	set_expected_bit_length(one);
}

void Controller::setup_track() {
	track_ = drive_->get_track();
	if(!track_) {
		track_.reset(new UnformattedTrack);
	}

	Time offset;
	Time track_time_now = get_time_into_track();
	assert(track_time_now >= Time(0) && current_event_.length <= Time(1));

	Time time_found = track_->seek_to(track_time_now);
	assert(time_found >= Time(0) && time_found <= track_time_now);
	offset = track_time_now - time_found;

	get_next_event(offset);
}

void Controller::run_for(const Cycles cycles) {
	Time zero(0);

	if(drive_ && drive_->has_disk() && motor_is_on_) {
		if(!track_) setup_track();

		int number_of_cycles = clock_rate_multiplier_ * cycles.as_int();
		while(number_of_cycles) {
			int cycles_until_next_event = (int)get_cycles_until_next_event();
			int cycles_to_run_for = std::min(cycles_until_next_event, number_of_cycles);
			if(!is_reading_ && cycles_until_bits_written_ > zero) {
				int write_cycles_target = (int)cycles_until_bits_written_.get_unsigned_int();
				if(cycles_until_bits_written_.length % cycles_until_bits_written_.clock_rate) write_cycles_target++;
				cycles_to_run_for = std::min(cycles_to_run_for, write_cycles_target);
			}

			cycles_since_index_hole_ += (unsigned int)cycles_to_run_for;

			number_of_cycles -= cycles_to_run_for;
			if(is_reading_) {
				pll_->run_for(Cycles(cycles_to_run_for));
			} else {
				if(cycles_until_bits_written_ > zero) {
					Storage::Time cycles_to_run_for_time(cycles_to_run_for);
					if(cycles_until_bits_written_ <= cycles_to_run_for_time) {
						process_write_completed();
						if(cycles_until_bits_written_ <= cycles_to_run_for_time)
							cycles_until_bits_written_.set_zero();
						else
							cycles_until_bits_written_ -= cycles_to_run_for_time;
					} else {
						cycles_until_bits_written_ -= cycles_to_run_for_time;
					}
				}
			}
			TimedEventLoop::run_for(Cycles(cycles_to_run_for));
		}
	}
}

#pragma mark - Track timed event loop

void Controller::get_next_event(const Time &duration_already_passed) {
	if(track_) {
		current_event_ = track_->get_next_event();
	} else {
		current_event_.length.length = 1;
		current_event_.length.clock_rate = 1;
		current_event_.type = Track::Event::IndexHole;
	}

	// divide interval, which is in terms of a single rotation of the disk, by rotation speed to
	// convert it into revolutions per second; this is achieved by multiplying by rotational_multiplier_
	assert(current_event_.length <= Time(1) && current_event_.length >= Time(0));
	Time interval = (current_event_.length - duration_already_passed) * rotational_multiplier_;
	set_next_event_time_interval(interval);
}

void Controller::process_next_event()
{
	switch(current_event_.type) {
		case Track::Event::FluxTransition:
			if(is_reading_) pll_->add_pulse();
		break;
		case Track::Event::IndexHole:
//			printf("%p %d [/%d = %d]\n", this, cycles_since_index_hole_, clock_rate_multiplier_, cycles_since_index_hole_ / clock_rate_multiplier_);
			cycles_since_index_hole_ = 0;
			process_index_hole();
		break;
	}
	get_next_event(Time(0));
}

Storage::Time Controller::get_time_into_track() {
	// this is proportion of a second
	Time result(cycles_since_index_hole_, 8000000 * clock_rate_multiplier_);
	result /= rotational_multiplier_;
	result.simplify();
	return result;
}

#pragma mark - Writing

void Controller::begin_writing(bool clamp_to_index_hole) {
	is_reading_ = false;
	clamp_writing_to_index_hole_ = clamp_to_index_hole;

	write_segment_.length_of_a_bit = bit_length_ / rotational_multiplier_;
	write_segment_.data.clear();
	write_segment_.number_of_bits = 0;

	write_start_time_ = get_time_into_track();
}

void Controller::write_bit(bool value) {
	bool needs_new_byte = !(write_segment_.number_of_bits&7);
	if(needs_new_byte) write_segment_.data.push_back(0);
	if(value) write_segment_.data[write_segment_.number_of_bits >> 3] |= 0x80 >> (write_segment_.number_of_bits & 7);
	write_segment_.number_of_bits++;

	cycles_until_bits_written_ += cycles_per_bit_;
}

void Controller::end_writing() {
	is_reading_ = true;

	if(!patched_track_) {
		// Avoid creating a new patched track if this one is already patched
		patched_track_ = std::dynamic_pointer_cast<PCMPatchedTrack>(track_);
		if(!patched_track_) {
			patched_track_.reset(new PCMPatchedTrack(track_));
		}
	}
	patched_track_->add_segment(write_start_time_, write_segment_, clamp_writing_to_index_hole_);
	cycles_since_index_hole_ %= 8000000 * clock_rate_multiplier_;
	invalidate_track();	// TEMPORARY: to force a seek
}

#pragma mark - PLL control and delegate

void Controller::set_expected_bit_length(Time bit_length) {
	bit_length_ = bit_length;
	bit_length_.simplify();

	cycles_per_bit_ = Storage::Time(clock_rate_) * bit_length;
	cycles_per_bit_.simplify();

	// this conversion doesn't need to be exact because there's a lot of variation to be taken
	// account of in rotation speed, air turbulence, etc, so a direct conversion will do
	int clocks_per_bit = (int)cycles_per_bit_.get_unsigned_int();
	pll_.reset(new DigitalPhaseLockedLoop(clocks_per_bit, 3));
	pll_->set_delegate(this);
}

void Controller::digital_phase_locked_loop_output_bit(int value) {
	process_input_bit(value, (unsigned int)cycles_since_index_hole_);
}

#pragma mark - Drive actions

bool Controller::get_is_track_zero() {
	if(!drive_) return false;
	return drive_->get_is_track_zero();
}

bool Controller::get_drive_is_ready() {
	if(!drive_) return false;
	return drive_->has_disk();
}

bool Controller::get_drive_is_read_only() {
	if(!drive_) return false;
	return drive_->get_is_read_only();
}

void Controller::step(int direction) {
	invalidate_track();
	if(drive_) drive_->step(direction);
}

void Controller::set_motor_on(bool motor_on) {
	motor_is_on_ = motor_on;
}

bool Controller::get_motor_on() {
	return motor_is_on_;
}

void Controller::set_drive(std::shared_ptr<Drive> drive) {
	if(drive_ != drive) {
		invalidate_track();
		drive_ = drive;
	}
}

void Controller::invalidate_track() {
	track_ = nullptr;
	if(patched_track_) {
		drive_->set_track(patched_track_);
		patched_track_ = nullptr;
	}
}

void Controller::process_write_completed() {}
<|endoftext|>


<commit_before>//
//  DiskController.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/07/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "DiskController.hpp"
#include "../../NumberTheory/Factors.hpp"

using namespace Storage::Disk;

Controller::Controller(Cycles clock_rate, int clock_rate_multiplier, int revolutions_per_minute) :
		clock_rate_(clock_rate.as_int() * clock_rate_multiplier),
		clock_rate_multiplier_(clock_rate_multiplier),
		rotational_multiplier_(60, revolutions_per_minute),

		cycles_since_index_hole_(0),
		motor_is_on_(false),

		is_reading_(true),

		TimedEventLoop((unsigned int)(clock_rate.as_int() * clock_rate_multiplier)) {
	// seed this class with a PLL, any PLL, so that it's safe to assume non-nullptr later
	Time one(1);
	set_expected_bit_length(one);
}

void Controller::setup_track() {
	track_ = drive_->get_track();

	Time offset;
	Time track_time_now = get_time_into_track();
	if(track_) {
		Time time_found = track_->seek_to(track_time_now);
		offset = track_time_now - time_found;
	}

	get_next_event(offset);
}

void Controller::run_for(const Cycles cycles) {
	Time zero(0);

	if(drive_ && drive_->has_disk() && motor_is_on_) {
		if(!track_) setup_track();

		int number_of_cycles = clock_rate_multiplier_ * cycles.as_int();
		while(number_of_cycles) {
			int cycles_until_next_event = (int)get_cycles_until_next_event();
			int cycles_to_run_for = std::min(cycles_until_next_event, number_of_cycles);
			if(!is_reading_ && cycles_until_bits_written_ > zero) {
				int write_cycles_target = (int)cycles_until_bits_written_.get_unsigned_int();
				if(cycles_until_bits_written_.length % cycles_until_bits_written_.clock_rate) write_cycles_target++;
				cycles_to_run_for = std::min(cycles_to_run_for, write_cycles_target);
			}

			cycles_since_index_hole_ += (unsigned int)cycles_to_run_for;

			number_of_cycles -= cycles_to_run_for;
			if(is_reading_) {
				pll_->run_for(Cycles(cycles_to_run_for));
			} else {
				if(cycles_until_bits_written_ > zero) {
					Storage::Time cycles_to_run_for_time(cycles_to_run_for);
					if(cycles_until_bits_written_ <= cycles_to_run_for_time) {
						process_write_completed();
						if(cycles_until_bits_written_ <= cycles_to_run_for_time)
							cycles_until_bits_written_.set_zero();
						else
							cycles_until_bits_written_ -= cycles_to_run_for_time;
					} else {
						cycles_until_bits_written_ -= cycles_to_run_for_time;
					}
				}
			}
			TimedEventLoop::run_for(Cycles(cycles_to_run_for));
		}
	}
}

#pragma mark - Track timed event loop

void Controller::get_next_event(const Time &duration_already_passed) {
	if(track_) {
		current_event_ = track_->get_next_event();
	} else {
		current_event_.length.length = 1;
		current_event_.length.clock_rate = 1;
		current_event_.type = Track::Event::IndexHole;
	}

	// divide interval, which is in terms of a single rotation of the disk, by rotation speed to
	// convert it into revolutions per second; this is achieved by multiplying by rotational_multiplier_
	set_next_event_time_interval((current_event_.length - duration_already_passed) * rotational_multiplier_);
}

void Controller::process_next_event()
{
	switch(current_event_.type) {
		case Track::Event::FluxTransition:
			if(is_reading_) pll_->add_pulse();
		break;
		case Track::Event::IndexHole:
			printf("%p %d [/%d = %d]\n", this, cycles_since_index_hole_, clock_rate_multiplier_, cycles_since_index_hole_ / clock_rate_multiplier_);
			cycles_since_index_hole_ = 0;
			process_index_hole();
		break;
	}
	get_next_event(Time(0));
}

Storage::Time Controller::get_time_into_track() {
	// this is proportion of a second
	Time result(cycles_since_index_hole_, 8000000 * clock_rate_multiplier_);
	result /= rotational_multiplier_;
	result.simplify();
	return result;
}

#pragma mark - Writing

void Controller::begin_writing() {
	is_reading_ = false;

	write_segment_.length_of_a_bit = bit_length_ / rotational_multiplier_;
	write_segment_.data.clear();
	write_segment_.number_of_bits = 0;

	write_start_time_ = get_time_into_track();
}

void Controller::write_bit(bool value) {
	bool needs_new_byte = !(write_segment_.number_of_bits&7);
	if(needs_new_byte) write_segment_.data.push_back(0);
	if(value) write_segment_.data[write_segment_.number_of_bits >> 3] |= 0x80 >> (write_segment_.number_of_bits & 7);
	write_segment_.number_of_bits++;

	cycles_until_bits_written_ += cycles_per_bit_;
}

void Controller::end_writing() {
	is_reading_ = true;

	if(!patched_track_) {
		// Avoid creating a new patched track if this one is already patched
		patched_track_ = std::dynamic_pointer_cast<PCMPatchedTrack>(track_);
		if(!patched_track_) {
			patched_track_.reset(new PCMPatchedTrack(track_));
		}
	}
	patched_track_->add_segment(write_start_time_, write_segment_);
	invalidate_track();	// TEMPORARY: to force a seek
}

#pragma mark - PLL control and delegate

void Controller::set_expected_bit_length(Time bit_length) {
	bit_length_ = bit_length;
	bit_length_.simplify();

	cycles_per_bit_ = Storage::Time(clock_rate_) * bit_length;
	cycles_per_bit_.simplify();

	// this conversion doesn't need to be exact because there's a lot of variation to be taken
	// account of in rotation speed, air turbulence, etc, so a direct conversion will do
	int clocks_per_bit = (int)cycles_per_bit_.get_unsigned_int();
	pll_.reset(new DigitalPhaseLockedLoop(clocks_per_bit, 3));
	pll_->set_delegate(this);
}

void Controller::digital_phase_locked_loop_output_bit(int value) {
	process_input_bit(value, (unsigned int)cycles_since_index_hole_);
}

#pragma mark - Drive actions

bool Controller::get_is_track_zero() {
	if(!drive_) return false;
	return drive_->get_is_track_zero();
}

bool Controller::get_drive_is_ready() {
	if(!drive_) return false;
	return drive_->has_disk();
}

bool Controller::get_drive_is_read_only() {
	if(!drive_) return false;
	return drive_->get_is_read_only();
}

void Controller::step(int direction) {
	invalidate_track();
	if(drive_) drive_->step(direction);
}

void Controller::set_motor_on(bool motor_on) {
	motor_is_on_ = motor_on;
}

bool Controller::get_motor_on() {
	return motor_is_on_;
}

void Controller::set_drive(std::shared_ptr<Drive> drive) {
	if(drive_ != drive)
	{
		invalidate_track();
		drive_ = drive;
	}
}

void Controller::invalidate_track() {
	track_ = nullptr;
	if(patched_track_) {
		drive_->set_track(patched_track_);
		patched_track_ = nullptr;
	}
}

void Controller::process_write_completed() {}
<commit_msg>Removed index-hole announcement.<commit_after>//
//  DiskController.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 14/07/2016.
//  Copyright © 2016 Thomas Harte. All rights reserved.
//

#include "DiskController.hpp"
#include "../../NumberTheory/Factors.hpp"

using namespace Storage::Disk;

Controller::Controller(Cycles clock_rate, int clock_rate_multiplier, int revolutions_per_minute) :
		clock_rate_(clock_rate.as_int() * clock_rate_multiplier),
		clock_rate_multiplier_(clock_rate_multiplier),
		rotational_multiplier_(60, revolutions_per_minute),

		cycles_since_index_hole_(0),
		motor_is_on_(false),

		is_reading_(true),

		TimedEventLoop((unsigned int)(clock_rate.as_int() * clock_rate_multiplier)) {
	// seed this class with a PLL, any PLL, so that it's safe to assume non-nullptr later
	Time one(1);
	set_expected_bit_length(one);
}

void Controller::setup_track() {
	track_ = drive_->get_track();

	Time offset;
	Time track_time_now = get_time_into_track();
	if(track_) {
		Time time_found = track_->seek_to(track_time_now);
		offset = track_time_now - time_found;
	}

	get_next_event(offset);
}

void Controller::run_for(const Cycles cycles) {
	Time zero(0);

	if(drive_ && drive_->has_disk() && motor_is_on_) {
		if(!track_) setup_track();

		int number_of_cycles = clock_rate_multiplier_ * cycles.as_int();
		while(number_of_cycles) {
			int cycles_until_next_event = (int)get_cycles_until_next_event();
			int cycles_to_run_for = std::min(cycles_until_next_event, number_of_cycles);
			if(!is_reading_ && cycles_until_bits_written_ > zero) {
				int write_cycles_target = (int)cycles_until_bits_written_.get_unsigned_int();
				if(cycles_until_bits_written_.length % cycles_until_bits_written_.clock_rate) write_cycles_target++;
				cycles_to_run_for = std::min(cycles_to_run_for, write_cycles_target);
			}

			cycles_since_index_hole_ += (unsigned int)cycles_to_run_for;

			number_of_cycles -= cycles_to_run_for;
			if(is_reading_) {
				pll_->run_for(Cycles(cycles_to_run_for));
			} else {
				if(cycles_until_bits_written_ > zero) {
					Storage::Time cycles_to_run_for_time(cycles_to_run_for);
					if(cycles_until_bits_written_ <= cycles_to_run_for_time) {
						process_write_completed();
						if(cycles_until_bits_written_ <= cycles_to_run_for_time)
							cycles_until_bits_written_.set_zero();
						else
							cycles_until_bits_written_ -= cycles_to_run_for_time;
					} else {
						cycles_until_bits_written_ -= cycles_to_run_for_time;
					}
				}
			}
			TimedEventLoop::run_for(Cycles(cycles_to_run_for));
		}
	}
}

#pragma mark - Track timed event loop

void Controller::get_next_event(const Time &duration_already_passed) {
	if(track_) {
		current_event_ = track_->get_next_event();
	} else {
		current_event_.length.length = 1;
		current_event_.length.clock_rate = 1;
		current_event_.type = Track::Event::IndexHole;
	}

	// divide interval, which is in terms of a single rotation of the disk, by rotation speed to
	// convert it into revolutions per second; this is achieved by multiplying by rotational_multiplier_
	set_next_event_time_interval((current_event_.length - duration_already_passed) * rotational_multiplier_);
}

void Controller::process_next_event()
{
	switch(current_event_.type) {
		case Track::Event::FluxTransition:
			if(is_reading_) pll_->add_pulse();
		break;
		case Track::Event::IndexHole:
//			printf("%p %d [/%d = %d]\n", this, cycles_since_index_hole_, clock_rate_multiplier_, cycles_since_index_hole_ / clock_rate_multiplier_);
			cycles_since_index_hole_ = 0;
			process_index_hole();
		break;
	}
	get_next_event(Time(0));
}

Storage::Time Controller::get_time_into_track() {
	// this is proportion of a second
	Time result(cycles_since_index_hole_, 8000000 * clock_rate_multiplier_);
	result /= rotational_multiplier_;
	result.simplify();
	return result;
}

#pragma mark - Writing

void Controller::begin_writing() {
	is_reading_ = false;

	write_segment_.length_of_a_bit = bit_length_ / rotational_multiplier_;
	write_segment_.data.clear();
	write_segment_.number_of_bits = 0;

	write_start_time_ = get_time_into_track();
}

void Controller::write_bit(bool value) {
	bool needs_new_byte = !(write_segment_.number_of_bits&7);
	if(needs_new_byte) write_segment_.data.push_back(0);
	if(value) write_segment_.data[write_segment_.number_of_bits >> 3] |= 0x80 >> (write_segment_.number_of_bits & 7);
	write_segment_.number_of_bits++;

	cycles_until_bits_written_ += cycles_per_bit_;
}

void Controller::end_writing() {
	is_reading_ = true;

	if(!patched_track_) {
		// Avoid creating a new patched track if this one is already patched
		patched_track_ = std::dynamic_pointer_cast<PCMPatchedTrack>(track_);
		if(!patched_track_) {
			patched_track_.reset(new PCMPatchedTrack(track_));
		}
	}
	patched_track_->add_segment(write_start_time_, write_segment_);
	invalidate_track();	// TEMPORARY: to force a seek
}

#pragma mark - PLL control and delegate

void Controller::set_expected_bit_length(Time bit_length) {
	bit_length_ = bit_length;
	bit_length_.simplify();

	cycles_per_bit_ = Storage::Time(clock_rate_) * bit_length;
	cycles_per_bit_.simplify();

	// this conversion doesn't need to be exact because there's a lot of variation to be taken
	// account of in rotation speed, air turbulence, etc, so a direct conversion will do
	int clocks_per_bit = (int)cycles_per_bit_.get_unsigned_int();
	pll_.reset(new DigitalPhaseLockedLoop(clocks_per_bit, 3));
	pll_->set_delegate(this);
}

void Controller::digital_phase_locked_loop_output_bit(int value) {
	process_input_bit(value, (unsigned int)cycles_since_index_hole_);
}

#pragma mark - Drive actions

bool Controller::get_is_track_zero() {
	if(!drive_) return false;
	return drive_->get_is_track_zero();
}

bool Controller::get_drive_is_ready() {
	if(!drive_) return false;
	return drive_->has_disk();
}

bool Controller::get_drive_is_read_only() {
	if(!drive_) return false;
	return drive_->get_is_read_only();
}

void Controller::step(int direction) {
	invalidate_track();
	if(drive_) drive_->step(direction);
}

void Controller::set_motor_on(bool motor_on) {
	motor_is_on_ = motor_on;
}

bool Controller::get_motor_on() {
	return motor_is_on_;
}

void Controller::set_drive(std::shared_ptr<Drive> drive) {
	if(drive_ != drive)
	{
		invalidate_track();
		drive_ = drive;
	}
}

void Controller::invalidate_track() {
	track_ = nullptr;
	if(patched_track_) {
		drive_->set_track(patched_track_);
		patched_track_ = nullptr;
	}
}

void Controller::process_write_completed() {}
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
//                                                                            //
// This file is part of Swift2D.                                              //
//                                                                            //
// Copyright: (c) 2011-2014 Simon Schneegans & Felix Lauer                    //
//                                                                            //
////////////////////////////////////////////////////////////////////////////////

// includes  -------------------------------------------------------------------
#include <swift2d/network/NetworkObjectBase.hpp>

#include <swift2d/utils/Logger.hpp>

#include <raknet/RakPeerInterface.h>
#include <raknet/GetTime.h>

namespace swift {

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::WriteAllocationID(RakNet::Connection_RM3 *con, RakNet::BitStream* stream) const {
  stream->Write(get_type());
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3ConstructionState NetworkObjectBase::QueryConstruction(RakNet::Connection_RM3 *con, RakNet::ReplicaManager3 *replicaManager3) {
  return QueryConstruction_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::QueryRemoteConstruction(RakNet::Connection_RM3 *con) {
  return QueryRemoteConstruction_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::SerializeConstruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  vd_serializer_.AddRemoteSystemVariableHistory(con->GetRakNetGUID());

  std::cout << "SerializeConstruction Begin" << std::endl;
  for (auto& member: distributed_members_) {
    member.print(std::cout);
    std::cout << std::endl;
    member.serialize(stream);
  }
  std::cout << "SerializeConstruction End" << std::endl;
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::DeserializeConstruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  std::cout << "DeserializeConstruction Begin" << std::endl;
  for (auto& member: distributed_members_) {
    member.print(std::cout);
    std::cout << std::endl;
    member.deserialize(stream);
  }

  std::cout << "DeserializeConstruction End" << std::endl;

  return true;
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::SerializeDestruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  vd_serializer_.RemoveRemoteSystemVariableHistory(con->GetRakNetGUID());

  for (auto& member: distributed_members_) {
    member.serialize(stream);
  }
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::DeserializeDestruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  for (auto& member: distributed_members_) {
    member.deserialize(stream);
  }

  return true;
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3ActionOnPopConnection NetworkObjectBase::QueryActionOnPopConnection(RakNet::Connection_RM3 *con) const {
  return QueryActionOnPopConnection_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::DeallocReplica(RakNet::Connection_RM3 *con) {
  on_remote_delete();
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3QuerySerializationResult NetworkObjectBase::QuerySerialization(RakNet::Connection_RM3 *con) {
  return QuerySerialization_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3SerializationResult NetworkObjectBase::Serialize(RakNet::SerializeParameters *serializeParameters) {

  RakNet::VariableDeltaSerializer::SerializationContext ctx;

  serializeParameters->pro[0].reliability=RELIABLE_ORDERED;
  vd_serializer_.BeginIdenticalSerialize(
    &ctx,
    serializeParameters->whenLastSerialized==0,
    &serializeParameters->outputBitstream[0]
  );
  for (auto& member: distributed_members_) {
    member.serialize(&ctx, &vd_serializer_);
  }
  vd_serializer_.EndSerialize(&ctx);

  return RakNet::RM3SR_SERIALIZED_ALWAYS_IDENTICALLY;
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::Deserialize(RakNet::DeserializeParameters *deserializeParameters) {
  RakNet::VariableDeltaSerializer::DeserializationContext ctx;

  vd_serializer_.BeginDeserialize(&ctx, &deserializeParameters->serializationBitstream[0]);
  for (auto& member: distributed_members_) {
    member.deserialize(&ctx, &vd_serializer_);
  }
  vd_serializer_.EndDeserialize(&ctx);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::OnUserReplicaPreSerializeTick(void) {
  vd_serializer_.OnPreSerializeTick();
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::distribute_member(SerializableReference const& value) {
  distributed_members_.push_back(value);
}

////////////////////////////////////////////////////////////////////////////////

}
<commit_msg>removed output<commit_after>////////////////////////////////////////////////////////////////////////////////
//                                                                            //
// This file is part of Swift2D.                                              //
//                                                                            //
// Copyright: (c) 2011-2014 Simon Schneegans & Felix Lauer                    //
//                                                                            //
////////////////////////////////////////////////////////////////////////////////

// includes  -------------------------------------------------------------------
#include <swift2d/network/NetworkObjectBase.hpp>

#include <swift2d/utils/Logger.hpp>

#include <raknet/RakPeerInterface.h>
#include <raknet/GetTime.h>

namespace swift {

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::WriteAllocationID(RakNet::Connection_RM3 *con, RakNet::BitStream* stream) const {
  stream->Write(get_type());
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3ConstructionState NetworkObjectBase::QueryConstruction(RakNet::Connection_RM3 *con, RakNet::ReplicaManager3 *replicaManager3) {
  return QueryConstruction_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::QueryRemoteConstruction(RakNet::Connection_RM3 *con) {
  return QueryRemoteConstruction_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::SerializeConstruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  vd_serializer_.AddRemoteSystemVariableHistory(con->GetRakNetGUID());

  for (auto& member: distributed_members_) {
    member.serialize(stream);
  }
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::DeserializeConstruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  for (auto& member: distributed_members_) {
    member.deserialize(stream);
  }

  return true;
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::SerializeDestruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  vd_serializer_.RemoveRemoteSystemVariableHistory(con->GetRakNetGUID());

  for (auto& member: distributed_members_) {
    member.serialize(stream);
  }
}

////////////////////////////////////////////////////////////////////////////////

bool NetworkObjectBase::DeserializeDestruction(RakNet::BitStream* stream, RakNet::Connection_RM3 *con) {
  for (auto& member: distributed_members_) {
    member.deserialize(stream);
  }

  return true;
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3ActionOnPopConnection NetworkObjectBase::QueryActionOnPopConnection(RakNet::Connection_RM3 *con) const {
  return QueryActionOnPopConnection_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::DeallocReplica(RakNet::Connection_RM3 *con) {
  on_remote_delete();
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3QuerySerializationResult NetworkObjectBase::QuerySerialization(RakNet::Connection_RM3 *con) {
  return QuerySerialization_PeerToPeer(con);
}

////////////////////////////////////////////////////////////////////////////////

RakNet::RM3SerializationResult NetworkObjectBase::Serialize(RakNet::SerializeParameters *serializeParameters) {

  RakNet::VariableDeltaSerializer::SerializationContext ctx;

  serializeParameters->pro[0].reliability=RELIABLE_ORDERED;
  vd_serializer_.BeginIdenticalSerialize(
    &ctx,
    serializeParameters->whenLastSerialized==0,
    &serializeParameters->outputBitstream[0]
  );
  for (auto& member: distributed_members_) {
    member.serialize(&ctx, &vd_serializer_);
  }
  vd_serializer_.EndSerialize(&ctx);

  return RakNet::RM3SR_SERIALIZED_ALWAYS_IDENTICALLY;
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::Deserialize(RakNet::DeserializeParameters *deserializeParameters) {
  RakNet::VariableDeltaSerializer::DeserializationContext ctx;

  vd_serializer_.BeginDeserialize(&ctx, &deserializeParameters->serializationBitstream[0]);
  for (auto& member: distributed_members_) {
    member.deserialize(&ctx, &vd_serializer_);
  }
  vd_serializer_.EndDeserialize(&ctx);
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::OnUserReplicaPreSerializeTick(void) {
  vd_serializer_.OnPreSerializeTick();
}

////////////////////////////////////////////////////////////////////////////////

void NetworkObjectBase::distribute_member(SerializableReference const& value) {
  distributed_members_.push_back(value);
}

////////////////////////////////////////////////////////////////////////////////

}
<|endoftext|>


<commit_before>/*++
Copyright (c) 2012 Microsoft Corporation

Module Name:

    fpa2bv_model_converter.h

Abstract:

    Model conversion for fpa2bv_converter

Author:

    Christoph (cwinter) 2012-02-09

Notes:

--*/
#include"ast_smt2_pp.h"
#include"fpa2bv_model_converter.h"

void fpa2bv_model_converter::display(std::ostream & out) {
    out << "(fpa2bv-model-converter";
    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    for (obj_map<func_decl, func_decl*>::iterator it = m_uf2bvuf.begin();
         it != m_uf2bvuf.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    out << ")" << std::endl;
}

model_converter * fpa2bv_model_converter::translate(ast_translation & translator) {
    fpa2bv_model_converter * res = alloc(fpa2bv_model_converter, translator.to());
    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++)
    {
        func_decl * k = translator(it->m_key);
        expr * v = translator(it->m_value);
        res->m_const2bv.insert(k, v);
        translator.to().inc_ref(k);
        translator.to().inc_ref(v);
    }
    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++)
    {
        func_decl * k = translator(it->m_key);
        expr * v = translator(it->m_value);
        res->m_rm_const2bv.insert(k, v);
        translator.to().inc_ref(k);
        translator.to().inc_ref(v);
    }
    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2fp(sort * s, expr * sgn, expr * exp, expr * sig) const {
    fpa_util fu(m);
    bv_util bu(m);
    unsynch_mpz_manager & mpzm = fu.fm().mpz_manager();
    unsynch_mpq_manager & mpqm = fu.fm().mpq_manager();

    expr_ref res(m);
    mpf fp_val;

    unsigned ebits = fu.get_ebits(s);
    unsigned sbits = fu.get_sbits(s);

    unsigned sgn_sz = 1;
    unsigned exp_sz = ebits;
    unsigned sig_sz = sbits - 1;

    rational sgn_q(0), sig_q(0), exp_q(0);

    if (sgn) bu.is_numeral(sgn, sgn_q, sgn_sz);
    if (exp) bu.is_numeral(exp, exp_q, exp_sz);
    if (sig) bu.is_numeral(sig, sig_q, sig_sz);

    // un-bias exponent
    rational exp_unbiased_q;
    exp_unbiased_q = exp_q - fu.fm().m_powers2.m1(ebits - 1);

    mpz sig_z; mpf_exp_t exp_z;
    mpzm.set(sig_z, sig_q.to_mpq().numerator());
    exp_z = mpzm.get_int64(exp_unbiased_q.to_mpq().numerator());

    fu.fm().set(fp_val, ebits, sbits, !mpqm.is_zero(sgn_q.to_mpq()), sig_z, exp_z);

    mpzm.del(sig_z);

    res = fu.mk_value(fp_val);

    TRACE("fpa2bv_mc", tout << "[" << mk_ismt2_pp(sgn, m) <<
                               " " << mk_ismt2_pp(exp, m) <<
                               " " << mk_ismt2_pp(sig, m) << "] == " <<
                               mk_ismt2_pp(res, m) << std::endl;);

    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2fp(model * bv_mdl, sort * s, expr * bv) const {
    fpa_util fu(m);
    bv_util bu(m);

    SASSERT(bu.is_bv(bv));

    unsigned ebits = fu.get_ebits(s);
    unsigned sbits = fu.get_sbits(s);
    unsigned bv_sz = sbits + ebits;

    expr_ref sgn(m), exp(m), sig(m);
    sgn = bu.mk_extract(bv_sz - 1, bv_sz - 1, bv);
    exp = bu.mk_extract(bv_sz - 2, sbits - 1, bv);
    sig = bu.mk_extract(sbits - 2, 0, bv);

    expr_ref v_sgn(m), v_exp(m), v_sig(m);
    bv_mdl->eval(sgn, v_sgn);
    bv_mdl->eval(exp, v_exp);
    bv_mdl->eval(sig, v_sig);

    return convert_bv2fp(s, v_sgn, v_exp, v_sig);
}

expr_ref fpa2bv_model_converter::convert_bv2rm(expr * eval_v) const {
    fpa_util fu(m);
    bv_util bu(m);
    expr_ref res(m);
    rational bv_val(0);
    unsigned sz = 0;

    if (bu.is_numeral(eval_v, bv_val, sz)) {
        SASSERT(bv_val.is_uint64());
        switch (bv_val.get_uint64()) {
        case BV_RM_TIES_TO_AWAY: res = fu.mk_round_nearest_ties_to_away(); break;
        case BV_RM_TIES_TO_EVEN: res = fu.mk_round_nearest_ties_to_even(); break;
        case BV_RM_TO_NEGATIVE: res = fu.mk_round_toward_negative(); break;
        case BV_RM_TO_POSITIVE: res = fu.mk_round_toward_positive(); break;
        case BV_RM_TO_ZERO:
        default: res = fu.mk_round_toward_zero();
        }
    }

    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2rm(model * bv_mdl, func_decl * var, expr * val) const {
    fpa_util fu(m);
    bv_util bu(m);
    expr_ref res(m);

    expr_ref eval_v(m);

    if (val && bv_mdl->eval(val, eval_v, true))
        res = convert_bv2rm(eval_v);

    return res;
}

void fpa2bv_model_converter::convert(model * bv_mdl, model * float_mdl) {
    fpa_util fu(m);
    bv_util bu(m);
    unsynch_mpz_manager & mpzm = fu.fm().mpz_manager();
    unsynch_mpq_manager & mpqm = fu.fm().mpq_manager();

    TRACE("fpa2bv_mc", tout << "BV Model: " << std::endl;
    for (unsigned i = 0; i < bv_mdl->get_num_constants(); i++)
        tout << bv_mdl->get_constant(i)->get_name() << " --> " <<
        mk_ismt2_pp(bv_mdl->get_const_interp(bv_mdl->get_constant(i)), m) << std::endl;
    );

    obj_hashtable<func_decl> seen;

    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++)
    {
        func_decl * var = it->m_key;
        app * val = to_app(it->m_value);
        SASSERT(fu.is_float(var->get_range()));
        SASSERT(var->get_range()->get_num_parameters() == 2);

        expr_ref sgn(m), sig(m), exp(m);
        bv_mdl->eval(val->get_arg(0), sgn, true);
        bv_mdl->eval(val->get_arg(1), exp, true);
        bv_mdl->eval(val->get_arg(2), sig, true);

        SASSERT(val->is_app_of(fu.get_family_id(), OP_FPA_FP));

#ifdef Z3DEBUG
        SASSERT(to_app(val->get_arg(0))->get_decl()->get_arity() == 0);
        SASSERT(to_app(val->get_arg(1))->get_decl()->get_arity() == 0);
        SASSERT(to_app(val->get_arg(2))->get_decl()->get_arity() == 0);
        seen.insert(to_app(val->get_arg(0))->get_decl());
        seen.insert(to_app(val->get_arg(1))->get_decl());
        seen.insert(to_app(val->get_arg(2))->get_decl());
#else
        SASSERT(a->get_arg(0)->get_kind() == OP_EXTRACT);
        SASSERT(to_app(a->get_arg(0))->get_arg(0)->get_kind() == OP_EXTRACT);
        seen.insert(to_app(to_app(a->get_arg(0))->get_arg(0))->get_decl());
#endif

        if (!sgn && !sig && !exp)
            continue;

        expr_ref cv(m);
        cv = convert_bv2fp(var->get_range(), sgn, exp, sig);
        float_mdl->register_decl(var, cv);

        TRACE("fpa2bv_mc", tout << var->get_name() << " == " << mk_ismt2_pp(cv, m) << std::endl;);
    }

    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++)
    {
        func_decl * var = it->m_key;
        SASSERT(fu.is_rm(var->get_range()));
        expr * val = it->m_value;
        expr_ref fv(m);
        fv = convert_bv2rm(bv_mdl, var, val);
        TRACE("fpa2bv_mc", tout << var->get_name() << " == " << mk_ismt2_pp(fv, m) << std::endl;);
        float_mdl->register_decl(var, fv);
        seen.insert(to_app(val)->get_decl());
    }

    for (obj_map<func_decl, func_decl*>::iterator it = m_uf2bvuf.begin();
         it != m_uf2bvuf.end();
         it++) {
        seen.insert(it->m_value);

        func_decl * f = it->m_key;
        unsigned arity = f->get_arity();
        sort * rng = f->get_range();
        func_interp * flt_fi = alloc(func_interp, m, f->get_arity());

        func_interp * bv_fi = bv_mdl->get_func_interp(it->m_value);
        SASSERT(bv_fi->args_are_values());

        for (unsigned i = 0; i < bv_fi->num_entries(); i++) {
            func_entry const * bv_fe = bv_fi->get_entry(i);
            expr * const * bv_args = bv_fe->get_args();
            expr_ref_buffer new_args(m);

            for (unsigned j = 0; j < arity; j++) {
                sort * dj = f->get_domain(j);
                expr * aj = bv_args[j];
                if (fu.is_float(dj))
                    new_args.push_back(convert_bv2fp(bv_mdl, dj, aj));
                else if (fu.is_rm(dj)) {
                    expr_ref fv(m);
                    fv = convert_bv2rm(aj);
                    new_args.push_back(fv);
                }
                else
                    new_args.push_back(aj);
            }

            expr_ref ret(m);
            ret = bv_fe->get_result();
            if (fu.is_float(rng))
                ret = convert_bv2fp(bv_mdl, rng, ret);
            else if (fu.is_rm(rng))
                ret = convert_bv2rm(ret);

            flt_fi->insert_new_entry(new_args.c_ptr(), ret);
        }

        expr_ref els(m);
        els = bv_fi->get_else();
        if (fu.is_float(rng))
            els = convert_bv2fp(bv_mdl, rng, els);
        else if (fu.is_rm(rng))
            els = convert_bv2rm(els);

        flt_fi->set_else(els);

        float_mdl->register_decl(f, flt_fi);
    }

    // Keep all the non-float constants.
    unsigned sz = bv_mdl->get_num_constants();
    for (unsigned i = 0; i < sz; i++)
    {
        func_decl * c = bv_mdl->get_constant(i);
        if (!seen.contains(c))
            float_mdl->register_decl(c, bv_mdl->get_const_interp(c));
    }

    // And keep everything else
    sz = bv_mdl->get_num_functions();
    for (unsigned i = 0; i < sz; i++)
    {
        func_decl * f = bv_mdl->get_function(i);
        if (!seen.contains(f))
        {
            TRACE("fpa2bv_mc", tout << "Keeping: " << mk_ismt2_pp(f, m) << std::endl;);
            func_interp * val = bv_mdl->get_func_interp(f);
            float_mdl->register_decl(f, val);
        }
    }

    sz = bv_mdl->get_num_uninterpreted_sorts();
    for (unsigned i = 0; i < sz; i++)
    {
        sort * s = bv_mdl->get_uninterpreted_sort(i);
        ptr_vector<expr> u = bv_mdl->get_universe(s);
        float_mdl->register_usort(s, u.size(), u.c_ptr());
    }
}

model_converter * mk_fpa2bv_model_converter(ast_manager & m,
                                            obj_map<func_decl, expr*> const & const2bv,
                                            obj_map<func_decl, expr*> const & rm_const2bv,
                                            obj_map<func_decl, func_decl*> const & uf2bvuf) {
    return alloc(fpa2bv_model_converter, m, const2bv, rm_const2bv, uf2bvuf);
}
<commit_msg>build fix<commit_after>/*++
Copyright (c) 2012 Microsoft Corporation

Module Name:

    fpa2bv_model_converter.h

Abstract:

    Model conversion for fpa2bv_converter

Author:

    Christoph (cwinter) 2012-02-09

Notes:

--*/
#include"ast_smt2_pp.h"
#include"fpa2bv_model_converter.h"

void fpa2bv_model_converter::display(std::ostream & out) {
    out << "(fpa2bv-model-converter";
    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    for (obj_map<func_decl, func_decl*>::iterator it = m_uf2bvuf.begin();
         it != m_uf2bvuf.end();
         it++) {
        const symbol & n = it->m_key->get_name();
        out << "\n  (" << n << " ";
        unsigned indent = n.size() + 4;
        out << mk_ismt2_pp(it->m_value, m, indent) << ")";
    }
    out << ")" << std::endl;
}

model_converter * fpa2bv_model_converter::translate(ast_translation & translator) {
    fpa2bv_model_converter * res = alloc(fpa2bv_model_converter, translator.to());
    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++)
    {
        func_decl * k = translator(it->m_key);
        expr * v = translator(it->m_value);
        res->m_const2bv.insert(k, v);
        translator.to().inc_ref(k);
        translator.to().inc_ref(v);
    }
    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++)
    {
        func_decl * k = translator(it->m_key);
        expr * v = translator(it->m_value);
        res->m_rm_const2bv.insert(k, v);
        translator.to().inc_ref(k);
        translator.to().inc_ref(v);
    }
    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2fp(sort * s, expr * sgn, expr * exp, expr * sig) const {
    fpa_util fu(m);
    bv_util bu(m);
    unsynch_mpz_manager & mpzm = fu.fm().mpz_manager();
    unsynch_mpq_manager & mpqm = fu.fm().mpq_manager();

    expr_ref res(m);
    mpf fp_val;

    unsigned ebits = fu.get_ebits(s);
    unsigned sbits = fu.get_sbits(s);

    unsigned sgn_sz = 1;
    unsigned exp_sz = ebits;
    unsigned sig_sz = sbits - 1;

    rational sgn_q(0), sig_q(0), exp_q(0);

    if (sgn) bu.is_numeral(sgn, sgn_q, sgn_sz);
    if (exp) bu.is_numeral(exp, exp_q, exp_sz);
    if (sig) bu.is_numeral(sig, sig_q, sig_sz);

    // un-bias exponent
    rational exp_unbiased_q;
    exp_unbiased_q = exp_q - fu.fm().m_powers2.m1(ebits - 1);

    mpz sig_z; mpf_exp_t exp_z;
    mpzm.set(sig_z, sig_q.to_mpq().numerator());
    exp_z = mpzm.get_int64(exp_unbiased_q.to_mpq().numerator());

    fu.fm().set(fp_val, ebits, sbits, !mpqm.is_zero(sgn_q.to_mpq()), sig_z, exp_z);

    mpzm.del(sig_z);

    res = fu.mk_value(fp_val);

    TRACE("fpa2bv_mc", tout << "[" << mk_ismt2_pp(sgn, m) <<
                               " " << mk_ismt2_pp(exp, m) <<
                               " " << mk_ismt2_pp(sig, m) << "] == " <<
                               mk_ismt2_pp(res, m) << std::endl;);

    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2fp(model * bv_mdl, sort * s, expr * bv) const {
    fpa_util fu(m);
    bv_util bu(m);

    SASSERT(bu.is_bv(bv));

    unsigned ebits = fu.get_ebits(s);
    unsigned sbits = fu.get_sbits(s);
    unsigned bv_sz = sbits + ebits;

    expr_ref sgn(m), exp(m), sig(m);
    sgn = bu.mk_extract(bv_sz - 1, bv_sz - 1, bv);
    exp = bu.mk_extract(bv_sz - 2, sbits - 1, bv);
    sig = bu.mk_extract(sbits - 2, 0, bv);

    expr_ref v_sgn(m), v_exp(m), v_sig(m);
    bv_mdl->eval(sgn, v_sgn);
    bv_mdl->eval(exp, v_exp);
    bv_mdl->eval(sig, v_sig);

    return convert_bv2fp(s, v_sgn, v_exp, v_sig);
}

expr_ref fpa2bv_model_converter::convert_bv2rm(expr * eval_v) const {
    fpa_util fu(m);
    bv_util bu(m);
    expr_ref res(m);
    rational bv_val(0);
    unsigned sz = 0;

    if (bu.is_numeral(eval_v, bv_val, sz)) {
        SASSERT(bv_val.is_uint64());
        switch (bv_val.get_uint64()) {
        case BV_RM_TIES_TO_AWAY: res = fu.mk_round_nearest_ties_to_away(); break;
        case BV_RM_TIES_TO_EVEN: res = fu.mk_round_nearest_ties_to_even(); break;
        case BV_RM_TO_NEGATIVE: res = fu.mk_round_toward_negative(); break;
        case BV_RM_TO_POSITIVE: res = fu.mk_round_toward_positive(); break;
        case BV_RM_TO_ZERO:
        default: res = fu.mk_round_toward_zero();
        }
    }

    return res;
}

expr_ref fpa2bv_model_converter::convert_bv2rm(model * bv_mdl, func_decl * var, expr * val) const {
    fpa_util fu(m);
    bv_util bu(m);
    expr_ref res(m);

    expr_ref eval_v(m);

    if (val && bv_mdl->eval(val, eval_v, true))
        res = convert_bv2rm(eval_v);

    return res;
}

void fpa2bv_model_converter::convert(model * bv_mdl, model * float_mdl) {
    fpa_util fu(m);
    bv_util bu(m);
    unsynch_mpz_manager & mpzm = fu.fm().mpz_manager();
    unsynch_mpq_manager & mpqm = fu.fm().mpq_manager();

    TRACE("fpa2bv_mc", tout << "BV Model: " << std::endl;
    for (unsigned i = 0; i < bv_mdl->get_num_constants(); i++)
        tout << bv_mdl->get_constant(i)->get_name() << " --> " <<
        mk_ismt2_pp(bv_mdl->get_const_interp(bv_mdl->get_constant(i)), m) << std::endl;
    );

    obj_hashtable<func_decl> seen;

    for (obj_map<func_decl, expr*>::iterator it = m_const2bv.begin();
         it != m_const2bv.end();
         it++)
    {
        func_decl * var = it->m_key;
        app * val = to_app(it->m_value);
        SASSERT(fu.is_float(var->get_range()));
        SASSERT(var->get_range()->get_num_parameters() == 2);

        expr_ref sgn(m), sig(m), exp(m);
        bv_mdl->eval(val->get_arg(0), sgn, true);
        bv_mdl->eval(val->get_arg(1), exp, true);
        bv_mdl->eval(val->get_arg(2), sig, true);

        SASSERT(val->is_app_of(fu.get_family_id(), OP_FPA_FP));

#ifdef Z3DEBUG
        SASSERT(to_app(val->get_arg(0))->get_decl()->get_arity() == 0);
        SASSERT(to_app(val->get_arg(1))->get_decl()->get_arity() == 0);
        SASSERT(to_app(val->get_arg(2))->get_decl()->get_arity() == 0);
        seen.insert(to_app(val->get_arg(0))->get_decl());
        seen.insert(to_app(val->get_arg(1))->get_decl());
        seen.insert(to_app(val->get_arg(2))->get_decl());
#else
        SASSERT(a->get_arg(0)->get_kind() == OP_EXTRACT);
        SASSERT(to_app(a->get_arg(0))->get_arg(0)->get_kind() == OP_EXTRACT);
        seen.insert(to_app(to_app(val->get_arg(0))->get_arg(0))->get_decl());
#endif

        if (!sgn && !sig && !exp)
            continue;

        expr_ref cv(m);
        cv = convert_bv2fp(var->get_range(), sgn, exp, sig);
        float_mdl->register_decl(var, cv);

        TRACE("fpa2bv_mc", tout << var->get_name() << " == " << mk_ismt2_pp(cv, m) << std::endl;);
    }

    for (obj_map<func_decl, expr*>::iterator it = m_rm_const2bv.begin();
         it != m_rm_const2bv.end();
         it++)
    {
        func_decl * var = it->m_key;
        SASSERT(fu.is_rm(var->get_range()));
        expr * val = it->m_value;
        expr_ref fv(m);
        fv = convert_bv2rm(bv_mdl, var, val);
        TRACE("fpa2bv_mc", tout << var->get_name() << " == " << mk_ismt2_pp(fv, m) << std::endl;);
        float_mdl->register_decl(var, fv);
        seen.insert(to_app(val)->get_decl());
    }

    for (obj_map<func_decl, func_decl*>::iterator it = m_uf2bvuf.begin();
         it != m_uf2bvuf.end();
         it++) {
        seen.insert(it->m_value);

        func_decl * f = it->m_key;
        unsigned arity = f->get_arity();
        sort * rng = f->get_range();
        func_interp * flt_fi = alloc(func_interp, m, f->get_arity());

        func_interp * bv_fi = bv_mdl->get_func_interp(it->m_value);
        SASSERT(bv_fi->args_are_values());

        for (unsigned i = 0; i < bv_fi->num_entries(); i++) {
            func_entry const * bv_fe = bv_fi->get_entry(i);
            expr * const * bv_args = bv_fe->get_args();
            expr_ref_buffer new_args(m);

            for (unsigned j = 0; j < arity; j++) {
                sort * dj = f->get_domain(j);
                expr * aj = bv_args[j];
                if (fu.is_float(dj))
                    new_args.push_back(convert_bv2fp(bv_mdl, dj, aj));
                else if (fu.is_rm(dj)) {
                    expr_ref fv(m);
                    fv = convert_bv2rm(aj);
                    new_args.push_back(fv);
                }
                else
                    new_args.push_back(aj);
            }

            expr_ref ret(m);
            ret = bv_fe->get_result();
            if (fu.is_float(rng))
                ret = convert_bv2fp(bv_mdl, rng, ret);
            else if (fu.is_rm(rng))
                ret = convert_bv2rm(ret);

            flt_fi->insert_new_entry(new_args.c_ptr(), ret);
        }

        expr_ref els(m);
        els = bv_fi->get_else();
        if (fu.is_float(rng))
            els = convert_bv2fp(bv_mdl, rng, els);
        else if (fu.is_rm(rng))
            els = convert_bv2rm(els);

        flt_fi->set_else(els);

        float_mdl->register_decl(f, flt_fi);
    }

    // Keep all the non-float constants.
    unsigned sz = bv_mdl->get_num_constants();
    for (unsigned i = 0; i < sz; i++)
    {
        func_decl * c = bv_mdl->get_constant(i);
        if (!seen.contains(c))
            float_mdl->register_decl(c, bv_mdl->get_const_interp(c));
    }

    // And keep everything else
    sz = bv_mdl->get_num_functions();
    for (unsigned i = 0; i < sz; i++)
    {
        func_decl * f = bv_mdl->get_function(i);
        if (!seen.contains(f))
        {
            TRACE("fpa2bv_mc", tout << "Keeping: " << mk_ismt2_pp(f, m) << std::endl;);
            func_interp * val = bv_mdl->get_func_interp(f);
            float_mdl->register_decl(f, val);
        }
    }

    sz = bv_mdl->get_num_uninterpreted_sorts();
    for (unsigned i = 0; i < sz; i++)
    {
        sort * s = bv_mdl->get_uninterpreted_sort(i);
        ptr_vector<expr> u = bv_mdl->get_universe(s);
        float_mdl->register_usort(s, u.size(), u.c_ptr());
    }
}

model_converter * mk_fpa2bv_model_converter(ast_manager & m,
                                            obj_map<func_decl, expr*> const & const2bv,
                                            obj_map<func_decl, expr*> const & rm_const2bv,
                                            obj_map<func_decl, func_decl*> const & uf2bvuf) {
    return alloc(fpa2bv_model_converter, m, const2bv, rm_const2bv, uf2bvuf);
}
<|endoftext|>