Updates router connect and restricted peer logic

[howardwu]

Motivation

This PR updates the Router connect and insert_restricted_peer logic.

Tests

This PR introduces a test framework for Router by implementing a TestRouter. Currently, only the Handshake and Disconnect capabilities are needed in testing, however the Reading and Writing traits are implemented with Inbound, Outbound, and Heartbeat traits as stubs to support extensibility in the future.