#include "pro_codec.h"
#include "unitree_legged_sdk/unitree_legged_sdk.h"

#include <chrono>
#include <cstdint>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>

namespace {

using Clock = std::chrono::steady_clock;

#ifdef GO1_PRO_BENCHMARK_CAPTURE
std::vector<uint8_t> ReadFile(const std::string& path) {
  std::ifstream stream(path, std::ios::binary);
  return std::vector<uint8_t>(
      (std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
}
#endif

double NanosecondsPerIteration(Clock::time_point start, Clock::time_point end,
                               std::size_t iterations) {
  return std::chrono::duration<double, std::nano>(end - start).count() /
         static_cast<double>(iterations);
}

}  // namespace

int main() {
  constexpr std::size_t kIterations = 200000;
  go1_pro_internal::ProCodec codec(go1_pro_internal::ProCodec::FindStateFile());

  UNITREE_LEGGED_SDK::LowCmd cmd{};
  go1_pro_internal::InitLowCmd(cmd);
  volatile uint64_t checksum = 0;

  for (std::size_t i = 0; i < 100; ++i) {
    const auto packet = codec.EncodeLowCmd(cmd);
    checksum += packet[i % packet.size()];
  }
  const auto encode_start = Clock::now();
  for (std::size_t i = 0; i < kIterations; ++i) {
    const auto packet = codec.EncodeLowCmd(cmd);
    checksum += packet[i % packet.size()];
  }
  const auto encode_end = Clock::now();

  std::cout << std::fixed << std::setprecision(1)
            << "EncodeLowCmd: "
            << NanosecondsPerIteration(encode_start, encode_end, kIterations)
            << " ns/frame\n";

#ifdef GO1_PRO_BENCHMARK_CAPTURE
  const auto capture = ReadFile(GO1_PRO_BENCHMARK_CAPTURE);
  if (capture.size() != go1_pro_internal::kLowStateDatagramSize) {
    std::cerr << "unexpected benchmark capture size: " << capture.size() << '\n';
    return 1;
  }

  UNITREE_LEGGED_SDK::LowState state{};
  for (std::size_t i = 0; i < 100; ++i) {
    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
    checksum += state.motorState[i % state.motorState.size()].temperature;
  }
  const auto decode_start = Clock::now();
  for (std::size_t i = 0; i < kIterations; ++i) {
    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
    checksum += state.motorState[i % state.motorState.size()].temperature;
  }
  const auto decode_end = Clock::now();
  std::cout << "DecodeLowState: "
            << NanosecondsPerIteration(decode_start, decode_end, kIterations)
            << " ns/frame\n";
#else
  std::cout << "DecodeLowState: skipped (capture fixture unavailable)\n";
#endif

  std::cout << "checksum: " << checksum << '\n';
  return 0;
}