// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include <cmath>
#include "Coord.h"
#include "JFJochException.h"

Coord::Coord() {
    x = 0.0; y = 0.0; z = 0.0;
}

Coord::Coord(const float in[3]) {
    x = in[0];
    y = in[1];
    z = in[2];
}

Coord::Coord(float in_x, float in_y, float in_z) {
    x = in_x;
    y = in_y;
    z = in_z;
}

Coord Coord::operator+(const Coord &in) const {
    return Coord(this->x+in.x, this->y+in.y, this->z+in.z);
}

Coord Coord::operator-(const Coord &in) const {
    return Coord(this->x-in.x, this->y-in.y, this->z-in.z);
}

Coord Coord::operator*(float in) const {
    return Coord(this->x*in, this->y*in, this->z*in);
}

Coord Coord::operator/(float in) const {
    return Coord(this->x/in, this->y/in, this->z/in);
};

Coord Coord::operator-() const {
    return Coord(- this->x, -this->y, -this->z);
}

Coord& Coord::operator+=(const Coord &in) {
    this->x += in.x;
    this->y += in.y;
    this->z += in.z;
    return *this;
}

Coord& Coord::operator-=(const Coord &in) {
    this->x -= in.x;
    this->y -= in.y;
    this->z -= in.z;
    return *this;
}

Coord& Coord::operator*=(float in) {
    this->x *= in;
    this->y *= in;
    this->z *= in;
    return *this;
}

Coord& Coord::operator/=(float in) {
    this->x /= in;
    this->y /= in;
    this->z /= in;
    return *this;
}

Coord Coord::operator%(const Coord &in) const {
    return Coord(this->y * in.z - this->z * in.y,
            this->z * in.x - this->x * in.z,
            this->x * in.y - this->y * in.x);
}; // Cross product

float Coord::operator*(const Coord &in) const {
    return this->x * in.x + this->y * in.y + this->z * in.z;
};

bool Coord::operator==(const Coord &other) const {
    if ((this->x == other.x) && (this->y == other.y) && (this->z == other.z))
        return true;
    else
        return false;
}

float Coord::Length() const {
    return sqrt(this->x*this->x + this->y*this->y + this->z*this->z);
}

Coord Coord::Normalize() const {
    float len = Length();
    if (len < 1e-12)
        return Coord(0,0,0);
    return Coord(this->x/len, this->y/len, this->z/len);
}

Coord operator*(float in1, const Coord& in2) {
    return in2 * in1;
}

const float& Coord::operator[](int64_t val) const {
    switch (val) {
        case 0:
            return x;
        case 1:
            return y;
        case 2:
            return z;
        default:
            throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                                  "Coord index must be in range 0-2");
    }
}

float& Coord::operator[](int64_t val) {
    switch (val) {
        case 0:
            return x;
        case 1:
            return y;
        case 2:
            return z;
        default:
            throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                                  "Coord index must be in range 0-2");
    }
}

std::ostream &operator<<( std::ostream &output, const Coord &in ) {
    output << in.x << " " << in.y << " " << in.z;
    return output;
}

float angle_deg(const Coord &c1, const Coord &c2) {
    float cos_ang = c1 * c2 / (c1.Length() * c2.Length());
    const float epsilon = 1e-5f;

    if (cos_ang > 1.0f && cos_ang < 1.0f + epsilon)
        cos_ang = 1.0f;
    else if (cos_ang < -1.0f && cos_ang > -1.0f - epsilon)
        cos_ang = -1.0f;

    return acosf(cos_ang) * (180.0f / static_cast<float>(M_PI));
}

void Coord::swap(Coord &other) noexcept {
    std::swap(x, other.x);
    std::swap(y, other.y);
    std::swap(z, other.z);
}

// Then outside the class but in the same namespace:
inline void swap(Coord& a, Coord& b) noexcept {
    a.swap(b);
}


RotMatrix::RotMatrix() {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++)
            v[i][j] = (i == j) ? 1 : 0;
    }
};

RotMatrix::RotMatrix(float alpha, const Coord &dir) {
    float c = cosf(alpha);
    float s = sinf(alpha);
    float t = 1.0f - c;
    Coord n = dir.Normalize();

    v[0][0] = t * n.x * n.x + c;
    v[0][1] = t * n.x * n.y - s * n.z;
    v[0][2] = t * n.x * n.z + s * n.y;
    v[1][0] = t * n.x * n.y + s * n.z;
    v[1][1] = t * n.y * n.y + c;
    v[1][2] = t * n.y * n.z - s * n.x;
    v[2][0] = t * n.x * n.z - s * n.y;
    v[2][1] = t * n.y * n.z + s * n.x;
    v[2][2] = t * n.z * n.z + c;
}

Coord RotMatrix::operator*(const Coord &in) const {
    return {
            v[0][0] * in.x + v[0][1] * in.y + v[0][2] * in.z,
            v[1][0] * in.x + v[1][1] * in.y + v[1][2] * in.z,
            v[2][0] * in.x + v[2][1] * in.y + v[2][2] * in.z
    };
}

RotMatrix RotMatrix::operator*(const RotMatrix &other) const {
    RotMatrix result;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result.v[i][j] = v[i][0] * other.v[0][j] +
                             v[i][1] * other.v[1][j] +
                             v[i][2] * other.v[2][j];
        }
    }
    return result;
}

RotMatrix RotMatrix::transpose() const {
    RotMatrix result;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result.v[i][j] = v[j][i];
        }
    }
    return result;
}

RotMatrix RotMatrix::invert() const {
    RotMatrix result;
    float det = v[0][0] * (v[1][1] * v[2][2] - v[1][2] * v[2][1])
                - v[0][1] * (v[1][0] * v[2][2] - v[1][2] * v[2][0])
                + v[0][2] * (v[1][0] * v[2][1] - v[1][1] * v[2][0]);

    if (det == 0)
        throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                              "Singular matrix, cannot invert");

    float invDet = 1.0f / det;

    result.v[0][0] = (v[1][1] * v[2][2] - v[1][2] * v[2][1]) * invDet;
    result.v[0][1] = (v[0][2] * v[2][1] - v[0][1] * v[2][2]) * invDet;
    result.v[0][2] = (v[0][1] * v[1][2] - v[0][2] * v[1][1]) * invDet;
    result.v[1][0] = (v[1][2] * v[2][0] - v[1][0] * v[2][2]) * invDet;
    result.v[1][1] = (v[0][0] * v[2][2] - v[0][2] * v[2][0]) * invDet;
    result.v[1][2] = (v[0][2] * v[1][0] - v[0][0] * v[1][2]) * invDet;
    result.v[2][0] = (v[1][0] * v[2][1] - v[1][1] * v[2][0]) * invDet;
    result.v[2][1] = (v[0][1] * v[2][0] - v[0][0] * v[2][1]) * invDet;
    result.v[2][2] = (v[0][0] * v[1][1] - v[0][1] * v[1][0]) * invDet;

    return result;
}

RotMatrix RotMatrix::operator!() const {
    return invert();
}

std::vector<float> RotMatrix::arr() const {
    std::vector<float> ret;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            ret.push_back(v[i][j]);
        }
    }
    return ret;
}