libsimple_motion/unit_tests/symphony.cpp

#include <cassert>
#include <chrono>
#include "simple/motion/movement.hpp"
#include "simple/motion/symphony.hpp"

using namespace std::literals;
using duration = std::chrono::duration<float>;
using simple::support::way;
using simple::motion::symphony;


template <typename T> class show_type;

void SharpTurn()
{
	// TODO: use rational, can't rely on float
	using movement = simple::motion::movement<duration,float,float>;
	std::array<float,2> block{0,0};

	auto sharp_turn = symphony(std::vector<movement>{
		movement{1250ms, 0,1},
		movement{1250ms, 0,1}
	});


	auto range = sharp_turn.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 0.5f / 1.25f) );
	assert( block[1] == 0.f );
	assert( range.end() - range.begin() == 1 );

	range = sharp_turn.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f / 1.25f) );
	assert( block[1] == 0.f );
	assert( range.end() - range.begin() == 1 );

	range = sharp_turn.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,1.f, 0.25f / 1.25f) );
	assert( range.end() - range.begin() == 2 );

	range = sharp_turn.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,1.f, 0.75f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

}

void Advance()
{
	using movement = simple::motion::movement<std::chrono::milliseconds,float,float>;

	auto sequence = symphony(std::vector{
		movement{10ms, 0,1},
		movement{10ms, 0,1},
		movement{10ms, 0,1},
		movement{10ms, 0,1}
	});

	const auto begin = sequence.range().begin();

	auto result = sequence.advance(10ms);
	assert( not result.done );
	assert( result.updated.lower() == begin+0 );
	assert( result.updated.upper() == begin+1 );
	assert( result.remaining == 0ms );

	result = sequence.advance(10ms);
	assert( not result.done );
	assert( result.updated.lower() == begin+1 );
	assert( result.updated.upper() == begin+2 );
	assert( result.remaining == 0ms );

	result = sequence.advance(10ms);
	assert( not result.done );
	assert( result.updated.lower() == begin+2 );
	assert( result.updated.upper() == begin+3 );
	assert( result.remaining == 0ms );

	result = sequence.advance(10ms);
	assert( result.done );
	assert( result.updated.lower() == begin+3 );
	assert( result.updated.upper() == begin+4 );
	assert( result.remaining == 0ms );

	sequence.reset();

	result = sequence.advance(15ms);
	assert( not result.done );
	assert( result.updated.lower() == begin+0 );
	assert( result.updated.upper() == begin+2 );
	assert( result.remaining == 0ms );

	result = sequence.advance(15ms);
	assert( not result.done );
	assert( result.updated.lower() == begin+1 );
	assert( result.updated.upper() == begin+3 );
	assert( result.remaining == 0ms );

	result = sequence.advance(15ms);
	assert( result.done );
	assert( result.updated.lower() == begin+3 );
	assert( result.updated.upper() == begin+4 );
	assert( result.remaining == 5ms );

}

void MoveWithSmallerTarget()
{
	using movement = simple::motion::movement<duration,float,float>;
	std::array<float,2> block{0,0};

	auto s = symphony(std::vector<movement>{
		movement{1250ms, 0,1},
		movement{1250ms, 0,1},
		movement{1250ms, 0,2},
		movement{1250ms, 0,2},
		movement{1250ms, 0,3},
		movement{1250ms, 0,3},
		movement{1250ms, 0,4},
		movement{1250ms, 0,4}
	});


	auto range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 0.5f / 1.25f) );
	assert( block[1] == 0.f );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f / 1.25f) );
	assert( block[1] == 0.f );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,1.f, 0.25f / 1.25f) );
	assert( range.end() - range.begin() == 2 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,1.f, 0.75f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,1.f, 1.f) );
	assert( range.end() - range.begin() == 1 );

	// and again

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) ); // <- this block is not set
	assert( block[1] == way(0.f,2.f, 0.5f / 1.25f) ); // <- only the last block is set
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,2.f, 1.f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,2.f, 1.f) ); // <- untill we get an update of size 2, then first block gets set to the earlier update, in general last N blocks are set by last N updates
	assert( block[1] == way(0.f,2.f, 0.25f / 1.25f) );
	assert( range.end() - range.begin() == 2 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,2.f, 1.f) );
	assert( block[1] == way(0.f,2.f, 0.75f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,2.f, 1.f) );
	assert( block[1] == way(0.f,2.f, 1.f) );
	assert( range.end() - range.begin() == 1 );

	// once more for good measure

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,2.f, 1.f) );
	assert( block[1] == way(0.f,3.f, 0.5f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,2.f, 1.f) );
	assert( block[1] == way(0.f,3.f, 1.f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,3.f, 1.f) ); // <- untill we get an update of size 2, then first block gets set to the earlier update, in general last N blocks are set by last N updates
	assert( block[1] == way(0.f,3.f, 0.25f / 1.25f) );
	assert( range.end() - range.begin() == 2 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,3.f, 1.f) );
	assert( block[1] == way(0.f,3.f, 0.75f / 1.25f) );
	assert( range.end() - range.begin() == 1 );

	range = s.move(block, 500ms).updated;
	assert( block[0] == way(0.f,3.f, 1.f) );
	assert( block[1] == way(0.f,3.f, 1.f) );
	assert( range.end() - range.begin() == 1 );
}

struct set_order
{
	static int order;
	int o = 0;
	set_order& operator=(float) { o = order++; return *this; }
};
int set_order::order = 0;

void MoveUpdateOrder()
{
	using movement = simple::motion::movement<duration,float,float>;
	std::array<set_order,3> block{};

	auto s = symphony(std::vector<movement>{
		movement{1250ms, 0,1},
		movement{1250ms, 0,1},
		movement{1250ms, 0,1},
		movement{1250ms, 0,1},
		movement{1250ms, 0,1},
		movement{1250ms, 0,1}
	});


	auto range = s.move(block, 3s).updated;
	assert( block[0].o == 0 );
	assert( block[1].o == 1 );
	assert( block[2].o == 2 );
	assert( range.end() - range.begin() == 3 );

	range = s.move(block, 3s).updated;
	assert( block[0].o == 3 );
	assert( block[1].o == 4 );
	assert( block[2].o == 5 );
	assert( range.end() - range.begin() == 3 );

	range = s.move(block, 3s).updated;
	assert( block[0].o == 3 ); // <- not set
	assert( block[1].o == 6 );
	assert( block[2].o == 7 );
	assert( range.end() - range.begin() == 2 );

}

void MoreUpdatesThanValues()
{
	using movement = simple::motion::movement<duration,float,float>;
	std::array<float,2> block{0,0};

	auto s = symphony(std::vector<movement>{
		movement{1250ms, 0,1}, // <- ignored
		movement{1250ms, 0,1}, // <- sets block[0]
		movement{1250ms, 0,2}, // <- sets block[1]
	});


	auto range = s.move(block, 3s).updated;
	assert( block[0] == way(0.f,1.f, 1.f) );
	assert( block[1] == way(0.f,2.f, 0.5f / 1.25f) );
	assert( range.end() - range.begin() == 3 );
}

int main()
{
	SharpTurn();
	Advance();
	MoveWithSmallerTarget();
	MoveUpdateOrder();
	MoreUpdatesThanValues();
	return 0;
}