#include "libexf1.h"

//  gcc -o ExF1Ctrl exf1ctrl.c libexf1.c  exf1api.c  -lusb 

/*  Command observations:
 *  Byte 1-4   = Number of bytes in packet.
 *  Byte 5-6   = Command idx. for broken up commands: 0x0001, 0x0002 ...
 *  Byte 7-8   = Command.
 *  Byte 9-12  = Command number: 0x00000000 - 0xFFFFFFFF.
 *  Byte 13-14 = Parameter 1.
 *  Byte 15-16 = Parameter 2.
 *  Byte 17-18 = Parameter 3.
 *  Byte 19-20 = Parameter 4.
 */

/*  Ack observations:
 *  Byte 1-4   = Number of bytes in packet.
 *  Byte 5-6   = Ack idx. Ends with 0x0003.
 *  Byte 7-8   = Ack. 0x2001 = OK, 0x0002 = More data on the way. 
 *  Byte 9-12  = Command number: 0x00000000 - 0xFFFFFFFF.
 *  Byte 13-14 = Parameter 1.
 *  Byte 15-16 = Parameter 2.
 *  Byte 17-18 = Parameter 3.
 *  Byte 19-20 = Parameter 4.
 *  Byte 21-22 = Parameter 5.
 *  Byte 23-24 = Parameter 6.
 */

/*  Command 0x1016 is used to configure camera settings.
 *  Byte 1-4   = Setting address.
 *  Byte 5-6   = Setting.
 *
 *  Examples:
 *
 *  Manual exposure:
 *  Exposure   = 0x0000500E, 0x0001 = M, 0x0002 = Auto, etc.
 *  ShutterSpe = 0x0000500D, 0x0023 = 1/40, 0x002B = 1/250, etc.
 *  Aperture   = 0x00005007, 0x0001 = F2.7, 0x0002 = F3.0, etc. 
 *
 *  Still image:
 *  Image size = 0x00005003, Size is written in ASCII.
 *  Quality    = 0x00005004, 0x0001 = Economy, 0x0002 = Normal, etc.
 *  WB         = 0x00005005, 0x0002 = Auto WB, 0x0004 = Daylight, etc.
 *  Metering   = 0x0000500B, 0x0002 = Center weighted, 0x0003 = Set multi, etc. 
 *  Flash      = 0x0000500C, 0x0001 = Auto flash, 0x0002 = Flash off, etc. 
 *  ISO        = 0x0000500F, 0xFFFF = Auto, 0x0064 = 100, 0x00C8 = 200, etc.
 *  EV         = 0x00005010, 0x0000 = -2.0EV, 0x0001 = -1.7EV, etc.
 *  Storage    = 0x0000D002, 0x0000 = Save to SD, 0x0001 = Save to PC. 
 *  Rec light  = 0x0000D008, 0x0000 = Off, 0x0001 = On
 *  Movie mode = 0x0000D00B, 0x0001 = HD, 0x0002 = HS
 *
 *  Continues shutter:
 *  High-speed = 0x0000D00F, 0x0001 = 1fps, 0x000C = 30fps, etc
 *  UpperLimit = 0x0000D010, 0x0001 = 3fps, 0x0002 = 5fps, etc.
 *
 *  Prerecord Still Image:
 *  CS shot    = 0x0000D011, 0x001E = 30, 0x0028 = 40, etc. 
 *
 *  Movie:
 *  HD setting = 0x0000D00C, 0x0000 = FHD, 0x0001 = HD
 *  HS setting = 0x0000D00D, 0x0000 = 300fps, 0x0001 = 600fps, etc. 
 *
 *  Focus      = 0x0000500A, 0x0002 = AF, 0x0003 = Macro focus, etc. 
 *
 */

/*  Command 0x1014 / 0x1015 reads out a camera setting.
 *  Byte 1-4   = Setting address.
 *
 *  1. packet  = Setting?
 *  2. packet  = ACK?
 */

usb_dev_handle *dev = NULL;
char tmp[BUF_SIZE];
char img[IMG_BUF_SIZE];
PTP_DEVICE_INFO deviceInfo;
PTP_DEVICE_PROPERTY deviceProperty;
PTP_OBJECT_INFO objectInfo;
DWORD_DATA_SET *objectHandles;
PTP_CONTAINER *rx; 
DWORD zoomSetting;
DWORD focusSetting;
DWORD USB_CMD_ID = 0xFFFFFFFF;

void exf1Cmd(WORD cmd, ...)
{
    DWORD dwordVal;
    WORD wordVal;
    char *pString;

    va_list ap;
    va_start(ap, cmd);

    switch(cmd){
        case CMD_WRITE:
            dwordVal = va_arg(ap, int);  // Address
            wordVal  = va_arg(ap, int);  // Value.
            usbTx(cmd, TYPE_CMD,  sizeof(dwordVal),(DWORD) dwordVal, 0);
            usbTx(cmd, TYPE_DATA, sizeof(wordVal), (DWORD) wordVal, 0);
            usbRx();

            if (dwordVal == ADDR_FUNCTIONALITY) {
                //do
                    usbRxEvent();
                //while (rx->code != EVT_DEVICE_INFO_CHANGED);
            }

            break;

        case CMD_READ:
            break;

        case CMD_MOVIE_RESET: 
        case CMD_CS_RELEASE:
        case CMD_MOVIE_RELEASE:
        case CMD_STILL_START:
        case CMD_MOVIE_START:
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, sizeof(dwordVal), (DWORD) dwordVal, 0);
            if (dwordVal) { // PreRecordEnabled...
                do
                    usbGetStatus();
                while (usbRx() < 0);
            }
            else
                usbRx();
            break;

        case CMD_CS_PRESS:
        case CMD_SHUTTER:
        case CMD_HALF_PRESS:
            usbTx(cmd, TYPE_CMD, 0, 0, 0);
            usbRxEvent();
            usbRx();
            break;

        case CMD_STILL_RESET:
        case CMD_MOVIE_PRESS:
        case CMD_CLOSE_SESSION:
        case CMD_HALF_RELEASE:
        case CMD_STILL_STOP:
        case CMD_MOVIE_STOP:
            usbTx(cmd, TYPE_CMD, 0, 0, 0);
            usbRx();
            break;

        case CMD_CF_PRESS:
        case CMD_CZ_PRESS:
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, 2*sizeof(dwordVal), (DWORD) dwordVal, 0);
            usbRx();
            break;
        
        case CMD_CF_RELEASE:
        case CMD_CZ_RELEASE:
        case CMD_ZOOM:
        case CMD_FOCUS:
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, 2*sizeof(dwordVal), (DWORD) dwordVal, 0);
            while (usbRxEvent() > 0);
            usbRx();
            while (usbRxEvent() > 0);
            break;

        case CMD_OPEN_SESSION:
            USB_CMD_ID = 0;
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, sizeof(dwordVal), (DWORD) dwordVal, 0);
            usbRx();
            break;

        case CMD_GET_STILL_HANDLES:
        case CMD_GET_MOVIE_HANDLES:
        case CMD_GET_DEVICE_INFO:
            usbTx(cmd, TYPE_CMD, 0, 0, 0);
            usbRx();
            usbRx();
            break;

        case CMD_GET_OBJECT_INFO:
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, 2*sizeof(DWORD), dwordVal, 0xFFFFFFFF);
            usbRx();
            usbRx();
            break;

        case CMD_GET_PROP_DESC:
            dwordVal = va_arg(ap, int);
            usbTx(cmd, TYPE_CMD, sizeof(dwordVal), (DWORD) dwordVal, 0);
            usbRx();
            usbRx();
            break;

        case CMD_GET_THUMBNAIL:
        case CMD_GET_OBJECT:
            wordVal  = va_arg(ap, int); // File/memory destination.
            dwordVal = va_arg(ap, int); // objectHandle
            usbTx(cmd, TYPE_CMD, sizeof(DWORD), dwordVal, 0);
            switch (wordVal) {
                case TO_FILE:
                    pString = (char *) va_arg(ap, int);
                    if (usbRxToFile(pString) < 0);
                        usbRxToFile(pString);
                    break;
                case TO_MEM:
                    break;
            }
            usbRx();
            break;

        default:
            printf("Unsupported command type!\n");
    }
    
    va_end(ap);
    USB_CMD_ID++;
}

void usbTx(WORD cmd, WORD cmdType, int nCmdParameterBytes, DWORD cmdParameter1, DWORD cmdParameter2) {

    PTP_CONTAINER tx;
    int packetSize = 12 + nCmdParameterBytes;

    tx.length   = packetSize;
    tx.type     = cmdType;
    tx.code     = cmd;
    tx.trans_id = USB_CMD_ID;

    if (nCmdParameterBytes == 2) {
        tx.payload.word_params.param1 = 0xFFFF & cmdParameter1;
    }
    else if (nCmdParameterBytes == 4) {
        tx.payload.dword_params.param1 = cmdParameter1;
    }
    else if (nCmdParameterBytes == 8) {
        tx.payload.dword_params.param1 = cmdParameter1;
        tx.payload.dword_params.param2 = cmdParameter2;
    }

    /*
    int i;
    char *pTx = (char *) &tx;
    for (i=0; i<packetSize; i++)
        printf("%02X-", 0xFF & *(pTx + i));
    printf("\n");
    */

    if (usb_bulk_write(dev, EP_OUT, (char *) &tx, packetSize, TIME_OUT) != packetSize)
    {
        printf("Error: Bulk write failed for this command: %02X!\n", 0xFFFF & cmd);
        printf("1\n");
        exit_camera();
    }
    
}

int usbRx() {

    int i, bytesRead = usb_bulk_read(dev, EP_IN, tmp, BUF_SIZE, TIME_OUT);
    rx = (PTP_CONTAINER *) tmp;
    
    if (bytesRead > 0) {
/*
        printf("usbRx: 0x%04X 0x%04X\n", rx->code, rx->type);
        char *pTx = rx->payload.data;
        for (i=0; i<rx->length-12; i++)
            printf("%02X-", 0xFF & *(pTx + i));
        printf("\n");
*/
        switch (rx->type) {
            case TYPE_DATA:
                switch (rx->code) {
                    case CMD_GET_DEVICE_INFO:
                        setDeviceInfo(rx->payload.data);
                        printDeviceInfo();
                        break;
                    
                    case CMD_GET_PROP_DESC:
                        setDeviceProperty(rx->payload.data);
                        printDeviceProperty();
                        break;

                    case CMD_GET_OBJECT_INFO:
                        setObjectInfo(rx->payload.data);
                        //printObjectInfo();
                        break;

                    case CMD_GET_STILL_HANDLES:
                    case CMD_GET_MOVIE_HANDLES:
                        free(objectHandles);
                        getDwordDataSet(&objectHandles, rx->payload.data);
                        //printDwordDataSet("ObjectHandles   : ", objectHandles);
                        break; 

                    default:
                        printf("Unhandled data package (usbRx): 0x%04X 0x%04X ", rx->code, rx->type);
                        char *pTx = rx->payload.data;
                        for (i=0; i<rx->length-12; i++)
                            printf("%02X-", 0xFF & *(pTx + i));
                        printf("\n");
                }
                break;
            case TYPE_RESPONSE:
                if (rx->code != CMD_OK) printf("Ack = %02X.\n", rx->code);
                break;
                /*
            case TYPE_EVENT:
                printf("Read an event message.\n");
                //usbRx();
                if (usb_interrupt_read(dev, EP_INT, tmp, 16, TIME_OUT) < 0)
                    printf(" Error: interrupt read failed!\n");
                bytesRead = rx->code; 
                break;
                 * */
            default:
                printf("Unknown message type: 0x%04x\n", 0xFFFF & rx->type);
        }
    }
    else if (bytesRead != -116) {
        printf("Error: Bulk read failed! %d %s\n", bytesRead, usb_strerror());
    }
    return bytesRead;
}

int usbRxToFile(char *fileName) {

    FILE *pFile;
    
    int bytesRead = usb_bulk_read(dev, EP_IN, img, IMG_BUF_SIZE, TIME_OUT);
    int bytesRemaining = -1, objectSize = 0;
    rx = (PTP_CONTAINER *) img;

    if (bytesRead > 0) {

        switch (rx->type) {
            case TYPE_DATA:
                objectSize = rx->length-12;
                pFile = fopen(fileName, "wb");
                fwrite(rx->payload.data, 1, bytesRead-12, pFile);
                printf("> Transferring %d bytes... \n", objectSize);
                for (bytesRemaining = rx->length - bytesRead; bytesRemaining > 0; bytesRemaining -= bytesRead) {
                    do bytesRead = usb_bulk_read(dev, EP_IN, img, IMG_BUF_SIZE, TIME_OUT);
                    while (bytesRead < 0);
                    fwrite(img, 1, bytesRead, pFile);
                }
                fclose (pFile);
                printf("> %s saved to disk. \n", fileName);
                break;
            case TYPE_RESPONSE:
                if (rx->code != CMD_OK) printf("Ack = %02X.\n", rx->code);
                break;
            default:
                printf("Unhandled data package (usbRxToFile): 0x%04X 0x%04X\n", rx->type, rx->code);
                char *pTx = rx->payload.data; 
                int i;
                for (i=0; i<rx->length-12; i++)
                    printf("%02X-", 0xFF & *(pTx + i));
                printf("\n");
                break; 
        }
    }
    return bytesRemaining;
}

int usbRxEvent(){

    int bytesRead = usb_interrupt_read(dev, EP_INT, tmp, 24, TIME_OUT/5);
    rx = (PTP_CONTAINER *) tmp;

    if (bytesRead > 0) {
/*
        printf("usbRxEvent: 0x%04X 0x%04X\n", rx->code, rx->type);
        char *pTx = rx->payload.data;
        for (i=0; i<rx->length-12; i++)
            printf("%02X-", 0xFF & *(pTx + i));
        printf("\n");
*/
        if (rx->type == TYPE_EVENT) {
            switch (rx->code) {
                case EVT_FOCUS_CHANGED:
                    focusSetting = rx->payload.dword_params.param1;
                    break;
                case EVT_FOCUS_OK:
                case EVT_DEVICE_INFO_CHANGED:
                    break;
                case EVT_ZOOM_CHANGED:
                    zoomSetting = rx->payload.dword_params.param1;
                    break;
                default:
                    printf("Unhandled event code: 0x%04x\n", rx->code);
            }
        }
        else
            printf("Not an event message!\n");
    }
    else if (bytesRead != -116 && bytesRead != -110)
        printf(" Error: interrupt read failed: %s!, %i\n", usb_strerror(),bytesRead );

    return bytesRead;
}

void usbGetStatus() {

    char statusBytes[2];
    
    if (usb_control_msg(dev, 0x82, 0x00, 0x00, 0x81, statusBytes, 0x2, TIME_OUT) < 0)
        printf("error: cmd write 1 failed\n");

    if (usb_control_msg(dev, 0x82, 0x00, 0x00, 0x2, statusBytes, 0x2, TIME_OUT) < 0)
        printf("error: cmd write 2 failed\n");

}

WORD getStringDataSet(STRING_DATA_SET **dst, char *src) {

    WORD byteSize;
    STRING_DATA_SET *sds = (STRING_DATA_SET *) src;

    byteSize = (sds->noItems) * sizeof(WORD) + 1;
    *dst     = malloc(byteSize);
    memcpy(*dst, sds, byteSize);

    return byteSize;
}

DWORD getWordDataSet(WORD_DATA_SET **dst, char *src) {

    DWORD byteSize;
    WORD_DATA_SET *wds = (WORD_DATA_SET *) src;

    byteSize = (wds->noItems + 2) * sizeof(WORD);
    *dst     = malloc(byteSize);
    memcpy(*dst, wds, byteSize);

    return byteSize;
}

DWORD getDwordDataSet(DWORD_DATA_SET **dst, char *src) {

    DWORD byteSize;
    DWORD_DATA_SET *wds = (DWORD_DATA_SET *) src;

    byteSize = (wds->noItems + 1) * sizeof(DWORD);
    *dst     = malloc(byteSize);
    memcpy(*dst, wds, byteSize);

    return byteSize;
}

void setDeviceInfo(char *pData) {

    // Free up old data sets. 
    free(deviceInfo.vendorExtensionDesc);
    free(deviceInfo.operationsSupported);
    free(deviceInfo.eventsSupported);
    free(deviceInfo.devicePropertiesSupported);
    free(deviceInfo.captureFormats);
    free(deviceInfo.imageFormats);
    free(deviceInfo.manufacturer);
    free(deviceInfo.model);
    free(deviceInfo.deviceVersion);
    free(deviceInfo.serialNumber);

    // Retrieve new device info from packet.
    deviceInfo.standardVersion          = GET_WORD (pData);  pData += sizeof(WORD);
    deviceInfo.vendorExtensionID        = GET_DWORD(pData);  pData += sizeof(DWORD);
    deviceInfo.vendorExtensionVersion   = GET_WORD (pData);  pData += sizeof(WORD);
    pData += getStringDataSet(&deviceInfo.vendorExtensionDesc, pData);
    deviceInfo.functionalMode           = GET_WORD (pData);  pData += sizeof(WORD);
    pData += getWordDataSet(&deviceInfo.operationsSupported, pData);
    pData += getWordDataSet(&deviceInfo.eventsSupported, pData);
    pData += getWordDataSet(&deviceInfo.devicePropertiesSupported, pData);
    pData += getWordDataSet(&deviceInfo.captureFormats, pData);
    pData += getWordDataSet(&deviceInfo.imageFormats, pData);
    pData += getStringDataSet(&deviceInfo.manufacturer, pData);
    pData += getStringDataSet(&deviceInfo.model, pData);
    pData += getStringDataSet(&deviceInfo.deviceVersion, pData);
    pData += getStringDataSet(&deviceInfo.serialNumber, pData);

}

void printStringDataSet(char *pDescrition, STRING_DATA_SET *pDataSet) {
    
    int i;
    printf("%s", pDescrition);
    for (i=0; i<pDataSet->noItems; i++)
        printf("%c", pDataSet->data[i]);
    printf("\n");
    
}

void printWordDataSet(char *pDescrition, WORD_DATA_SET *pDataSet) {

    int i;
    printf("%s", pDescrition);
    for (i=0; i<pDataSet->noItems; i++)
        printf("0x%04X, ", pDataSet->data[i]);
    printf("\n");

}

void printDwordDataSet(char *pDescrition, DWORD_DATA_SET *pDataSet) {

    int i;
    printf("%s", pDescrition);
    for (i=0; i<pDataSet->noItems; i++)
        printf("0x%08X, ", pDataSet->data[i]);
    printf("\n");

}

void printDeviceInfo() {

    printStringDataSet("Manufacturer  : ", deviceInfo.manufacturer);
    printStringDataSet("Model         : ", deviceInfo.model);
    printStringDataSet("DeviceVersion : ", deviceInfo.deviceVersion);
    printStringDataSet("SerialNumber  : ", deviceInfo.serialNumber);
                printf("ExtensionID   : 0x%08X\n", deviceInfo.vendorExtensionID);
    printStringDataSet("Description   : ", deviceInfo.vendorExtensionDesc);
                printf("Version       : 0x%04X\n", deviceInfo.vendorExtensionVersion);
      printWordDataSet("Properties    : ", deviceInfo.devicePropertiesSupported);
      printWordDataSet("CaptureFormats: ", deviceInfo.captureFormats);
      printWordDataSet("ImageFormats  : ", deviceInfo.imageFormats);

}

void printEnumDataSet(char *pDescrition, ENUM_FORM *pDataSet, WORD dataType) {

    int i;
    printf("%s", pDescrition);
    switch (dataType) {
        case DATA_TYPE_CHAR:
            for (i=0; i<pDataSet->numberOfValues; i++) printf("0x%02X, ", *(char *)pDataSet->supportedValue[i]);
            break;
        case DATA_TYPE_WORD:
            for (i=0; i<pDataSet->numberOfValues; i++) printf("0x%04X, ", *(WORD *)pDataSet->supportedValue[i]);
            break;
        case DATA_TYPE_DWORD:
            for (i=0; i<pDataSet->numberOfValues; i++) printf("0x%08X, ", *(DWORD *)pDataSet->supportedValue[i]);
            break;
        case DATA_TYPE_STRING:
            for (i=0; i<pDataSet->numberOfValues; i++) {
                printStringDataSet("", (STRING_DATA_SET *)pDataSet->supportedValue[i]);
            }
            break; 
    }
    printf("\n");

}

void setDeviceProperty(char *pData) {

    int i; 

    // Free up old data sets.
    free(deviceProperty.defaultValue);
    free(deviceProperty.currentValue);

    switch (deviceProperty.formFlag) {
        case PROPERTY_FORM_RANGE:
            free(deviceProperty.form.rangeForm.maximumValue);
            free(deviceProperty.form.rangeForm.minimumValue);
            free(deviceProperty.form.rangeForm.stepSize);
            break;
            
        case PROPERTY_FORM_ENUM:
            for (i=0; i<deviceProperty.form.enumForm.numberOfValues; i++)
                free(deviceProperty.form.enumForm.supportedValue[i]);
            free(deviceProperty.form.enumForm.supportedValue);
            break;
             
    }

    // Retrieve new device info from packet.
    deviceProperty.code     = GET_WORD (pData);  pData += sizeof(WORD);
    deviceProperty.dataType = GET_WORD (pData);  pData += sizeof(WORD);
    deviceProperty.getSet   = 0xFF & GET_WORD (pData);  pData += 1;

    switch (deviceProperty.dataType) {
        case DATA_TYPE_CHAR:
            deviceProperty.defaultValue = malloc(sizeof(char));
            deviceProperty.currentValue = malloc(sizeof(char)); 
            *(char*)deviceProperty.defaultValue = *pData; pData += sizeof(char);
            *(char*)deviceProperty.currentValue = *pData; pData += sizeof(char);
            break;

        case DATA_TYPE_WORD:
            deviceProperty.defaultValue = malloc(sizeof(WORD));
            deviceProperty.currentValue = malloc(sizeof(WORD));
            *(WORD *)deviceProperty.defaultValue = *pData; pData += sizeof(WORD);
            *(WORD *)deviceProperty.currentValue = *pData; pData += sizeof(WORD);
            break;

        case DATA_TYPE_DWORD:
            deviceProperty.defaultValue = malloc(sizeof(DWORD));
            deviceProperty.currentValue = malloc(sizeof(DWORD));
            *(DWORD *)deviceProperty.defaultValue = *pData; pData += sizeof(DWORD);
            *(DWORD *)deviceProperty.currentValue = *pData; pData += sizeof(DWORD);
            break;

        case DATA_TYPE_STRING:
            pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.defaultValue, pData);
            pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.currentValue, pData);
            break;

        default:
            printf("Unknown data type: 0x%04X!", deviceProperty.dataType);

    }

    deviceProperty.formFlag = 0xFF & GET_WORD (pData);  pData += 1;

    switch (deviceProperty.formFlag) {
        case PROPERTY_FORM_NONE:
            break;
        case PROPERTY_FORM_RANGE:
            switch (deviceProperty.dataType) {
                case DATA_TYPE_CHAR:
                    deviceProperty.form.rangeForm.minimumValue = malloc(sizeof(char));
                    deviceProperty.form.rangeForm.maximumValue = malloc(sizeof(char));
                    deviceProperty.form.rangeForm.stepSize = malloc(sizeof(char));
                    *(char*)deviceProperty.form.rangeForm.minimumValue = *pData; pData += sizeof(char);
                    *(char*)deviceProperty.form.rangeForm.maximumValue = *pData; pData += sizeof(char);
                    *(char*)deviceProperty.form.rangeForm.stepSize = *pData; pData += sizeof(char);
                    break;

                case DATA_TYPE_WORD:
                    deviceProperty.form.rangeForm.minimumValue = malloc(sizeof(WORD));
                    deviceProperty.form.rangeForm.maximumValue = malloc(sizeof(WORD));
                    deviceProperty.form.rangeForm.stepSize = malloc(sizeof(WORD));
                    *(WORD*)deviceProperty.form.rangeForm.minimumValue = *pData; pData += sizeof(WORD);
                    *(WORD*)deviceProperty.form.rangeForm.maximumValue = *pData; pData += sizeof(WORD);
                    *(WORD*)deviceProperty.form.rangeForm.stepSize = *pData; pData += sizeof(WORD);
                    break;

                case DATA_TYPE_DWORD:
                    deviceProperty.form.rangeForm.minimumValue = malloc(sizeof(DWORD));
                    deviceProperty.form.rangeForm.maximumValue = malloc(sizeof(DWORD));
                    deviceProperty.form.rangeForm.stepSize = malloc(sizeof(DWORD));
                    *(DWORD*)deviceProperty.form.rangeForm.minimumValue = *pData; pData += sizeof(DWORD);
                    *(DWORD*)deviceProperty.form.rangeForm.maximumValue = *pData; pData += sizeof(DWORD);
                    *(DWORD*)deviceProperty.form.rangeForm.stepSize = *pData; pData += sizeof(DWORD);
                    break;

                case DATA_TYPE_STRING:
                    pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.form.rangeForm.minimumValue, pData);
                    pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.form.rangeForm.maximumValue, pData);
                    pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.form.rangeForm.stepSize, pData);
                    break;

                default:
                    printf("Unknown data type: 0x%04X!", deviceProperty.dataType);

            }
            break;
        case PROPERTY_FORM_ENUM:

            deviceProperty.form.enumForm.numberOfValues = GET_WORD(pData); pData += sizeof(WORD);
            switch (deviceProperty.dataType) {
                case DATA_TYPE_CHAR:
                    deviceProperty.form.enumForm.supportedValue = malloc(deviceProperty.form.enumForm.numberOfValues * sizeof(char*));
                    for (i=0; i<deviceProperty.form.enumForm.numberOfValues ; i++) {
                        deviceProperty.form.enumForm.supportedValue[i]  = malloc(sizeof(char));
                        *(char *)deviceProperty.form.enumForm.supportedValue[i] = *pData; pData += sizeof(char);
                    }
                    break;

                case DATA_TYPE_WORD:
                    deviceProperty.form.enumForm.supportedValue = malloc(deviceProperty.form.enumForm.numberOfValues * sizeof(WORD*));
                    for (i=0; i<deviceProperty.form.enumForm.numberOfValues ; i++) {
                        deviceProperty.form.enumForm.supportedValue[i]  = malloc(sizeof(WORD));
                        *(WORD *)deviceProperty.form.enumForm.supportedValue[i] = GET_WORD(pData); pData += sizeof(WORD);
                    }
                    break;

                case DATA_TYPE_DWORD:
                    deviceProperty.form.enumForm.supportedValue = malloc(deviceProperty.form.enumForm.numberOfValues * sizeof(DWORD*));
                    for (i=0; i<deviceProperty.form.enumForm.numberOfValues ; i++) {
                        deviceProperty.form.enumForm.supportedValue[i]  = malloc(sizeof(DWORD));
                        *(DWORD *)deviceProperty.form.enumForm.supportedValue[i] = GET_DWORD(pData); pData += sizeof(DWORD);
                    }
                    break;

                case DATA_TYPE_STRING:
                    deviceProperty.form.enumForm.supportedValue = malloc(deviceProperty.form.enumForm.numberOfValues * sizeof(char*));
                    for (i=0; i<deviceProperty.form.enumForm.numberOfValues ; i++) {
                        deviceProperty.form.enumForm.supportedValue[i]  = malloc(255*sizeof(char));
                        pData += getStringDataSet((STRING_DATA_SET **)&deviceProperty.form.enumForm.supportedValue[i], pData);
                    }
                    break;

                default:
                    printf("Unknown data type: 0x%04X!", deviceProperty.dataType);

            }
            break;
        default:
            printf("Unknown form flag: 0x%01X!", deviceProperty.formFlag);
    }

}

void printDeviceProperty() {

    printf("Code     : 0x%02X\n", deviceProperty.code);
    printf("DataType : 0x%02X\n", deviceProperty.dataType);
    printf("GetSet   : 0x%01X\n", deviceProperty.getSet);
    printf("FormFlag : 0x%01X\n", deviceProperty.formFlag);

    switch (deviceProperty.dataType) {
        case DATA_TYPE_CHAR:
            printf("Default  : 0x%02X\n", *(char *)deviceProperty.defaultValue);
            printf("Current  : 0x%02X\n", *(char *)deviceProperty.currentValue);
            break;

        case DATA_TYPE_WORD:
            printf("Default  : 0x%04X\n", *(WORD *)deviceProperty.defaultValue);
            printf("Current  : 0x%04X\n", *(WORD *)deviceProperty.currentValue);
            break;

        case DATA_TYPE_DWORD:
            printf("Default  : 0x%08X\n", *(DWORD *)deviceProperty.defaultValue);
            printf("Current  : 0x%08X\n", *(DWORD *)deviceProperty.currentValue);
            break;

        case DATA_TYPE_STRING:
            printStringDataSet("Default  : ", deviceProperty.defaultValue);
            printStringDataSet("Current  : ", deviceProperty.currentValue);
            break;

        default:
            printf("Unknown data type: 0x%04X!", deviceProperty.dataType);

    }

     switch (deviceProperty.formFlag) {
        case PROPERTY_FORM_NONE:
            break;
        case PROPERTY_FORM_RANGE:
            switch (deviceProperty.dataType) {
                case DATA_TYPE_CHAR:
                    printf("Minimum  : 0x%02X\n", *(char *)deviceProperty.form.rangeForm.minimumValue);
                    printf("Maximum  : 0x%02X\n", *(char *)deviceProperty.form.rangeForm.maximumValue);
                    printf("StepSize : 0x%02X\n", *(char *)deviceProperty.form.rangeForm.stepSize);
                    break;

                case DATA_TYPE_WORD:
                    printf("Minimum  : 0x%04X\n", *(WORD *)deviceProperty.form.rangeForm.minimumValue);
                    printf("Maximum  : 0x%04X\n", *(WORD *)deviceProperty.form.rangeForm.maximumValue);
                    printf("StepSize : 0x%04X\n", *(WORD *)deviceProperty.form.rangeForm.stepSize);
                    break;

                case DATA_TYPE_DWORD:
                    printf("Minimum  : 0x%08X\n", *(DWORD *)deviceProperty.form.rangeForm.minimumValue);
                    printf("Maximum  : 0x%08X\n", *(DWORD *)deviceProperty.form.rangeForm.maximumValue);
                    printf("StepSize : 0x%08X\n", *(DWORD *)deviceProperty.form.rangeForm.stepSize);
                    break;

                case DATA_TYPE_STRING:
                    printStringDataSet("Minimum  : ", deviceProperty.form.rangeForm.minimumValue);
                    printStringDataSet("Maximum  : ", deviceProperty.form.rangeForm.maximumValue);
                    printStringDataSet("StepSize : ", deviceProperty.form.rangeForm.stepSize);
                    break;

                default:
                    printf("Unknown data type: 0x%04X!", deviceProperty.dataType);

            }        
            break; 
         case PROPERTY_FORM_ENUM:
            printEnumDataSet("Values   : ", &deviceProperty.form.enumForm, deviceProperty.dataType);
            break;
            
        default:
            printf("Unknown form flag: 0x%02X!", deviceProperty.formFlag);
    }
}



void setObjectInfo(char *pData) {

    // Free up old data sets.
    free(objectInfo.fileName);
    free(objectInfo.captureDate);
    free(objectInfo.modificationDate);
    free(objectInfo.keyWords);

    // Retrieve new object info from packet.
    objectInfo.storageId = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.objectFormat = GET_WORD (pData);  pData += sizeof(WORD);
    objectInfo.protectionStatus = GET_WORD (pData);  pData += sizeof(WORD);
    objectInfo.objectCompressedSize = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.thumbFormat = GET_WORD (pData);  pData += sizeof(WORD);
    objectInfo.thumbCompressedSize = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.thumbPixWidth = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.thumbPixHeight = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.imagePixWidth = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.imagePixHeight = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.imageBitDepth = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.parentObject = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.associationType = GET_WORD (pData);  pData += sizeof(WORD);
    objectInfo.associationDesc = GET_DWORD(pData);  pData += sizeof(DWORD);
    objectInfo.sequenceNumber = GET_DWORD(pData);  pData += sizeof(DWORD);
    pData += getStringDataSet(&objectInfo.fileName, pData);
    pData += getStringDataSet(&objectInfo.captureDate, pData);
    pData += getStringDataSet(&objectInfo.modificationDate, pData);
    pData += getStringDataSet(&objectInfo.keyWords, pData);

}

void printObjectInfo() {

    printStringDataSet("Filename        : ", objectInfo.fileName);
    printStringDataSet("CaptureDate     : ", objectInfo.captureDate);
    printStringDataSet("Modification    : ", objectInfo.modificationDate);
    printStringDataSet("Keywords        : ", objectInfo.keyWords);
                printf("StorageID       : 0x%08X\n", objectInfo.storageId);
                printf("ObjectFormat    : 0x%04X\n", objectInfo.objectFormat);
                printf("Protection      : 0x%04X\n", objectInfo.protectionStatus);
                printf("ObjectDimension : %dx%dx%d\n", objectInfo.imagePixWidth, objectInfo.imagePixHeight, objectInfo.imageBitDepth);
                printf("CompressedSize  : %d bytes\n", objectInfo.objectCompressedSize);
                printf("ThumbFormat     : 0x%04X\n", objectInfo.thumbFormat);
                printf("ThumbSize       : %d bytes\n", objectInfo.thumbCompressedSize);
                printf("ThumbDimension  : %dx%d\n", objectInfo.thumbPixWidth, objectInfo.thumbPixHeight);
                printf("ParentObject    : 0x%08X\n", objectInfo.parentObject);
                printf("AssociationType : 0x%04X\n", objectInfo.associationType);
                printf("AssociationDesc : 0x%08X\n", objectInfo.associationDesc);
                printf("SequenceNumber  : 0x%08X\n", objectInfo.sequenceNumber);

}

//void usbCmdGen(short int cmdId, int cmdParameter, short int postCmdReads) {
void usbCmdGen(short int cmd, short int postCmdReads, int nCmdParameters, char cmdParameters[]) {

    int bytesRead = 0, packetSize = 12, i, cmdIndex = 1;
    char cmdBuffer[20], *pCmdBuffer, *pCmdParameters;

    if (nCmdParameters%4 == 0 || nCmdParameters > 4) {

        // Calculate package size;
        packetSize = 12 + 4*(int)(nCmdParameters / 4);
        //printf("PacketSize=%d\n", packetSize);

        // Assuming big endianess here!
        pCmdBuffer  = cmdBuffer;
        pCmdBuffer += 0; SET_DWORD(pCmdBuffer, packetSize);   // Number of bytes in USB packet.
        pCmdBuffer += 4; SET_WORD (pCmdBuffer, cmdIndex);     // Command index.
        pCmdBuffer += 2; SET_WORD (pCmdBuffer, cmd);          // Command.
        pCmdBuffer += 2; SET_DWORD(pCmdBuffer, USB_CMD_ID);   // Command number.
        pCmdBuffer += 4;

        pCmdParameters = cmdParameters;
        for (i=12; i<packetSize; i++)
            *(pCmdBuffer++) = *(pCmdParameters++);               // Command parameters.

        /*
        for (i=0; i<packetSize; i++)
            printf("%02X-", 0xFF & cmdBuffer[i]);
        printf("\n");
        */

        if (usb_bulk_write(dev, EP_OUT, cmdBuffer, packetSize, TIME_OUT) != packetSize)
        {
            printf("Error: Bulk write failed for this command: %02X!\n", 0xFFFF & cmd);
            printf("2\n");
            exit_camera();
        }
        cmdIndex++;
    }

    if (nCmdParameters%4 == 2) {

        // Calculate package size;
        packetSize = 14;
        //printf("PacketSize=%d\n", packetSize);

        // Assuming big endianess here!
        pCmdBuffer  = cmdBuffer;
        pCmdBuffer += 0; SET_DWORD(pCmdBuffer, packetSize);   // Number of bytes in USB packet.
        pCmdBuffer += 4; SET_WORD (pCmdBuffer, cmdIndex);     // Command index.
        pCmdBuffer += 2; SET_WORD (pCmdBuffer, cmd);          // Command.
        pCmdBuffer += 2; SET_DWORD(pCmdBuffer, USB_CMD_ID);   // Command number.
        pCmdBuffer += 4;

        pCmdParameters = cmdParameters + nCmdParameters - 2;
        for (i=12; i<packetSize; i++)
            *(pCmdBuffer++) = *(pCmdParameters++);               // Command parameters.

        /*
        for (i=0; i<packetSize; i++)
            printf("%02X-", 0xFF & cmdBuffer[i]);
        printf("\n");
        */

        if (usb_bulk_write(dev, EP_OUT, cmdBuffer, packetSize, TIME_OUT) != packetSize)
        {
            printf("Error: Bulk write failed for this command: %02X!\n", 0xFFFF & cmd);
            printf("3\n");
            exit_camera();
        }
    }

    for (i=0; i<postCmdReads; i++) {
        bytesRead = usb_bulk_read(dev, EP_IN, tmp, BUF_SIZE, TIME_OUT);

        char *pTx = tmp;
        int j; 
        for (j=0; j<bytesRead; j++)
            printf("%02X-", 0xFF & *(pTx + j));
        printf("\n");

        if (bytesRead < 0)
            printf("Error: Bulk read failed for this command: %02X!\n", 0xFFFF & cmd);
    }

    USB_CMD_ID++;
}

int usbInit(int device_num) {
    usb_init();
    usb_find_busses();
    usb_find_devices();

    if(!(dev = open_dev(device_num))) {
      printf(" Error: camera not found!\n");
      return 0;
    }

    if(usb_set_configuration(dev, 1) < 0) {
      printf(" Error: setting config 1. \n");
      usb_close(dev);
      return 0;
    }

    if(usb_claim_interface(dev, 0) < 0) {
      printf(" Error: claiming interface 0 failed. \n");
      usb_close(dev);
      return 0;
    }
   
    // DeviceReset.
    if (usb_control_msg(dev, 0x21, 0x66, 0x00, 0x00, NULL, 0, TIME_OUT) < 0)
      printf("error: cmd write 1 failed\n");

    usbRxEvent();

    if (usb_clear_halt(dev, EP_IN) < 0)
      printf("error: halt clear failed.\n");

    if (usb_clear_halt(dev, EP_OUT) < 0)
      printf("error: halt clear failed.\n");

    // GetDeviceStatus.
    if (usb_control_msg(dev, 0xA1, 0x67, 0x00, 0x00, &tmp[0], 0x0400, TIME_OUT) < 0)
      printf("error: cmd write 2 failed\n");
          printf("INICIALIZACE OK\n");
    return 1; 
}

usb_dev_handle *open_dev(int device_num)
{
    //!!! HLEDANI TOHO FOTAKU !!!
  struct usb_bus *bus;
  struct usb_device *dev;
  int i = 0;
  
  for(bus = usb_get_busses(); bus; bus = bus->next)
    {
      for(dev = bus->devices; dev; dev = dev->next)
        {
          if(dev->descriptor.idVendor == MY_VID
             && dev->descriptor.idProduct == MY_PID)
            {
		printf("hledani USB %i %i", i, device_num);
		if( i == device_num )
		   return usb_open(dev);
		else
		    i++;
            }
        }
    }
  return NULL;
}