#include <OzIDManager.h>
#include <OzUltraSonicSensor.h>

// global pointers
OzIDManager* manager;
OzUltraSonicSensor* ultraSonicSensor_01;
OzUltraSonicSensor* ultraSonicSensor_02;
OzUltraSonicSensor* ultraSonicSensor_03;
OzUltraSonicSensor* ultraSonicSensor_04;

const int triggerPin_01 = 14;
const int echoPin_01 = 15;
const int triggerPin_02 = 4;
const int echoPin_02 = 2;
const int triggerPin_03 = 5;
const int echoPin_03 = 3;
const int triggerPin_04 = 8;
const int echoPin_04 = 9;

void setup()
{
  Serial.begin(115200);

  manager = new OzIDManager;
  manager->_sendACK = true;
  manager->_checksum = true;

  OzCommunication::setIDManager(manager);

  ultraSonicSensor_01 = new OzUltraSonicSensor(triggerPin_01, echoPin_01);
  ultraSonicSensor_02 = new OzUltraSonicSensor(triggerPin_02, echoPin_02);
  ultraSonicSensor_03 = new OzUltraSonicSensor(triggerPin_03, echoPin_03);
  ultraSonicSensor_04 = new OzUltraSonicSensor(triggerPin_04, echoPin_04);

  int x=1;
  manager->sendLinkSetup();
  manager->PrintWelcomeLine(ultraSonicSensor_01, x++, "Ultra_1");
  manager->PrintWelcomeLine(ultraSonicSensor_02, x++, "Ultra_2");
  manager->PrintWelcomeLine(ultraSonicSensor_03, x++, "Ultra_3");
  manager->PrintWelcomeLine(ultraSonicSensor_04, x++, "Ultra_4");

  Serial.println("c=welcome&type=OzMatrixUltraSonic&id=OZMULT");
}

void loop()
{
  ultraSonicSensor_01->ownLoop();
  ultraSonicSensor_02->ownLoop();
  ultraSonicSensor_03->ownLoop();
  ultraSonicSensor_04->ownLoop();

  OzCommunication::communicate();
}