#include "PS3000CAmpereControl.hpp"
#include "schneide_base/Logger.hpp"

using namespace vera;

namespace {
std::string withDecimalPoint(double value)
{
  auto result = fmt::format("{0:.2f}", value);
  for (char& x : result)
    if (x == ',')
      x = '.';
  return result;
}
}

PS3000CAmpereControl::PS3000CAmpereControl(std::string const& address, std::string const& port)
  : mSocket(mService)
{
  asio::ip::tcp::resolver resolver(mService);
  asio::connect(mSocket, resolver.resolve(address, port));
}

PS3000CAmpereControl::~PS3000CAmpereControl() = default;

void PS3000CAmpereControl::start(Interactor& interactor)
{
  query(interactor, "*IDN?");
  query(interactor, "SOUR:CURR?");
  
  command(interactor, "SYST:LOCK 1");
  command(interactor, "OUTP ON");// Output on!

  // Set voltage to maxium
  command(interactor, "SOUR:VOLTAGE MAX");
}

void PS3000CAmpereControl::stop(Interactor& interactor)
{
  command(interactor, "OUTP OFF");// Output off!
  command(interactor, "SYST:LOCK 0");
}

void PS3000CAmpereControl::setAmpere(Interactor& interactor, double ampere)
{
  command(interactor, "SOUR:CURR " + withDecimalPoint(ampere));
}

inline void vera::PS3000CAmpereControl::send(std::string const & message)
{
  asio::write(mSocket, asio::buffer(message));
}

inline std::string vera::PS3000CAmpereControl::receive()
{
  auto constexpr END_MARKER = '\n';
  std::string result;

  asio::read_until(mSocket, asio::dynamic_buffer(result), '\n');

  if (!result.empty() && result.back() == END_MARKER)
    result.pop_back();

  return result;
}

std::string PS3000CAmpereControl::query(Interactor& interactor, std::string message)
{
  command(interactor, std::move(message));
  auto response = receive();
  Logger::get()->info("PS3000C Response \"{0}\"", response);
  return response;
}

void PS3000CAmpereControl::command(Interactor& interactor, std::string message)
{
  auto constexpr DEAD_TIME = std::chrono::milliseconds(300);
  Logger::get()->info("PS3000C Command \"{0}\"", message);
  message += "\n";
  send(message);
  interactor.wait(DEAD_TIME);
}
