#include "vector.hpp"
#include "point.hpp"
#include "matrix.hpp"

#include <stdexcept>
#include <iostream>
#include <cmath>

using namespace visual_engine::detail::linalg;
using namespace visual_engine::detail::matrix;

coord_t vector::length()    const { return std::sqrt(x_*x_ + y_*y_ + z_*z_); }
bool vector::is_valid()     const { return (std::isfinite(x_) && std::isfinite(y_) && std::isfinite(z_)); }
void vector::console_dump() const { std::cout << "vec=(" << x_ << "," << y_ << "," << z_ << ")" << " len=" << length() << std::endl; }

vector& vector::normalize() {
    coord_t len(length());
    if (linalg::equal(len, 0)) return *this; // @TODO maybe better throw an exception
    x_ = x_ / len;
    y_ = y_ / len;
    z_ = z_ / len;
    return *this;
}

vector vector::normalized() const {
    coord_t len(length());
    if (linalg::equal(len, 0)) return *this; // @TODO maybe better throw an exception
    coord_t x = x_ / len,
            y = y_ / len,
            z = z_ / len;
    return vector{x, y, z};
}

vector vector::product(const vector& vec) const {
    coord_t x = y_ * vec.z_ - z_ * vec.y_,
            y = z_ * vec.x_ - x_ * vec.z_,
            z = x_ * vec.y_ - y_ * vec.x_;
    return vector{x, y, z};
}

coord_t vector::scal_product(const vector& vec) const {
    return x_ * vec.x_ + y_ * vec.y_ + z_ * vec.z_;
}

vector::vector(const point& p) : x_(p.get_x()), y_(p.get_y()), z_(p.get_z()) {}

struct bad_size_for_vector : public std::runtime_error {
    bad_size_for_vector() : std::runtime_error("Matrix is not 3x1 to construct vector\n") {}
};

void vector::rotate(const vector &v) {
    matrix_t<float> vec(*this);
    matrix_t<float> new_vec(matrix_t<float>::get_rotate_matix(v, v.length()).product(vec));
    x_ = new_vec[0][0];
    y_ = new_vec[1][0];
    z_ = new_vec[2][0];
}

// vector::vector(const matrix::matrix_t<float> &mtx) {
//     if (mtx.get_ncols() != 1 || mtx.get_nrows() != 3) {
//         throw bad_size_for_vector();
//     }

//     x_ = mtx[0][0];
//     y_ = mtx[1][0];
//     z_ = mtx[2][0];
// }