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

#include <ip_connection.h>
#include <brick_dc.h>
#include <bricklet_rotary_poti.h>
#include <bricklet_voltage_current.h>


RotaryPoti potiDevice;
VoltageCurrent powerDevice;
DC motorDevice;


void accelerateTo(int speed)
{
    int16_t v;
    if (speed <= -32767)
        v = -32767;
    else if (speed >= 32767)
        v = 32767;
    else
        v = speed;

    int rc = dc_set_velocity(&motorDevice, v);
    if (rc != E_OK)
        printf("dc_set_velocity returned %d", rc);
}


void potentiometerCallback(int16_t value, void *user_data)
{
    double f = 32767.0 / 150;
    f *= value;
    int speed = f;

    accelerateTo(speed);
    printf("Poti value: %hd -> speed = %d\n", value, speed);
}


void currentCallback(int32_t c, void *user_data)
{
    printf("current update: %d ...\n", c);
}


void powerCallback(int32_t power, void *user_data)
{
    printf("power update: %d ...\n", power);
}

/**
 * Power consumption callback.
 */
void voltageCallback(int32_t v, void *user_data)
{
    printf("voltage update: %d ...\n", v);
}


/**
 * Main program loop.
 */
int main(int argc, char **argv)
{
    static const char *host     = "tinkertest";
    static const char *uidPoti  = "cEQ";
    static const char *uidPower = "cTS";
    static const char *uidDC    = "6eSsEN";

    IPConnection ipcon;
    ipcon_create(&ipcon);

    if (ipcon_connect(&ipcon, host, 4223) < 0) {
        printf("Could not connect to brickd at %s:4223", host);
        return 1;
    }

    sleep(1);
    rotary_poti_create(&potiDevice, uidPoti, &ipcon);
    dc_create(&motorDevice, uidDC, &ipcon);
    voltage_current_create(&powerDevice, uidPower, &ipcon);

    rotary_poti_register_callback(&potiDevice, ROTARY_POTI_CALLBACK_POSITION, (void*)potentiometerCallback, NULL);
    rotary_poti_set_position_callback_period(&potiDevice, 200);

    voltage_current_register_callback(&powerDevice, VOLTAGE_CURRENT_CALLBACK_POWER, (void*)powerCallback, NULL);
    voltage_current_register_callback(&powerDevice, VOLTAGE_CURRENT_CALLBACK_CURRENT, (void*)currentCallback, NULL);
    voltage_current_register_callback(&powerDevice, VOLTAGE_CURRENT_CALLBACK_VOLTAGE, (void*)voltageCallback, NULL);
    voltage_current_set_power_callback_period(&powerDevice, 500);
    voltage_current_set_current_callback_period(&powerDevice, 500);
    voltage_current_set_voltage_callback_period(&powerDevice, 500);

    if (argc > 1)
        dc_set_acceleration(&motorDevice, 32767);
    else {
        dc_set_acceleration(&motorDevice, 0);
        puts("Acceleration is 0");
    }
    dc_set_velocity(&motorDevice, 0);
    dc_enable(&motorDevice);

    getchar();
    dc_disable(&motorDevice);
    ipcon_destroy(&ipcon);
    return 0;
}