// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#pragma once

#include "carla/rpc/Location.h"

#include <cstdint>
#include <vector>

namespace carla {
namespace sensor {
namespace s11n {

  /// Helper class to store and serialize the data generated by a Lidar.
  ///
  /// The header of a Lidar measurement consists of an array of uint32_t's in
  /// the following layout
  ///
  ///    {
  ///      Horizontal angle (float),
  ///      Channel count,
  ///      Point count of channel 0,
  ///      ...
  ///      Point count of channel n,
  ///    }
  ///
  /// The points are stored in an array of floats
  ///
  ///    {
  ///      X0, Y0, Z0,
  ///      ...
  ///      Xn, Yn, Zn,
  ///    }
  ///
  /// @warning WritePoint should be called sequentially in the order in which
  /// the points are going to be stored, i.e., starting at channel zero and
  /// increasing steadily.
  class LidarMeasurement {
    static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");

    friend class LidarSerializer;
    friend class LidarHeaderView;

    enum Index : size_t {
      HorizontalAngle,
      ChannelCount,
      SIZE
    };

  public:

    explicit LidarMeasurement(uint32_t ChannelCount = 0u)
      : _header(Index::SIZE + ChannelCount, 0u) {
      _header[Index::ChannelCount] = ChannelCount;
    }

    LidarMeasurement &operator=(LidarMeasurement &&) = default;

    float GetHorizontalAngle() const {
      return reinterpret_cast<const float &>(_header[Index::HorizontalAngle]);
    }

    void SetHorizontalAngle(float angle) {
      std::memcpy(&_header[Index::HorizontalAngle], &angle, sizeof(uint32_t));
    }

    uint32_t GetChannelCount() const {
      return _header[Index::ChannelCount];
    }

    void Reset(uint32_t total_point_count) {
      std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
      _points.clear();
      _points.reserve(3u * total_point_count);
    }

    void WritePoint(uint32_t channel, rpc::Location point) {
      DEBUG_ASSERT(GetChannelCount() > channel);
      _header[Index::SIZE + channel] += 1u;
      _points.emplace_back(point.x);
      _points.emplace_back(point.y);
      _points.emplace_back(point.z);
    }

  private:

    std::vector<uint32_t> _header;

    std::vector<float> _points;
  };

} // namespace s11n
} // namespace sensor
} // namespace carla
