2 files changed, 97 insertions, 5 deletions

diff --git a/lib/core/c++/math/n_linear.hpp b/lib/core/c++/math/n_linear.hpp
index 4f47965..bb7653c 100644
--- a/lib/core/c++/math/n_linear.hpp
+++ b/lib/core/c++/math/n_linear.hpp
@@ -1,12 +1,51 @@
 #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 - 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 FieldSchema, typename T, uint64_t D>
-auto n_linear_interpolate(const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
+auto n_linear_interpolate(
+		const saw::data<FieldSchema>& field, const saw::data<sch::Vector<T,D>>& pos){
 
 	// Pos
 	auto pos_bound = pos;
@@ -39,9 +78,40 @@ auto n_linear_interpolate(const saw::data<FieldSchema>& field, const saw::data<s
 	}
 
 	// Base value
-	auto res = field.at(ind);
+	saw::data<typename FieldSchema::ValueType> res;
 
-	constexpr uint64_t d_corners = 1ull << D;
+	// 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);
+}
+
+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/lib/core/tests/math.cpp b/lib/core/tests/math.cpp
index 738d637..7c9dc14 100644
--- a/lib/core/tests/math.cpp
+++ b/lib/core/tests/math.cpp
@@ -1,5 +1,6 @@
 #include <forstio/test/suite.hpp>
 
+#include <iostream>
 #include "../c++/math/n_linear.hpp"
 
 namespace {
@@ -7,13 +8,34 @@ namespace sch {
 using namespace saw::schema;
 }
 
-SAW_TEST("Math 2-Linear"){
+SAW_TEST("Math 1-Linear"){
 	using namespace kel;
+	
+	saw::data<sch::FixedArray<sch::Vector<sch::Float64,1u>,2u>> field;
+	{
+		field.at({{0u}}).at({{0u}}).set(-1.0);
+		field.at({{1u}}).at({{0u}}).set(2.0);
+	}
+	saw::data<sch::Vector<sch::Float64,1u>> pos;
+	pos.at({{0u}}).set(0.3);
+}
 
-	saw::data<sch::FixedArray<sch::Vector<sch::Float64,2u>,32u,32u>> field;
+SAW_TEST("Math/Floor Index from Pos"){
+	using namespace kel;
 
 	saw::data<sch::Vector<sch::Float64,2u>> pos;
 
+	{
+		pos.at({{0u}}) = 43.999;
+		pos.at({{1u}}) = -50.0;
+	}
+
+	auto ind = lbm::floor_index_from_position(pos);
+	for(uint64_t i = 0u; i < 2u; ++i){
+		std::cout<<ind.at({{i}}).get()<<" ";
+	}
+	std::cout<<std::endl;
+
 	SAW_EXPECT(true, "Default true check");
 }