#include <cmath>
#include <cstdint>
#include <algorithm>

#include "math.h"
#include "../object/point.h"

float distance(Point& a, Point& b) {
    return sqrt( pow(a.getX() - b.getX(), 2) +
                 pow(a.getY() - b.getY(), 2) +
                 pow(a.getZ() - b.getZ(), 2)
            );
}

uint64_t st64(std::string hex) {
    std::reverse(hex.begin(), hex.end());
    uint64_t res = 0;

    for (int i = 0; i<hex.length(); i++) {
        if (hex.at(i) >= 'A' && hex.at(i) <= 'F') {
            res += (hex.at(i) - 'A' + 10) * pow(16, i);
        } else if (hex.at(i) >= 'a' && hex.at(i) <= 'f') {
            res += (hex.at(i) - 'a' + 10) * pow(16, i);
        } else if (hex.at(i) >= '0' && hex.at(i) <= '9') {
            res += (hex.at(i) - '0') * pow(16, i);
        } else {
            std::cerr << "Color value seem not to be hexadecimal, returning 0 (" << hex << ")." << std::endl;
            return 0;
        }
    }

    return res;
}

uint8_t getR(uint64_t color) { return (color << 16) >> 56; }
uint8_t getG(uint64_t color) { return (color << 24) >> 56; }
uint8_t getB(uint64_t color) { return (color << 32) >> 56; }
uint8_t getA(uint64_t color) { return (color << 40) >> 56; }
uint8_t getL(uint64_t color) { return (color << 48) >> 56; }
uint8_t getZ(uint64_t color) { return (color << 56) >> 56; }