V2V and V2I Based Safety and Platooning Algorithms for Connected and Autonomous Vehicles