#include "robomaster.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

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

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

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

    subnode_reset(client);
    poll_message(client, &msg);
    if(msg.header.cmdid != SUBNODE_RESET_CMDID || msg.resp.subnodereset.retcode) {
        fprintf(stderr, "Could not reset subnode subscription\n");
        return 1;
    }

    subscribe_add_node(client);
    poll_message(client, &msg);
    if(msg.header.cmdid != SUBSCRIBE_ADD_NODE_CMDID || (msg.resp.subnodeadd.retcode && msg.resp.subnodeadd.retcode != 0x50)) {
        fprintf(stderr, "Could not subscribe node\n");
        return 1;
    }

    set_robot_mode(client, MOVEMENTMODE_FREE);
    poll_message(client, &msg);
    if(msg.header.cmdid != SET_ROBOT_MODE_CMDID || msg.resp.mvmode.retcode) {
        fprintf(stderr, "Could not set move mode\n");
        return 1;
    }

    sdk_heartbeat(client);
    //poll_message(client, &msg);
    //if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
    //    fprintf(stderr, "Did not receive heartbeat\n");
    //    return 1;
    //}

    int c;
    while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
        switch(c) {
            case 'l':
                set_system_led (
                		client,
                		strtol(argv[optind + 0], NULL, 0),
                		strtol(argv[optind + 1], NULL, 0),
                		strtol(argv[optind + 2], NULL, 0),
                		LEDCOMP_ALL,
                		0xFFFF,
                		LEDEFFECT_ON,
                		100,
                		100 );
                poll_message(client, &msg);
                if(msg.header.cmdid != SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
                    fprintf(stderr, "Could not set LED color\n");
                    return 1;
                }
		break;
            case 's':
                chassis_speed_mode (
                		client,
                		atof(argv[optind + 0]),
                		atof(argv[optind + 1]),
                		atof(argv[optind + 2]) );
		break;
            case '?':
            default:
		fprintf(stderr, "Unknown argument %c\n", optopt);
            case 'h':
		printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]");
		return 0;
	}
    }

    return 0;
}