#include "vn-interface/vn200.h"

namespace VN200 {
namespace SPI {

// payload_size_t::payload_size_t() {
//   group1 = {{BinaryGroups::Group1::TimeStartup, 8},   {BinaryGroups::Group1::TimeGps, 8},     {BinaryGroups::Group1::TimeSyncIn, 8},
//             {BinaryGroups::Group1::YawPitchRoll, 12}, {BinaryGroups::Group1::Quaternion, 16}, {BinaryGroups::Group1::AngularRate, 12},
//             {BinaryGroups::Group1::Position, 24},     {BinaryGroups::Group1::Velocity, 12},   {BinaryGroups::Group1::Accel, 12},
//             {BinaryGroups::Group1::Imu, 24},          {BinaryGroups::Group1::MagPres, 20},    {BinaryGroups::Group1::DeltaTheta, 28},
//             {BinaryGroups::Group1::InsStatus, 2},     {BinaryGroups::Group1::SyncInCnt, 4},   {BinaryGroups::Group1::TimeGpsPps, 8},
//             {BinaryGroups::Group1::EMPTY15, 0}};
// }

// uint16_t configuration_t::size() { return sizeof(AsyncMode) + sizeof(RateDivisor) + sizeof(OutputGroup) + NumOutputFields * sizeof(uint16_t); }

// uint8_t *configuration_t::to_array() {
//   v.clear();
//   v.push_back(AsyncMode && 0xFF);
//   v.push_back(AsyncMode >> 8);
//   v.push_back(RateDivisor && 0xFF);
//   v.push_back(RateDivisor >> 8);
//   v.push_back(OutputGroup && 0xFF);
//   v.push_back(OutputGroup >> 8);
//   for (uint8_t i = 0; i < NumOutputFields; i++) {
//     v.push_back(OutputFields[i] && 0xFF);
//     v.push_back(OutputFields[i] >> 8);
//   }
//   return &v[0];
// }

// void configuration_t::setBinaryGroups(std::vector<SPI::BinaryGroups::bit_offset_t> desired_groups) {
//   bool hit = false;
//   /* Iterate over every bit of the OutputGroup */
//   for (uint8_t i = 0; i < 8; i++) {
//     for (auto desired_group : desired_groups) {
//       if (desired_group == i) {
//         hit = true;
//         break;
//       }
//     }
//     if (hit) {
//       OutputGroup++;
//       hit = false;
//     }
//     OutputGroup = OutputGroup << 1;
//   }
// }

// void configuration_t::setOutputFields(SPI::BinaryGroups::bit_offset_t group_bit_offset, std::vector<int> desired_output_fields) {
//   bool hit = false;
//   /* Iterate over every bit of the OutputGroup */
//   for (uint8_t i = 0; i < 16; i++) {
//     for (auto desired_output_field : desired_output_fields) {
//       if (desired_output_field == i) {
//         hit = true;
//         break;
//       }
//     }
//     if (hit) {
//       OutputFields[group_bit_offset]++;
//       hit = false;
//     }
//     OutputFields[group_bit_offset] = OutputFields[group_bit_offset] << 1;
//   }
// }

} // namespace SPI
// namespace SPI
} // namespace VN200