// Copyright 2022, FOSS-VG Developers and Contributers
//
// Author(s):
//   BodgeMaster
//
// This program is free software: you can redistribute it and/or modify it
// under the terms of the GNU Affero General Public License as published
// by the Free Software Foundation, version 3.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied
// warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
// See the GNU Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// version 3 along with this program.
// If not, see https://www.gnu.org/licenses/agpl-3.0.en.html

#pragma once

#include <cstdint>
#include <vector>

#include "../../error.hpp"

namespace Position {
    struct Location {
        // 26 bit integer
        int32_t x;
        // 12 bit integer
        int16_t y;
        // 26 bit integer
        int32_t z;
    }

    Location fromPosition(uint64_t position) {
        Location location;

        location.x = (int32_t) ((0xFFFFFFC000000000 & position) >> 38);
        if (location.x >= 0x02000000) {
            location.x -= 0x04000000;
        }

        location.y = (int16_t) ((0x0000000000000FFF & position));
        if (location.y >= 0x0800) {
            location.y -= 0x1000;
        }

        location.z = (int32_t) ((0x0000003FFFFFF000 & position) >> 12);
        if (location.z >= 0x02000000) {
            location.z -= 0x04000000;
        }
    }

    ErrorOr<Location> fromPosition(std::vector data, uint64_t initialPosition=0) {
        if (inititalPosition >= data.size()) {
            return ErrorOr<Location>(true, ErrorCodes::OUT_OF_BOUNDS);
        }
        if (initialPosition+7 >= data.size()) {
            return ErrorOr<Location>(true, ErrorCodes::OVERRUN);
        }

        uint64_t deserialized = 0;
        for (uint8_t i=0; i<8; i++) {
            deserialized += (uint64_t) data[initialPosition+i] << i*8;
        }

        return ErrorOr<Location>(fromPosition(deserialized));
    }
}
