// 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 "Sensor/SensorDataView.h"

#include "Containers/Array.h"

/// Stores the data generated by ALidar. To be used by ALidar solely.
///
/// The header consists of an array of uint32's in the following layout
///
///    {
///      Frame number (uint64)
///      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,
///    }
///
class FLidarMeasurement {
  static_assert(sizeof(float) == sizeof(uint32), "Invalid float size");
public:

  explicit FLidarMeasurement(uint32 SensorId = 0u, uint32 ChannelCount = 0u)
    : SensorId(SensorId)
  {
    Header.AddDefaulted(4u + ChannelCount);
    Header[3] = ChannelCount;
  }

  FLidarMeasurement &operator=(FLidarMeasurement &&Other)
  {
    SensorId = Other.SensorId;
    Header = std::move(Other.Header);
    Points = std::move(Other.Points);
    Other.SensorId = 0u;
    return *this;
  }

  void SetFrameNumber(uint64 FrameNumber)
  {
    std::memcpy(Header.GetData(), reinterpret_cast<const void *>(&FrameNumber), 2u);
  }

  float GetHorizontalAngle() const
  {
    return reinterpret_cast<const float &>(Header[2]);
  }

  void SetHorizontalAngle(float HorizontalAngle)
  {
    Header[2] = reinterpret_cast<const uint32 &>(HorizontalAngle);
  }

  uint32 GetChannelCount() const
  {
    return Header[3];
  }

  void Reset(uint32 TotalPointCount)
  {
    std::memset(Header.GetData() + 4u, 0, sizeof(uint32) * GetChannelCount());
    Points.Reset(3u * TotalPointCount);
  }

  void WritePoint(uint32 Channel, const FVector &Point)
  {
    check(Header[3] > Channel);
    Header[4u + Channel] += 1u;
    constexpr float TO_METERS = 1e-2f;
    Points.Emplace(TO_METERS * Point.X);
    Points.Emplace(TO_METERS * Point.Y);
    Points.Emplace(TO_METERS * Point.Z);
  }

  FSensorDataView GetView() const
  {
    return FSensorDataView(SensorId, Header, Points);
  }

private:

  uint32 SensorId;

  TArray<uint32> Header;

  TArray<float> Points;
};
