#include <iostream>
#include "dji_vehicle.hpp"
using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

int
main()
{
  uint32_t app_id  = 1026762;
  char     enc_key[] = "1b838efe0d1b733e9015c750b2250fd61eb75dcbc3b309bf16813bd57294ed36";
  char     device_name[] = "/dev/ttyUSB0";
  uint32_t baud_rate = 921600;
  Vehicle* vehicle   = new Vehicle(device_name, baud_rate, true, false);

  // Check if the communication is working fine
  if (!vehicle->protocolLayer->getDriver()->getDeviceStatus())
  {
    std::cout << "Comms appear to be incorrectly set up. Exiting.\n";
    delete vehicle;
    return -1;
  }

  // Activate the drone
  DJI::OSDK::Vehicle::ActivateData activateData;
  activateData.version = vehicle->getFwVersion();
  activateData.ID = app_id;
  activateData.encKey = enc_key;

  ACK::ErrorCode ack = vehicle->activate(&activateData, 1);

  if (ACK::getError(ack))
  {
    ACK::getErrorCodeMessage(ack, __func__);
    delete vehicle;
    return -1;
  }

  uint8_t a = 128;
  vehicle->moc->sendDataToMSDK(&a, 1);
  ACK::ErrorCode takeoffStatus = vehicle->control->takeoff(1);
  if (ACK::getError(takeoffStatus) != ACK::SUCCESS)
  {
    ACK::getErrorCodeMessage(takeoffStatus, __func__);
    delete vehicle;
    return -1;
  }

  a++;
  vehicle->moc->sendDataToMSDK(&a, 1);

  sleep(20);

  ACK::ErrorCode landStatus = vehicle->control->land(1);
  if (ACK::getError(landStatus) != ACK::SUCCESS)
  {
    ACK::getErrorCodeMessage(landStatus, __func__);
    delete vehicle;
    return -1;
  }
  a++;
  vehicle->moc->sendDataToMSDK(&a, 1);




  delete vehicle;
  return 0;
}