#include "robomaster.h"

#include <stdio.h>

int main(int argc, char* argv[])
{
    Client client = client_new();
    client_connect(client);

    union Message resp;
    poll_message(client, &resp);
    if(resp.header.cmdid != SET_SDK_CONNECTION_CMDID || resp.resp.sdkconn.retcode) {
        fprintf(stderr, "Could not set SDK connection\n");
        return 1;
    }

    set_sdk_mode(client, true);
    //poll_message(client, &resp);
    //if(resp.header.cmdid == SET_SDK_MODE_CMDID || resp.resp.sdkmode.retcode) {
    //    fprintf(stderr, "Could not set SDK mode\n");
    //    return 1;
    //}

    set_system_led (
    		client,
    		0xFF,
    		0x00,
    		0xFF,
    		LEDCOMP_ALL,
    		0xFFFF,
    		LEDEFFECT_ON,
    		100,
    		100 );
    //poll_message(client, &resp);
    //if(resp.header.cmdid == SET_SYSTEM_LED_CMDID || resp.resp.led.retcode) {
    //    fprintf(stderr, "Could not set LED color\n");
    //    return 1;
    //}

    return 0;
}