#pragma once

#include "serialCommand.h"
#include "serialPort.h"

#include <cstdint>

struct GPSData
{
    float latitude;  // Decimal degrees
    float longitude; // Decimal degrees
    float altitude;  // Meters
};

struct CameraData
{
    uint16_t height; // Pixels
    uint16_t width;  // Pixels
    float pitch;     // Degrees
    float yaw;       // Degrees
    float fow;       // Degrees
};

struct DroneData
{
    GPSData gps;
    float yaw;   // Degrees
    float pitch; // Degrees
    float roll;  // Degrees
};

class UtilsTargetLocation
{
public:
    UtilsTargetLocation(SerialPort *serial, SerialCommand *command);
    GPSData getLocation(float altitude, float latitude, float lognitude, float yaw, float pitch, float roll, float targetTrueSize, uint16_t targetPixelSize);

private:
    CameraData getCameraData();
    float calculateTargetDistance(float targetSize, uint16_t targetPixelSize, uint16_t imageWidth, float fov);
    float degreesToRadians(float degrees);
    GPSData calculateTargetLocation(DroneData drone, CameraData camera, float distance, float bearing);
    void captureImageFromStream();

    SerialPort *mSerial;
    SerialCommand *mCommand;
};