#include "cvmmap_streamer/ipc/contracts.hpp"

#include <cvmmap/parser.hpp>

#include <algorithm>
#include <limits>

namespace cvmmap_streamer::ipc {

namespace {

	constexpr std::size_t kSyncMessageSize            = 48;
	constexpr std::size_t kModuleStatusMessageSize    = 32;
	constexpr std::size_t kControlRequestBaseSize     = 36;
	constexpr std::size_t kControlResponseBaseSize    = 40;
	constexpr std::size_t kFrameMetadataRequiredBytes = 36;

	constexpr std::size_t kSyncFrameCountOffset = 4;
	constexpr std::size_t kSyncTimestampOffset  = 16;
	constexpr std::size_t kSyncLabelOffset      = 24;

	constexpr std::size_t kModuleStatusCodeOffset  = 4;
	constexpr std::size_t kModuleStatusLabelOffset = 8;

	constexpr std::size_t kControlCommandOffset    = 4;
	constexpr std::size_t kControlReqLabelOffset   = 8;
	constexpr std::size_t kControlReqLengthOffset  = 32;
	constexpr std::size_t kControlReqPayloadOffset = 34;

	constexpr std::size_t kControlRespCodeOffset    = 8;
	constexpr std::size_t kControlRespLabelOffset   = 12;
	constexpr std::size_t kControlRespLengthOffset  = 36;
	constexpr std::size_t kControlRespPayloadOffset = 38;

	constexpr std::size_t kMetaVersionMajorOffset = 8;
	constexpr std::size_t kMetaVersionMinorOffset = 9;
	constexpr std::size_t kMetaFrameCountOffset   = 12;
	constexpr std::size_t kMetaTimestampOffset    = 16;
	constexpr std::size_t kMetaFrameInfoOffset    = 24;

	constexpr std::size_t kFrameInfoWidthOffset      = 0;
	constexpr std::size_t kFrameInfoHeightOffset     = 2;
	constexpr std::size_t kFrameInfoChannelsOffset   = 4;
	constexpr std::size_t kFrameInfoDepthOffset      = 5;
	constexpr std::size_t kFrameInfoPixelFmtOffset   = 6;
	constexpr std::size_t kFrameInfoBufferSizeOffset = 8;

	[[nodiscard]]
	std::uint16_t read_u16_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
		return static_cast<std::uint16_t>(bytes[offset]) |
			   (static_cast<std::uint16_t>(bytes[offset + 1]) << 8);
	}

	[[nodiscard]]
	std::uint32_t read_u32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
		return static_cast<std::uint32_t>(bytes[offset]) |
			   (static_cast<std::uint32_t>(bytes[offset + 1]) << 8) |
			   (static_cast<std::uint32_t>(bytes[offset + 2]) << 16) |
			   (static_cast<std::uint32_t>(bytes[offset + 3]) << 24);
	}

	[[nodiscard]]
	std::int32_t read_i32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
		return static_cast<std::int32_t>(read_u32_le(bytes, offset));
	}

	[[nodiscard]]
	std::uint64_t read_u64_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
		return static_cast<std::uint64_t>(bytes[offset]) |
			   (static_cast<std::uint64_t>(bytes[offset + 1]) << 8) |
			   (static_cast<std::uint64_t>(bytes[offset + 2]) << 16) |
			   (static_cast<std::uint64_t>(bytes[offset + 3]) << 24) |
			   (static_cast<std::uint64_t>(bytes[offset + 4]) << 32) |
			   (static_cast<std::uint64_t>(bytes[offset + 5]) << 40) |
			   (static_cast<std::uint64_t>(bytes[offset + 6]) << 48) |
			   (static_cast<std::uint64_t>(bytes[offset + 7]) << 56);
	}

	[[nodiscard]]
	bool is_supported_version(std::uint8_t major, std::uint8_t minor) {
		return major == kVersionMajor && minor == kVersionMinor;
	}

	[[nodiscard]]
	std::expected<Depth, ParseError> validate_depth(std::uint8_t depth_raw) {
		if (depth_raw > static_cast<std::uint8_t>(Depth::F16)) {
			return std::unexpected(ParseError::InvalidDepth);
		}
		return static_cast<Depth>(depth_raw);
	}

	[[nodiscard]]
	std::expected<PixelFormat, ParseError> validate_pixel_format(std::uint8_t pixel_format_raw) {
		if (pixel_format_raw > static_cast<std::uint8_t>(PixelFormat::YUYV)) {
			return std::unexpected(ParseError::InvalidPixelFormat);
		}
		return static_cast<PixelFormat>(pixel_format_raw);
	}

	[[nodiscard]]
	DepthUnit from_core_depth_unit(const cvmmap::DepthUnit unit) {
		switch (unit) {
		case cvmmap::DepthUnit::Millimeter:
			return DepthUnit::Millimeter;
		case cvmmap::DepthUnit::Meter:
			return DepthUnit::Meter;
		case cvmmap::DepthUnit::Unknown:
		default:
			return DepthUnit::Unknown;
		}
	}

	[[nodiscard]]
	std::expected<ModuleStatus, ParseError> validate_module_status(std::int32_t status_raw) {
		switch (status_raw) {
		case static_cast<std::int32_t>(ModuleStatus::Online):
			return ModuleStatus::Online;
		case static_cast<std::int32_t>(ModuleStatus::Offline):
			return ModuleStatus::Offline;
		case static_cast<std::int32_t>(ModuleStatus::StreamReset):
			return ModuleStatus::StreamReset;
		default:
			return std::unexpected(ParseError::InvalidModuleStatus);
		}
	}

	[[nodiscard]]
	ParseError map_core_parser_error(const std::string_view error) {
		if (error.contains("magic")) {
			return ParseError::InvalidMagic;
		}
		if (error.contains("version")) {
			return ParseError::UnsupportedVersion;
		}
		if (error.contains("depth_unit")) {
			return ParseError::InvalidDepthUnit;
		}
		if (error.contains("depth")) {
			return ParseError::InvalidDepth;
		}
		if (error.contains("pixel_format")) {
			return ParseError::InvalidPixelFormat;
		}
		return ParseError::InvalidSize;
	}

	[[nodiscard]]
	FrameMetadata from_core_metadata(const cvmmap::frame_metadata_t &metadata) {
		FrameMetadata converted{};
		std::copy_n(metadata.magic, converted.magic.size(), converted.magic.begin());
		converted.versions_major     = metadata.versions_major;
		converted.versions_minor     = metadata.versions_minor;
		converted.frame_count        = metadata.frame_count;
		converted.timestamp_ns       = metadata.timestamp_ns;
		converted.info.width         = metadata.info.width;
		converted.info.height        = metadata.info.height;
		converted.info.channels      = metadata.info.channels;
		converted.info.depth         = static_cast<Depth>(metadata.info.depth);
		converted.info.pixel_format  = static_cast<PixelFormat>(metadata.info.pixel_format);
		converted.info.buffer_size   = metadata.info.buffer_size;
		return converted;
	}

	[[nodiscard]]
	FrameInfo from_core_frame_info(const cvmmap::frame_info_t &info) {
		FrameInfo converted{};
		converted.width = info.width;
		converted.height = info.height;
		converted.channels = info.channels;
		converted.depth = static_cast<Depth>(info.depth);
		converted.pixel_format = static_cast<PixelFormat>(info.pixel_format);
		converted.buffer_size = info.buffer_size;
		return converted;
	}

	[[nodiscard]]
	std::size_t span_offset(
		const std::span<const std::uint8_t> outer,
		const std::span<const std::uint8_t> inner) {
		if (inner.empty()) {
			return 0;
		}
		return static_cast<std::size_t>(inner.data() - outer.data());
	}

	[[nodiscard]]
	std::size_t payload_bytes_for(
		const std::span<const std::uint8_t> payload_region,
		const cvmmap::parsed_frame_metadata_t &metadata) {
		std::size_t payload_bytes = metadata.left_plane.size();
		const auto consider = [&](std::span<const std::uint8_t> plane) {
			if (plane.empty()) {
				return;
			}
			payload_bytes = std::max(
				payload_bytes,
				span_offset(payload_region, plane) + plane.size());
		};
		consider(metadata.depth_plane);
		consider(metadata.confidence_plane);
		return payload_bytes;
	}

	[[nodiscard]]
	std::span<const std::uint8_t> translate_span(
		const std::span<const std::uint8_t> source_payload,
		const std::span<const std::uint8_t> destination_payload,
		const std::span<const std::uint8_t> source_plane) {
		if (source_plane.empty()) {
			return {};
		}
		const auto offset = span_offset(source_payload, source_plane);
		return destination_payload.subspan(offset, source_plane.size());
	}

}

std::string_view to_string(ParseError error) {
	switch (error) {
	case ParseError::BufferTooSmall:
		return "buffer too small";
	case ParseError::InvalidSize:
		return "invalid message size";
	case ParseError::InvalidMagic:
		return "invalid magic";
	case ParseError::UnsupportedVersion:
		return "unsupported version";
	case ParseError::InvalidDepth:
		return "invalid depth";
	case ParseError::InvalidDepthUnit:
		return "invalid depth unit";
	case ParseError::InvalidPixelFormat:
		return "invalid pixel format";
	case ParseError::InvalidModuleStatus:
		return "invalid module status";
	case ParseError::PayloadLengthMismatch:
		return "payload length mismatch";
	}
	return "unknown parse error";
}

std::string_view to_string(SnapshotError error) {
	switch (error) {
	case SnapshotError::InvalidShmLayout:
		return "invalid shared memory layout";
	case SnapshotError::DestinationTooSmall:
		return "destination buffer too small";
	case SnapshotError::TornRead:
		return "torn read";
	}
	return "unknown snapshot error";
}

std::expected<FrameMetadata, ParseError> parse_frame_metadata(std::span<const std::uint8_t> bytes) {
	if (bytes.size() < kFrameMetadataRequiredBytes) {
		return std::unexpected(ParseError::BufferTooSmall);
	}

	FrameMetadata metadata{};
	std::copy_n(bytes.begin(), kFrameMetadataMagic.size(), metadata.magic.begin());
	if (metadata.magic != kFrameMetadataMagic) {
		return std::unexpected(ParseError::InvalidMagic);
	}

	metadata.versions_major = bytes[kMetaVersionMajorOffset];
	metadata.versions_minor = bytes[kMetaVersionMinorOffset];
	if (!is_supported_version(metadata.versions_major, metadata.versions_minor)) {
		return std::unexpected(ParseError::UnsupportedVersion);
	}

	metadata.frame_count  = read_u32_le(bytes, kMetaFrameCountOffset);
	metadata.timestamp_ns = read_u64_le(bytes, kMetaTimestampOffset);

	auto frame_info_bytes = bytes.subspan(kMetaFrameInfoOffset);
	auto depth            = validate_depth(frame_info_bytes[kFrameInfoDepthOffset]);
	if (!depth) {
		return std::unexpected(depth.error());
	}
	auto pixel_format = validate_pixel_format(frame_info_bytes[kFrameInfoPixelFmtOffset]);
	if (!pixel_format) {
		return std::unexpected(pixel_format.error());
	}

	metadata.info.width        = read_u16_le(frame_info_bytes, kFrameInfoWidthOffset);
	metadata.info.height       = read_u16_le(frame_info_bytes, kFrameInfoHeightOffset);
	metadata.info.channels     = frame_info_bytes[kFrameInfoChannelsOffset];
	metadata.info.depth        = *depth;
	metadata.info.pixel_format = *pixel_format;
	metadata.info.buffer_size  = read_u32_le(frame_info_bytes, kFrameInfoBufferSizeOffset);

	return metadata;
}

std::expected<SyncMessage, ParseError> parse_sync_message(std::span<const std::uint8_t> bytes) {
	if (bytes.size() < kSyncMessageSize) {
		return std::unexpected(ParseError::BufferTooSmall);
	}
	if (bytes.size() != kSyncMessageSize) {
		return std::unexpected(ParseError::InvalidSize);
	}
	if (bytes[0] != kFrameTopicMagic) {
		return std::unexpected(ParseError::InvalidMagic);
	}
	if (!is_supported_version(bytes[2], bytes[3])) {
		return std::unexpected(ParseError::UnsupportedVersion);
	}

	SyncMessage message{};
	message.versions_major = bytes[2];
	message.versions_minor = bytes[3];
	message.frame_count    = read_u32_le(bytes, kSyncFrameCountOffset);
	message.timestamp_ns   = read_u64_le(bytes, kSyncTimestampOffset);
	std::copy_n(bytes.begin() + kSyncLabelOffset, kLabelLenMax, message.label_bytes.begin());

	return message;
}

std::expected<ModuleStatusMessage, ParseError> parse_module_status_message(std::span<const std::uint8_t> bytes) {
	if (bytes.size() < kModuleStatusMessageSize) {
		return std::unexpected(ParseError::BufferTooSmall);
	}
	if (bytes.size() != kModuleStatusMessageSize) {
		return std::unexpected(ParseError::InvalidSize);
	}
	if (bytes[0] != kModuleStatusMagic) {
		return std::unexpected(ParseError::InvalidMagic);
	}
	if (!is_supported_version(bytes[2], bytes[3])) {
		return std::unexpected(ParseError::UnsupportedVersion);
	}

	auto status = validate_module_status(read_i32_le(bytes, kModuleStatusCodeOffset));
	if (!status) {
		return std::unexpected(status.error());
	}

	ModuleStatusMessage message{};
	message.versions_major = bytes[2];
	message.versions_minor = bytes[3];
	message.module_status  = *status;
	std::copy_n(bytes.begin() + kModuleStatusLabelOffset, kLabelLenMax, message.label_bytes.begin());

	return message;
}

std::expected<ControlRequestMessage, ParseError> parse_control_request_message(std::span<const std::uint8_t> bytes) {
	if (bytes.size() < kControlRequestBaseSize) {
		return std::unexpected(ParseError::BufferTooSmall);
	}
	if (bytes[0] != kControlRequestMagic) {
		return std::unexpected(ParseError::InvalidMagic);
	}
	if (!is_supported_version(bytes[2], bytes[3])) {
		return std::unexpected(ParseError::UnsupportedVersion);
	}

	const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlReqLengthOffset));
	if (payload_size > bytes.size() - kControlReqPayloadOffset) {
		return std::unexpected(ParseError::PayloadLengthMismatch);
	}
	if (bytes.size() != kControlRequestBaseSize + payload_size) {
		return std::unexpected(ParseError::InvalidSize);
	}

	ControlRequestMessage message{};
	message.versions_major = bytes[2];
	message.versions_minor = bytes[3];
	message.command_id     = read_i32_le(bytes, kControlCommandOffset);
	std::copy_n(bytes.begin() + kControlReqLabelOffset, kLabelLenMax, message.label_bytes.begin());
	message.request_payload = bytes.subspan(kControlReqPayloadOffset, payload_size);

	return message;
}

std::expected<ControlResponseMessage, ParseError> parse_control_response_message(std::span<const std::uint8_t> bytes) {
	if (bytes.size() < kControlResponseBaseSize) {
		return std::unexpected(ParseError::BufferTooSmall);
	}
	if (bytes[0] != kControlResponseMagic) {
		return std::unexpected(ParseError::InvalidMagic);
	}
	if (!is_supported_version(bytes[2], bytes[3])) {
		return std::unexpected(ParseError::UnsupportedVersion);
	}

	const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlRespLengthOffset));
	if (payload_size > bytes.size() - kControlRespPayloadOffset) {
		return std::unexpected(ParseError::PayloadLengthMismatch);
	}
	if (bytes.size() != kControlResponseBaseSize + payload_size) {
		return std::unexpected(ParseError::InvalidSize);
	}

	ControlResponseMessage message{};
	message.versions_major = bytes[2];
	message.versions_minor = bytes[3];
	message.command_id     = read_i32_le(bytes, kControlCommandOffset);
	message.response_code  = read_i32_le(bytes, kControlRespCodeOffset);
	std::copy_n(bytes.begin() + kControlRespLabelOffset, kLabelLenMax, message.label_bytes.begin());
	message.response_payload = bytes.subspan(kControlRespPayloadOffset, payload_size);

	return message;
}

std::expected<ValidatedShmView, ParseError> validate_shm_region(std::span<const std::uint8_t> shm_region) {
	if (shm_region.size() < kShmPayloadOffset) {
		return std::unexpected(ParseError::BufferTooSmall);
	}

	auto metadata_result = cvmmap::parse_frame_metadata_regions(
		shm_region.first(kShmPayloadOffset),
		shm_region.subspan(kShmPayloadOffset));
	if (!metadata_result) {
		return std::unexpected(map_core_parser_error(metadata_result.error()));
	}

	const auto payload_region = shm_region.subspan(kShmPayloadOffset);
	const auto payload_bytes = payload_bytes_for(payload_region, *metadata_result);

	return ValidatedShmView{
		.metadata = from_core_metadata(metadata_result->normalized_metadata),
		.depth_unit = from_core_depth_unit(metadata_result->depth_unit),
		.payload  = payload_region.first(payload_bytes),
		.left = metadata_result->left_plane,
		.depth_info = metadata_result->depth_info
			? std::optional<FrameInfo>(from_core_frame_info(*metadata_result->depth_info))
			: std::nullopt,
		.depth = metadata_result->depth_plane,
		.confidence_info = metadata_result->confidence_info
			? std::optional<FrameInfo>(from_core_frame_info(*metadata_result->confidence_info))
			: std::nullopt,
		.confidence = metadata_result->confidence_plane};
}

std::expected<CoherentSnapshot, SnapshotError> read_coherent_snapshot(
	std::span<const std::uint8_t> shm_region,
	std::span<std::uint8_t> destination,
	const SnapshotReadHook &before_second_metadata_read) {
	auto first = validate_shm_region(shm_region);
	if (!first) {
		return std::unexpected(SnapshotError::InvalidShmLayout);
	}

	if (destination.size() < first->payload.size()) {
		return std::unexpected(SnapshotError::DestinationTooSmall);
	}

	std::copy(first->payload.begin(), first->payload.end(), destination.begin());

	if (before_second_metadata_read) {
		before_second_metadata_read();
	}

	auto second = validate_shm_region(shm_region);
	if (!second) {
		return std::unexpected(SnapshotError::InvalidShmLayout);
	}

	if (first->metadata.frame_count != second->metadata.frame_count ||
		first->metadata.timestamp_ns != second->metadata.timestamp_ns) {
		return std::unexpected(SnapshotError::TornRead);
	}

	const auto copied_payload = std::span<const std::uint8_t>(destination.data(), first->payload.size());

	return CoherentSnapshot{
		.metadata     = first->metadata,
		.depth_unit   = first->depth_unit,
		.left         = translate_span(first->payload, copied_payload, first->left),
		.depth_info   = first->depth_info,
		.depth        = translate_span(first->payload, copied_payload, first->depth),
		.confidence_info = first->confidence_info,
		.confidence   = translate_span(first->payload, copied_payload, first->confidence),
		.bytes_copied = first->payload.size()};
}

}