#include <stdio.h>
#include <stdlib.h>
#include "Automation1.h"

int main(int argc, char* argv[])
{
    Automation1Controller controller = NULL;
    Automation1StatusConfig statusConfig = NULL;
    double results[1];
    double positionFeedback = 0.0;
    int32_t axisIndex = -1;
    const char* axisName = "U";  // the axis you want
    bool ok;

    // 1. Connect to controller
    //ok = Automation1_Connect(&controller);
    ok = Automation1_ConnectWithHost("129.129.118.96", &controller);
    if (!ok) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to connect: %d – %s\n", err, msg);
        return EXIT_FAILURE;
    }

    // 2. (Optional) Get axis index from axis name
    ok = Automation1_Controller_GetAxisIndexFromAxisName(controller, axisName, &axisIndex);
    if (!ok || axisIndex < 0) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to get axis index for \"%s\": %d – %s\n", axisName, err, msg);
        Automation1_Disconnect(controller);
        return EXIT_FAILURE;
    }

    // 3. Create status configuration
    ok = Automation1_StatusConfig_Create(&statusConfig);
    if (!ok) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to create status config: %d – %s\n", err, msg);
        Automation1_Disconnect(controller);
        return EXIT_FAILURE;
    }

    // 4. Add axis status item for feedback position
    ok = Automation1_StatusConfig_AddAxisStatusItem(
        statusConfig,
        axisIndex,
        Automation1AxisStatusItem_PositionFeedback,
        0
    );
    if (!ok) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to add axis status item: %d – %s\n", err, msg);
        Automation1_StatusConfig_Destroy(statusConfig);
        Automation1_Disconnect(controller);
        return EXIT_FAILURE;
    }

    // 5. Get the results
    ok = Automation1_Status_GetResults(controller, statusConfig, results, sizeof(results) / sizeof(results[0]));
    if (!ok) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to get status results: %d – %s\n", err, msg);
        Automation1_StatusConfig_Destroy(statusConfig);
        Automation1_Disconnect(controller);
        return EXIT_FAILURE;
    }

    // 6. Extract the position feedback value
    ok = Automation1_Status_GetAxisResult(
        statusConfig,
        results,
        sizeof(results) / sizeof(results[0]),
        axisIndex,
        Automation1AxisStatusItem_PositionFeedback,
        0,
        &positionFeedback
    );
    if (!ok) {
        int32_t err = Automation1_GetLastError();
        char msg[2048];
        Automation1_GetLastErrorMessage(msg, sizeof(msg));
        fprintf(stderr, "Failed to get axis result: %d – %s\n", err, msg);
    } else {
        printf("Axis \"%s\" (index %d) current position feedback: %f\n", axisName, axisIndex, positionFeedback);
    }

    // 7. Clean up
    Automation1_StatusConfig_Destroy(statusConfig);
    Automation1_Disconnect(controller);

    return ok ? EXIT_SUCCESS : EXIT_FAILURE;
}