4 files changed, 223 insertions, 0 deletions
diff --git a/modules/core/c++/math/math.hpp b/modules/core/c++/math/math.hpp
new file mode 100644
index 0000000..3920bec
--- /dev/null
+++ b/modules/core/c++/math/math.hpp
@@ -0,0 +1,4 @@
+#pragma once
+
+#include "n_linear.hpp"
+#include "n_closest.hpp"
diff --git a/modules/core/c++/math/n_closest.hpp b/modules/core/c++/math/n_closest.hpp
new file mode 100644
index 0000000..ac0fe2f
--- /dev/null
+++ b/modules/core/c++/math/n_closest.hpp
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "../common.hpp"
+#include "../iterator.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename FieldSchema, typename Encode, typename T, uint64_t D>
+saw::data<typename FieldSchema::StoredValueSchema> n_closest_read(const saw::data<sch::Ptr<FieldSchema>,Encode>& f, const saw::data<sch::Vector<T,D>>& frac_ind){
+
+	auto shift_frac_ind = frac_ind;
+	for(uint64_t i{0u}; i < D; ++i){
+		
+		shift_frac_ind.at({{i}}) = shift_frac_ind.at({{i}})  + saw::data<T>{0.5};
+		if(shift_frac_ind.at({{i}}).get() < 0){
+			shift_frac_ind.at({{i}}) = {};
+		}
+	}
+
+	saw::data<sch::FixedArray<sch::UInt64,D>> shift_ind;
+	for(uint64_t i{0u}; i < D; ++i){
+		shift_ind.at({i}) = frac_ind.at({{i}}).template cast_to<sch::UInt64>();
+	}
+
+	return f.at(shift_ind);
+}
+
+template<typename FieldSchema, typename Encode, typename T, uint64_t D>
+void n_closest_add(const saw::data<sch::Ptr<FieldSchema>,Encode>& f, const saw::data<sch::Vector<T,D>>& frac_ind, const saw::data<typename FieldSchema::StoredValueSchema>& val){
+	auto shift_frac_ind = frac_ind;
+	for(uint64_t i{0u}; i < D; ++i){
+		
+		shift_frac_ind.at({{i}}) = shift_frac_ind.at({{i}})  + saw::data<T>{0.5};
+		if(shift_frac_ind.at({{i}}).get() < 0){
+			shift_frac_ind.at({{i}}) = {};
+		}
+	}
+
+	auto f_meta = f.meta();
+	saw::data<sch::FixedArray<sch::UInt64,D>> shift_ind;
+	for(uint64_t i{0u}; i < D; ++i){
+		shift_ind.at({i}) = frac_ind.at({{i}}).template cast_to<sch::UInt64>();
+		if(shift_ind.at({i}) < f_meta.at({i})){
+			shift_ind.at({i}) = f_meta.at({i}) - 1u;
+		}
+	}
+	auto& f_i = f.at(shift_ind);
+
+	f_i = f_i + val;
+}
+
+}
+}
diff --git a/modules/core/c++/math/n_linear.hpp b/modules/core/c++/math/n_linear.hpp
new file mode 100644
index 0000000..b378440
--- /dev/null
+++ b/modules/core/c++/math/n_linear.hpp
@@ -0,0 +1,139 @@
+#pragma once
+
+#include "../common.hpp"
+#include "../iterator.hpp"
+
+namespace kel {
+namespace lbm {
+namespace impl {
+template<typename FieldSchema, typename T, uint64_t D>
+struct n_linear_interpolate_helper final {
+	template<uint64_t i = 0u>
+	auto apply(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
+		return pos;
+	}
+};
+}
+
+template<typename T, uint64_t D>
+saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> position_to_index_and_fraction(const saw::data<sch::Vector<T,D>>& pos){
+	saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> sep;
+
+	auto& ind = sep.template get<0u>();
+	auto& frac = sep.template get<1u>();
+
+	auto pos_cpy = pos;
+	// Guarantee that the pos is at least 0
+	for(uint64_t i = 0u; i < D; ++i){
+		pos_cpy.at({{i}}).set(std::max(pos.at({{i}}).get(), static_cast<typename saw::native_data_type<T>::type>(0)));
+	}
+
+	// Now we can cast to uint64_t
+	for(uint64_t i = 0u; i < D; ++i){
+		ind.at({{i}}) = pos_cpy.at({{i}}).template cast_to<sch::UInt64>();
+	}
+
+	frac = pos_cpy - ind.template cast_to<T>();
+
+	return sep;
+}
+
+template<typename T, uint64_t D>
+auto floor_index_from_position(const saw::data<sch::Vector<T,D>>& pos){
+	return position_to_index_and_fraction(pos).template get<0u>();
+}
+
+template<typename T, uint64_t D>
+saw::data<sch::Tuple<sch::Vector<sch::UInt64,D>,sch::Vector<T,D>>> position_to_index_and_fraction_bounded(
+	const saw::data<sch::Vector<T,D>>& pos,
+	const saw::data<sch::Vector<sch::UInt64,D>>& bound)
+{
+	auto infr = position_to_index_and_fraction(pos);
+	auto& ind = infr.template get<0u>();
+	auto& fra = infr.template get<1u>();
+	for(uint64_t i = 0u; i < D; ++i){
+		// If index is higher than bound. Set to bound and reset fraction
+		if((ind.at({{i}}).get()+1u) >= bound.at({{i}}).get()){
+			ind.at({{i}}).set(bound.at({{i}}).get()-1u);
+			fra.at({{i}}) = {};
+		}
+	}
+	return infr;
+}
+
+
+template<typename FieldSchema, typename T, uint64_t D>
+auto n_linear_interpolate(
+		const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
+
+	// Pos
+	auto pos_bound = pos;
+	
+	// Dimensions
+	auto meta = field.dims();
+
+	// Lower Index
+	saw::data<sch::FixedArray<sch::UInt64,D>> ind;
+
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		// Native Positive i
+		auto npos_i = pos.at({i}).get();
+
+		{
+			// Ok I want to stay in bounds
+			npos_i = std::min(npos_i,meta.at(i).get()-1.0);
+			npos_i = std::max(npos_i,1.0);
+		}
+
+		// Native Index i
+		auto nind_i = static_cast<uint64_t>(std::floor(npos_i))-1ul;
+
+		// Set index to i
+		ind.at(i).set(nind_i);
+	}
+	saw::data<sch::Vector<T,D>> pos_frac;
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		pos_frac.at({i}) = pos_bound.at({i}) - ind.at(i).template cast_to<T>();
+	}
+
+	// Base value
+	saw::data<typename FieldSchema::ValueType> res;
+
+	// constexpr uint64_t d_corners = 1ul << D;
+
+	saw::data<sch::FixedArray<sch::UInt64,D>> ones_ind;
+	for(saw::data<sch::UInt64> i{0u}; i < saw::data<sch::UInt64>{D}; ++i){
+		ones_ind.at({i}).set(1u);
+	}
+
+	iterator<D>::apply([&](auto ind){
+		// Iterates over (0,0,0) to (1,1,1)
+		saw::data<T> weight{1.0};
+
+		for(saw::data<sch::UInt64> d{0u}; d < saw::data<sch::UInt64>{D}; ++d){
+
+			saw::data<T> t = pos_frac.at({d});
+
+			if(ind.at(d).get() == 0u){
+				weight = weight * (saw::data<T>{1} - t);
+			}else{
+				weight = weight * t;
+			}
+		}
+
+	}, {}, ones_ind);
+
+	/// TODO I need to actually calc stuff
+	return field.at({});
+}
+
+template<typename FieldSchema, typename T>
+saw::data<sch::Vector<T,2u>> bilinear_interpolate(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,2u>>& pos){
+	saw::data<sch::Vector<T,2u>> res;
+
+	{
+	}
+	return {};
+}
+}
+}
diff --git a/modules/core/c++/math/round.hpp b/modules/core/c++/math/round.hpp
new file mode 100644
index 0000000..d3a2586
--- /dev/null
+++ b/modules/core/c++/math/round.hpp
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "../common.hpp"
+
+namespace kel {
+namespace lbm {
+
+template<typename T, uint64_t D>
+saw::data<sch::FixedArray<sch::UInt64,D>> round_to_unsigned(const saw::data<sch::Vector<T,D>>& inp){
+	saw::data<sch::FixedArray<sch::UInt64,D>> rv;
+				
+	auto zero = static_cast<saw::native_data_type<T>::type>(0);
+	auto half = static_cast<saw::native_data_type<T>::type>(0.5);
+
+	for(uint64_t i{0u}; i < D; ++i){
+		auto val = inp.at({{i}}).get()+half;
+		val = std::max(zero,val);
+
+		rv.at({i}).set(static_cast<uint64_t>(val));
+	}
+
+	return rv;
+}
+
+}
+}