#include <locale.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <wchar.h>

#if defined(__APPLE__) && !defined(NOFRAMEWORK)
#include <libximc/ximc.h>
#else
#include <ximc.h>
#endif

#ifndef _WIN32
#include <syslog.h>
#else
#include <io.h>
#include <fcntl.h>
#endif

void print_state(status_t* state)
{
    wprintf(L"Status:\n");
    wprintf(L" rpm: %d,", state->CurSpeed);
    wprintf(L" pos: %d,", state->CurPosition);
    wprintf(L" upwr: %d,", state->Upwr);
    wprintf(L" ipwr: %d,", state->Ipwr);
    wprintf(L" flags: %x,", state->Flags);
    wprintf(L" mvsts: %x", state->MvCmdSts);
    if (state->Flags & STATE_ALARM) {
        wprintf(L", ALARM");
    }

    if (state->Flags & STATE_ERRC) {
        wprintf(L", ERRC");
    }

    if (state->Flags & STATE_ERRD) {
        wprintf(L", ERRD");
    }

    wprintf(L"\n");
}

const wchar_t* error_string(result_t result)
{
    switch (result) {
    case result_error:				return L"error";
    case result_not_implemented:	return L"not implemented";
    case result_nodevice:			return L"no device";
    default:						return L"success";
    }
}

void get_status_and_print(device_t device)
{
    result_t result;
    status_t status;
    if ((result = get_status(device, &status)) != result_ok) {
        wprintf(L"Error getting status: %ls\n", error_string(result));
    }
    else {
        print_state(&status);
    }
}

void print_state_calb(status_calb_t* state)
{
    wprintf(L"Status calb:\n");
    wprintf(L" rpm: %f,", state->CurSpeed);
    wprintf(L" pos: %f,", state->CurPosition);
    wprintf(L" upwr: %d,", state->Upwr);
    wprintf(L" ipwr: %d,", state->Ipwr);
    wprintf(L" flags: %x,", state->Flags);
    wprintf(L" mvsts: %x", state->MvCmdSts);
    if (state->Flags & STATE_ALARM) {
        wprintf(L", ALARM");
    }

    if (state->Flags & STATE_ERRC) {
        wprintf(L", ERRC");
    }

    if (state->Flags & STATE_ERRD) {
        wprintf(L", ERRD");
    }

    wprintf(L"\n");
}

void print_edges(edges_settings_t* edges)
{
    wprintf(L"Edges settings:\n");
    wprintf(L" Lpos: %d,", edges->LeftBorder);
    wprintf(L" uLpos: %d,", edges->uLeftBorder);
    wprintf(L" Rpos: %d,", edges->RightBorder);
    wprintf(L" uRpos: %d", edges->uRightBorder);
    wprintf(L"\n");
}

void print_edges_calb(edges_settings_calb_t* edges)
{
    wprintf(L"User unit edges settings:\n");
    wprintf(L" Lpos: %f,", edges->LeftBorder);
    wprintf(L" Rpos: %f", edges->RightBorder);
    wprintf(L"\n");
}

const wchar_t* loglevel_string(int loglevel)
{
    switch (loglevel) {
        case LOGLEVEL_ERROR: 	return L"ERROR";
        case LOGLEVEL_WARNING:	return L"WARN";
        case LOGLEVEL_INFO:		return L"INFO";
        case LOGLEVEL_DEBUG:	return L"DEBUG";
        default:				return L"UNKNOWN";
    }
}

#ifndef _WIN32
int loglevel_to_sysloglevel(int loglevel)
{
    switch (loglevel) {
        case LOGLEVEL_ERROR: 	return LOG_ERR;
        case LOGLEVEL_WARNING:	return LOG_WARNING;
        case LOGLEVEL_INFO:		return LOG_INFO;
        case LOGLEVEL_DEBUG:	return LOG_DEBUG;
        default:				return LOG_INFO;
    }
}
#endif

static char* widestr_to_str(const wchar_t* str)
{
    char *result;
    mbstate_t mbs;
    size_t len;
    memset(&mbs, 0, sizeof(mbs));
    len = wcsrtombs(NULL, &str, 0, &mbs);
    if (len == (size_t)(-1))
        return NULL;

    result = malloc(sizeof(char)*(len + 1));
    if (result && wcsrtombs(result, &str, len+1, &mbs) != len) {
        free(result);
        return NULL;
    }

    return result;
}

void XIMC_CALLCONV my_logging_callback(int loglevel, const wchar_t* message, void* user_data)
{
    wchar_t wbuf[2048];
    char *abuf;
    (void)user_data;
    int used_loglevel = user_data ? *((int*)user_data) : LOGLEVEL_DEBUG;
    if (loglevel > used_loglevel)
        return;

    /* Print to console unicode chars */
    swprintf(wbuf, sizeof(wbuf) / sizeof(wbuf[0]) - 1, L"XIMC %ls: %ls", loglevel_string(loglevel), message);
    fwprintf(stderr, L"%ls\n", wbuf);

#ifdef _WIN32
    (void)abuf;
#else
    /* Print to syslog ANSI chars */
    abuf = widestr_to_str(wbuf);
    if (abuf) {
        syslog(loglevel_to_sysloglevel(loglevel), "%s", abuf);
        free(abuf);
    }
#endif
}

device_enumeration_t search_devices(int* device_count)
{
    wprintf(L"\nSearching for devices...\n");
    const int probe_flags = ENUMERATE_PROBE | ENUMERATE_NETWORK;
    const char* enumerate_hints = "addr=";
    device_enumeration_t devenum = enumerate_devices(probe_flags, enumerate_hints);
    if (!devenum) {
        wprintf(L"Error searching for devices\n");
        *device_count = 0;
        return devenum;
    }

    *device_count = get_device_count(devenum);
    if (*device_count < 1) {
        wprintf(L"No devices found\n");
        return devenum;
    }

    wprintf(L"Found %d devices\n", *device_count + 1);
    return devenum;
}

void print_device_list(device_enumeration_t devenum, int device_count)
{
    wprintf(L"\nFound devices list:\n");
	int i = 0;
    for (i = 0; i < device_count; ++i) {
        wprintf(L" #%d. %hs\n", i + 1, get_device_name(devenum, i));
    }
	wprintf(L" #%d. virtual\n", i + 1);
}

char* create_virtual_device_name()
{
    static char device_name[256];
    static char path[255];

#if defined(_WIN32) || defined(_WIN64)
    strcpy(path, getenv("USERPROFILE"));
    strcat(path, "\\virt_controller.bin");
    strcpy(device_name, "xi-emu:///");
    strcat(device_name, path);
#else
    strcpy(path, getenv("HOME"));
    strcat(path, "/virt_controller.bin");
    strcpy(device_name, "xi-emu:///");
    strcat(device_name, path);
#endif
    wprintf(L"Virtual controller file path: '%hs'\n", path);

    return device_name;
}

char* get_device_name_from_list(device_enumeration_t devenum, int device_count)
{
    char buffer[256];
    static char device_name[256];
    while (1) {
        print_device_list(devenum, device_count);
        wprintf(L"When using an Ethernet (TCP protocol) connection, multi-user interaction with the device is possible. In this case, the same device may be controlled simultaneously from multiple sources, which can potentially lead to control conflicts.\nEnter device number from the list:\n");
        fgets(buffer, sizeof(buffer), stdin);
        int number;
		if (sscanf(buffer, "%d", &number) != 1) {
			continue;
		}
		
		if (1 <= number && number <= device_count) {
            char* name = get_device_name(devenum, number - 1);
            strcpy(device_name, name);
            wprintf(L"Selected device: '%hs'\n", device_name);
            return device_name;
        }

		if (number == device_count + 1) {
			return create_virtual_device_name();
		}
    }
}

char* choose_device()
{
    int device_count = 0;
    device_enumeration_t devenum = search_devices(&device_count);
    char* device_name;
    if (device_count < 1) {
		wprintf(L"The real controller is not found or busy with another app.\nThe virtual controller is opening to check the operation of the library...\n");
        device_name = create_virtual_device_name();
    }
    else {
        device_name = get_device_name_from_list(devenum, device_count);
    }
    free_enumerate_devices(devenum);
    return device_name;
}

int warn_user_about_modifications()
{
    char buffer[256];
    while (1) {
        wprintf(L"\nThe script will modify the following controller settings:\n"
            L" engine advanced settings\n"
            L" correction table (will be cleared)\nContinue? [y/n]:\n");
        fgets(buffer, sizeof(buffer), stdin);
        char answer;
        if (sscanf(buffer, "%c", &answer) == 1) {
            if (answer == 'y' || answer == 'Y') {
                return 0;
            }

            if (answer == 'n' || answer == 'N') {
                return 1;
            }
        }
    }
}

void print_motor_type(device_t device)
{
    /*
    Reading motor parameters and output its type.
    MotorType = 0 motor not defined or not connected.
    MotorType = 1 step.
    MotorType = 2 DC.
    MotorType = 3 BLDC.
    */

    motor_settings_t motor_settings;
    result_t result;
    if ((result = get_motor_settings(device, &motor_settings)) != result_ok) {
        wprintf(L"Error getting motor settings: %ls\n", error_string(result));
    }
    else {
        char* motor_type;
        switch (motor_settings.MotorType) {
        case 1:
            motor_type = "step";
            break;
        case 2:
            motor_type = "DC";
            break;
        case 3:
            motor_type = "BLDC";
            break;
        default:
            motor_type = "motor not defined or not connected";
        }
        wprintf(L"Motor type: %d (%hs)\n", motor_settings.MotorType, motor_type);
    }
}

int main(int argc, char* argv[])
{
    /* Inherit system locale */
    setlocale(LC_ALL,"");
#ifdef _MSC_VER
    /* UTF-16 output on windows */
    _setmode(_fileno(stdout), _O_U16TEXT);
    _setmode(_fileno(stderr), _O_U16TEXT);
#endif

    int used_loglevel = getenv("XIMC_testapp_C_VERBOSE") ? LOGLEVEL_DEBUG : LOGLEVEL_WARNING;
    set_logging_callback(my_logging_callback, &used_loglevel);
    char ximc_version_str[32];
    ximc_version(ximc_version_str);
    wprintf(L"This is a ximc test program\nlibximc version %hs\n", ximc_version_str);

    if (warn_user_about_modifications()) {
        return 0;
    }

    char* device_name = choose_device();
    wprintf(L"\nOpening device...\n");
    device_t device = open_device(device_name);
    if (device == device_undefined) {
        wprintf(L"Error opening device\nPress Enter to exit...");
        char terminating_str[2];
        fgets(terminating_str, 2, stdin);
        return 1;
    }

    // Read device status
    get_status_and_print(device);
    
    // Read device information
    result_t result;
    device_information_t di;
    if ((result = get_device_information(device, &di)) != result_ok) {
        wprintf(L"Error getting device information: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Device information:\n manufacturer: %hs, id: %hs, product: %hs, version: %d.%d.%d\n",
            di.Manufacturer, di.ManufacturerId, di.ProductDescription, di.Major, di.Minor, di.Release);
    }

    // Send "zero" command to device
    if ((result = command_zero(device)) != result_ok) {
        wprintf(L"Error zeroing: %ls\n", error_string(result));
    }

    // Read device status
    get_status_and_print(device);

    // Read engine settings
    engine_settings_t engine_settings;
    if ((result = get_engine_settings(device, &engine_settings)) != result_ok) {
        wprintf(L"Error getting engine settings: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Engine settings:\n voltage: %d, current: %d, speed %d\n",
            engine_settings.NomVoltage, engine_settings.NomCurrent, engine_settings.NomSpeed);
    }

    // Read electromechanical settings
    emf_settings_t emf_settings;
    if ((result = get_emf_settings(device, &emf_settings)) != result_ok) {
        wprintf(L"Error getting electromechanical settings: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Electromechanical settings:\n Km: %f, L: %f, R: %f\n",
            emf_settings.Km, emf_settings.L, emf_settings.R);
    }

    // Set new engine advanced settings
    engine_advanced_setup_t eas_settings;
    eas_settings.stepcloseloop_Kw = 98;
    eas_settings.stepcloseloop_Kp_low = 99;
    eas_settings.stepcloseloop_Kp_high = 100;
    if ((result = set_engine_advanced_setup(device, &eas_settings)) != result_ok) {
        wprintf(L"Failed to apply engine advanced settings: %ls\n", error_string(result));
    }
    
    // Read engine advanced settings
    eas_settings.stepcloseloop_Kw = 0;
    eas_settings.stepcloseloop_Kp_high = 0;
    if ((result = get_engine_advanced_setup(device, &eas_settings)) != result_ok) {
        wprintf(L"Error getting engine advanced settings: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Engine advanced settings:\n 1 %d, 2 %d, 3 %d\n",
            eas_settings.stepcloseloop_Kw, eas_settings.stepcloseloop_Kp_low, eas_settings.stepcloseloop_Kp_high);
    }
    
    print_motor_type(device);
    
    // Read engine calb settings
    calibration_t calibration;
    calibration.A = 1;
    calibration.MicrostepMode = MICROSTEP_MODE_FULL;
    engine_settings_calb_t engine_settings_calb;
    if ((result = get_engine_settings_calb(device, &engine_settings_calb, &calibration)) != result_ok) {
        wprintf(L"Error getting engine calb settings: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Engine calb settings:\n voltage: %d, current: %d, speed: %f\n",
            engine_settings_calb.NomVoltage, engine_settings_calb.NomCurrent, engine_settings_calb.NomSpeed);
    }
    
    int sec = 2;
    wprintf(L"\nEngine rotating left for %d seconds...\n\n", sec);
    if ((result = command_left(device)) != result_ok) {
        wprintf(L"Error in command left: %ls\n", error_string(result));
    }
    else {
        msec_sleep(sec * 1000);
    }

    // Read device status
    get_status_and_print(device);

    // The device_t device parameter in this function is a C pointer, unlike most library functions that use this parameter
    char* ch1 = "Corr_table_example.tbl";
    if ((result = set_correction_table(device, ch1)) != result_ok) {
        wprintf(L"Error loading correction table '%hs': %ls\n", ch1, error_string(result));
    }

    result = command_move_calb(device, 380, &calibration);
    wprintf(L"command_move_calb result: %ls\n", error_string(result));

    if ((result = command_wait_for_stop(device, 100)) != result_ok) {
        wprintf(L"Error command_wait_for_stop: %ls\n", error_string(result));
    }

    // Read position in user units
    get_position_calb_t get_pos;
    if ((result = get_position_calb(device, &get_pos, &calibration)) != result_ok) {
        wprintf(L"Error getting position in user units: %ls\n", error_string(result));
    }
    else {
        wprintf(L"Position in user units:\n pos: %f\n", get_pos.Position);
    }

    // Read device calibrated status
    status_calb_t status_calb;
    if ((result = get_status_calb(device, &status_calb, &calibration)) != result_ok) {
        wprintf(L"Error getting calibrated state: %ls\n", error_string(result));
    }
    else {
        print_state_calb(&status_calb);
    }

    // Read user unit edges settings
    edges_settings_calb_t edges_settings_calb;
    if ((result = get_edges_settings_calb(device, &edges_settings_calb, &calibration)) != result_ok) {
        wprintf(L"Error getting user unit edges settings %ls\n", error_string(result));
    }
    else {
        print_edges_calb(&edges_settings_calb);
    }

    // Read edges settings
    edges_settings_t edges_settings;
    if ((result = get_edges_settings(device, &edges_settings)) != result_ok) {
        wprintf(L"Error getting edges settings: %ls\n", error_string(result));
    }
    else {
        print_edges(&edges_settings);
    }

    wprintf(L"\nStopping engine...\n\n");
    if ((result = command_stop(device)) != result_ok) {
        wprintf(L"Error in command stop: %ls\n", error_string(result));
    }

    // Read device status
    get_status_and_print(device);

    if ((result = set_correction_table(device, NULL)) != result_ok) {
        wprintf(L"Error clearing correction table: %ls", error_string(result));
    }
    
    // Read device calibrated status
    if ((result = get_status_calb(device, &status_calb, &calibration)) != result_ok) {
        wprintf(L"Error getting calibrated state: %ls\n", error_string(result));
    }
    else {
        print_state_calb(&status_calb);
    }

    wprintf(L"\nSleeping for %d seconds...\n\n", sec);
    msec_sleep(sec * 1000);

    // The id parameter device_t device this function is a C pointer, unlike most library functions that use this parameter
    if ((result = close_device(&device)) != result_ok) {
        wprintf(L"Error closing device: %ls\n", error_string(result));
    }

    wprintf(L"Done\nPress Enter to exit...");
    char terminating_str[2];
    fgets(terminating_str, 2, stdin);
    return 0;
}
