TEST(Servo, AllTurnedOnSamePosition)
{
	SysClock::SetClockFreq(24000000);
	Servo<Pins, ServoTestTimer> servo;

	int expectedPauseDivider = 7;
	
	servo.UpdateTimings();
	
	servo.SetPosition(0, 1);
	servo.SetPosition(1, 1);
	servo.SetPosition(2, 1);
	servo.SetPosition(3, 1);
	
	servo.TimerHandler();
	EXPECT_EQ(expectedPauseDivider, ServoTestTimer::timerData.CurrentDivider);
	EXPECT_NE(0, ServoTestTimer::timerData.Counter);
	EXPECT_EQ(Pins::Read(), 0);
	
	servo.TimerHandler();
	EXPECT_EQ(1, ServoTestTimer::timerData.CurrentDivider);
	EXPECT_NE(0, ServoTestTimer::timerData.Counter);
	EXPECT_EQ(Pins::Read(), 15);
	
	servo.TimerHandler();
	EXPECT_EQ(expectedPauseDivider, ServoTestTimer::timerData.CurrentDivider);
	EXPECT_NE(0, ServoTestTimer::timerData.Counter);
	EXPECT_EQ(Pins::Read(), 0);
}