/**  @file bta_eth_example.cpp
*    @version 1.0.0
*  
*    @brief This example shows a common use case of the SDK
*  
*    BLT_DISCLAIMER
*  
*    @author Alex Falkensteiner
*  
*    @cond svn
*  
*    Information of last commit
*    $Rev::               $:  Revision of last commit
*    $Author::            $:  Author of last commit
*    $Date::              $:  Date of last commit
*  
*    @endcond
*/

#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);
    uint8_t udpDataIpAddr[] = { 224, 0, 0, 1 };
    config.udpDataIpAddr = udpDataIpAddr;
    config.udpDataIpAddrLen = 4;
    config.udpDataPort = 10002;
    uint8_t tcpDeviceIpAddr[] = { 192, 168, 0, 10 };
    config.tcpDeviceIpAddr = tcpDeviceIpAddr;
    config.tcpDeviceIpAddrLen = 4;
    config.tcpControlPort = 10001;
    config.frameMode = BTA_FrameModeXYZAmp;
    config.infoEvent = &infoEvent;
    config.verbosity = 5;
    config.frameArrived = &frameArrived;
    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, 5, &usValue, 0);
    if (status != BTA_StatusOk) {
        return -1;
    }
    
    printf("BTAwriteRegister()\n");
    status = BTAwriteRegister(btaHandle, 5, &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;
    }
    
    uint16_t *amplitudes;
    BTA_DataFormat dataFormat;
    BTA_Unit unit;
    uint16_t xRes, yRes;
    printf("BTAgetAmplitudes()\n");
    status = BTAgetAmplitudes(frame, (void **)&amplitudes, &dataFormat, &unit, &xRes, &yRes);
    if (status == BTA_StatusOk) {
        if (dataFormat == BTA_DataFormatUInt16) {
            if (unit == BTA_UnitUnitLess) {
                printf("Got amplitude data\n");
                // as expected -> process amplitude data: amplitudes[i]
            }
        }
    }
    
    void *xCoordinates, *yCoordinates, *zCoordinates;
    printf("BTAgetXYZcoordinates()\n");
    status = BTAgetXYZcoordinates(frame, &xCoordinates, &yCoordinates, &zCoordinates, &dataFormat, &unit, &xRes, &yRes);
    if (status == BTA_StatusOk) {
        if (dataFormat == BTA_DataFormatSInt16) {
            if (unit == BTA_UnitMillimeter) {
                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);
}
