#pragma once

#include "schema_math.hpp"

#include <cmath>

namespace saw {
template<typename T>
class data<schema::Quarternion<T>,encode::Native> {
public:
	using Schema = schema::Quaternion<T>;
private:
	data<schema::FixedArray<T,4u>,encode::Native> vals_;

	using NativeType = typename native_data_type<T>::type;
public:
	data() = default;

	data(data<T,encode::Native> i__, data<T,encode::Native> j__, data<T,encode::Native> k__, data<T,encode::Native> w__):
		vals_{i__,j__,k__,w__}
	{}

	constexpr void normalize() {
		data<T,encode::Native> norm{};

		for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){
			norm += vals_.at(i) * vals_.at(i);
		}

		norm.set(std::sqrt(norm.get()));

		for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){
			vals_.at(i) = vals_.at(i) / norm;
		}
	}

	constexpr data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i){
		return vals_.at(i);
	}

	constexpr const data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i) const {
		return vals_.at(i);
	}

	constexpr data<Schema, encode::Native> operator*(const data<schema::Quaternion,encode::Native>& rhs) const {
		data<Schema,encode::Native> mult;

		data<schema::UInt64,encode::Native> i_i{0u};
		data<schema::UInt64,encode::Native> j_i{1u};
		data<schema::UInt64,encode::Native> k_i{2u};
		data<schema::UInt64,encode::Native> w_i{3u};

		mult.at(i_i) = at(w_i) * rhs.at(i_i) + at(i_i) * rhs.at(w_i) + at(j_i) * rhs.at(k_i) - at(k_i) * rhs.at(j_i);
		
		mult.at(j_i) = at(w_i) * rhs.at(j_i) + at(j_i) * rhs.at(w_i) - at(i_i) * rhs.at(k_i) + at(k_i) * rhs.at(i_i);
		
		mult.at(k_i) = at(w_i) * rhs.at(k_i) + at(k_i) * rhs.at(w_i) + at(i_i) * rhs.at(j_i) - at(j_i) * rhs.at(i_i);

		mult.at(w_i) = at(w_i) * rhs.at(w_i) - at(i_i) * rhs.at(i_i) - at(j_i) * rhs.at(j_i) - at(k_i) * rhs.at(k_i);

		return mult;
	}
};

}