#include <stdio.h>
#include <bta.h>

static void BTA_CALLCONV infoEvent(BTA_EventId eventId, int8_t *msg) {
    printf("   Callback: infoEvent (%d) %s\n", eventId, msg);
}

/*static void BTA_CALLCONV frameArrived(BTA_Frame *frame) {
    printf("   Callback: frameArrived FrameCounter %d\n", frame->frameCounter);
    BTA_Status status;
    uint16_t *distances;
    BTA_DataFormat dataFormat;
    BTA_Unit unit;
    uint16_t xRes, yRes;
    status = BTAgetDistances(frame, (void **)&distances, &dataFormat, &unit, &xRes, &yRes);
    if (status == BTA_StatusOk) {
        if (dataFormat == BTA_DataFormatUInt16) {
            if (unit == BTA_UnitMillimeter) {
                printf("    Callback: frameArrived Got distance data\n");
                // as expected -> process amplitude data: amplitudes[i]
            }
        }
    }
}*/


int main() {
    BTA_Config config;
    printf("BTAinitConfig()\n");
    BTAinitConfig(&config);
    config.frameMode = BTA_FrameModeXYZ;
    config.infoEvent = &infoEvent;
    config.verbosity = 5;
    //config.frameQueueMode = BTA_QueueModeDropOldest;
    //config.frameQueueLength = 1;

    
    BTA_Status status;
    BTA_Handle btaHandle;
    printf("BTAopen()\n");
    status = BTAopen(&config, &btaHandle);
    if (status != BTA_StatusOk) {
        return -1;
    }
    
    BTA_DeviceInfo *deviceInfo;
    printf("BTAgetDeviceInfo()\n");
    status = BTAgetDeviceInfo(btaHandle, &deviceInfo);
    if (status != BTA_StatusOk) {
        return -1;
    }
    printf("Device type: 0x%x\n", deviceInfo->deviceType);
    printf("BTAfreeDeviceInfo()\n");
    BTAfreeDeviceInfo(deviceInfo);
    
    printf("Service running: %d\n", BTAisRunning(btaHandle));
    printf("Connection up: %d\n", BTAisConnected(btaHandle));
    
    uint32_t usValue;
    printf("BTAreadRegister()\n");
    status = BTAreadRegister(btaHandle, 0x81, &usValue, 0);
    if (status != BTA_StatusOk) {
        return -1;
    }
    
    printf("BTAwriteRegister()\n");
    status = BTAwriteRegister(btaHandle, 0x81, &usValue, 0);
    if (status != BTA_StatusOk) {
        return -1;
    }
    
    // Actively get a frame
    BTA_Frame *frame;
    printf("BTAgetFrame()\n");
    status = BTAgetFrame(btaHandle, &frame, 3000);
    if (status != BTA_StatusOk) {
        return -1;
    }
    
    void *xCoordinates, *yCoordinates, *zCoordinates;
    BTA_DataFormat dataFormat;
    BTA_Unit unit;
    uint16_t xRes, yRes;
    printf("BTAgetXYZcoordinates()\n");
    status = BTAgetXYZcoordinates(frame, &xCoordinates, &yCoordinates, &zCoordinates, &dataFormat, &unit, &xRes, &yRes);
    if (status == BTA_StatusOk) {
        if (dataFormat == BTA_DataFormatFloat32) {
            if (unit == BTA_UnitMeter) {
                printf("Got 3D data\n");
                // as expected -> process point in space: ( xCoordinates[i], yCoordinates[i], zCoordinates[i] )
            }
        }
    }
    printf("BTAfreeFrame()\n");
    BTAfreeFrame(&frame);
    printf("BTAclose()\n");
    BTAclose(&btaHandle);
    printf("Hit <Return> to end the example\n");
    fgetc(stdin);
}
