//////////////////////////////////////////////////////////////////////
// File - CPIA.C
//
// Utility functions for accessing the CPIA USB Web Cameras devices
// using WinDrive library.
//
// Copyrights (c) Jungo Ltd. 2000
// Written by Nir Borenshtein
//////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include "cpia.h"
#include "usb_diag_lib.h"

#define BYTES_IN_LINE 16
#define HEX_CHARS_PER_BYTE 3
#define HEX_STOP_POS BYTES_IN_LINE * HEX_CHARS_PER_BYTE

// This string is set to an error message, if one occurs
CHAR CAMERA_ErrorString[1024];

// Input of command from user
static char line [4096];

BOOL GrabFrame(PVOID pBuffer, FRAME_INFO frameInfo, BOOL first_time, BOOL last_time)
{
    HANDLE hWD;
    static CAMERA_HANDLE hCAMERA = NULL;
    DWORD dwBytesTransfered= 0;
    DWORD dwSize;
    BYTE  SetupPacket[8];
    char SetupPacketStr[17];

    if (first_time)
    {
        // Make sure WinDriver is loaded
        if (!USB_Get_WD_handle(&hWD)) 
            return FALSE;
        WD_Close (hWD);

        hCAMERA = CAMERA_LocateAndOpenDevice(frameInfo.uniqueId);
        if (!hCAMERA)
            return FALSE;

        // GotoHiPower Command
        SetSetupPacket(SetupPacket, 8, "4004000000000000");
        CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);

        // Check camera status
        if (!IsCamHiPwr(hCAMERA))
        {
            printf("Camera is not on Hi Poewer.\n");
            IsCmdOrFatalError(hCAMERA);
            return (FALSE);
        }

        // Setting Video Format Required
        sprintf(SetupPacketStr, "%02X%02X%02X%02X%02X%02X%02X%02X",
            0x40, 
            0xC8,
            (frameInfo.frameType == QCIF) ? 0x00 : 0x01,
            (frameInfo.subSample == SS_420) ? 0x00 : 0x01,
            0x00, // 0x00 for YUYV, 0x01 for UYVY
            0x00,
            0x00,
            0x00);

        SetSetupPacket(SetupPacket, 8, SetupPacketStr);
        CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
        if (IsCmdOrFatalError(hCAMERA))
        {
            printf("Setting Video\n");
            return (FALSE);
        }
        
        
    
        // Setting ROI of Capture
        sprintf(SetupPacketStr, "%02X%02X%02X%02X%02X%02X%02X%02X",
            0x40, 
            0xC9,
            0x00,                                       // ColStart
            (frameInfo.frameType == QCIF) ? 22 : 44,    // ColEnd
            0x00,                                       // RowStart
            (frameInfo.frameType == QCIF) ? 36 : 72,    // RowEnd
            0x00,
            0x00);

        SetSetupPacket(SetupPacket, 8, SetupPacketStr);
        CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
        if (IsCmdOrFatalError(hCAMERA))
        {
            printf("Setting ROI\n");
            return (FALSE);
        }

        // Setting Color Parameters
        sprintf(SetupPacketStr, "%02X%02X%02X%02X%02X%02X%02X%02X",
            0x40, 
            0xA3,
            BRIGHTNESS, 
            CONTRAST,
            SATURATION,
            0x00,   
            0x00,
            0x00);

        SetSetupPacket(SetupPacket, 8, SetupPacketStr);
        CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
        if (IsCmdOrFatalError(hCAMERA))
        {
            printf("Setting color\n");
            return (FALSE);
        }
    }
    
    // Frame capture
    sprintf(SetupPacketStr, "%02X%02X%02X%02X%02X%02X%02X%02X",
        0x40, 
        0xC1,
        0x00,   
        0x00,
        0x00,
        0x00,   
        0x00,
        0x00);
    
    SetSetupPacket(SetupPacket, 8, SetupPacketStr);
    CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
    if (IsCmdOrFatalError(hCAMERA))
    {
        printf("Frame capture\n");
        return (FALSE);
    }
    
    dwSize = frameInfo.frameType==QCIF ? QCIF_BUFFER_SIZE : CIF_BUFFER_SIZE;

    // Reading frame data from device into memory
    CAMERA_ReadPipe81(hCAMERA, pBuffer, dwSize, &dwBytesTransfered);
    if (last_time)
    {
        GotoSuspend(hCAMERA);
        CAMERA_Close(hCAMERA);
        hCAMERA = NULL;
    }
    
    if (!(dwBytesTransfered==QCIF_BUFFER_SIZE || dwBytesTransfered==CIF_BUFFER_SIZE))
        return FALSE;
    
    return TRUE;
}

void GotoLoPower(CAMERA_HANDLE hCAMERA)
{
    BYTE  SetupPacket[8];
    
    // GotoLoPower Command
    SetSetupPacket(SetupPacket, 8, "4005000000000000");
    CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
}

void GotoSuspend(CAMERA_HANDLE hCAMERA)
{
    BYTE  SetupPacket[8];
    
    // GotoSuspend Command
    SetSetupPacket(SetupPacket, 8, "4003000000000000");
    CAMERA_WritePipe00(hCAMERA, NULL, 0, SetupPacket);
}

BOOL IsCamHiPwr(CAMERA_HANDLE hCAMERA)
{
    BYTE res = 0;
    PBYTE pBuffer = (PBYTE)malloc (sizeof(BYTE)*8);
    DWORD dwBytesTransfered;
    BYTE  SetupPacket[8];

    SetSetupPacket(SetupPacket, 8, "C003000000000800");
    dwBytesTransfered = CAMERA_ReadPipe00(hCAMERA, pBuffer, 8, SetupPacket);
    if (dwBytesTransfered==0xffffffff)
    {
        printf ("error on reading camera status\n");
        return FALSE;
    }

    if (pBuffer[0] != HI_POWER_STATE)
        return FALSE;

    free(pBuffer);
    return TRUE;
}

void printErrMsg(BYTE rslt, char *errStr)
{
    if (rslt & (0x1 << CPIA))
        printf("%s Error on CPIA\n", errStr);
    if (rslt & (0x1 << SYSTEM))
        printf("%s Error on SYSTEM\n", errStr);
    if (rslt & (0x1 << INT_CTRL))
        printf("%s Error on INT_CTRL\n", errStr);
    if (rslt & (0x1 << PROCESS))
        printf("%s Error on PROCESS\n", errStr);
    if (rslt & (0x1 << USB_COM))
        printf("%s Error on USB_COM\n", errStr);
    if (rslt & (0x1 << VP_CTRL))
        printf("%s Error on VP_CTRL\n", errStr);
    if (rslt & (0x1 << CAPTURE))
        printf("%s Error on CAPTURE\n", errStr);
    if (rslt & (0x1 << DEBUG))
        printf("%s Error on DEBUG\n", errStr);
}

BOOL IsCmdOrFatalError(CAMERA_HANDLE hCAMERA)
{
    BYTE pBuffer[8];
    DWORD dwBytesTransfered;
    BYTE  SetupPacket[8];
    BOOL rc = FALSE;

    BZERO(pBuffer);
    SetSetupPacket(SetupPacket, 8, "C003000000000800");
    dwBytesTransfered = CAMERA_ReadPipe00(hCAMERA, pBuffer, 8, SetupPacket);
    if (dwBytesTransfered==0xffffffff)
    {
        printf ("error on reading camera status\n");
        rc = FALSE;
        goto Exit;
    }

    if (pBuffer[3])
        printErrMsg(pBuffer[3], "Fatal");
    
    if (pBuffer[4])
        printErrMsg(pBuffer[4], "Command");
    
    if (pBuffer[3]||pBuffer[4])
    {
        printf("Camera status: %02X %02X %02X %02X% 02X% 02X% 02X% 02X",
            pBuffer[0],
            pBuffer[1],
            pBuffer[2],
            pBuffer[3],
            pBuffer[4],
            pBuffer[5],
            pBuffer[6],
            pBuffer[7],
            pBuffer[8]);
        GotoSuspend(hCAMERA);
        CAMERA_Close(hCAMERA);
        rc = TRUE;
    }

Exit:
    return rc;
}

DWORD SetSetupPacket(PVOID pBuffer, DWORD dwBytes, char *strVal)
{
    DWORD i, strPos;
    PBYTE pData = pBuffer;
    BYTE res;
    int ch;

    for (strPos=0, i=0; i<dwBytes;)
    {
        ch = SetHexChar(strVal[strPos++]);

        res = ch << 4;

        ch = SetHexChar(strVal[strPos++]);
        
        res += ch;
        pData[i] = res;
        i++;
    }

    // Return the number of bytes that was read
    return i;
}

int SetHexChar(int ch)
{
    if (!isxdigit(ch))
        return -1;

    if (isdigit(ch))
        return ch - '0';
    else
        return toupper(ch) - 'A' + 10;
}

#define COLOR_RED 2
#define COLOR_BLUE 0
#define COLOR_GREEN 1

void YUV2RGB_Calc(BYTE Y, BYTE Cb, BYTE Cr, PBYTE buffer)
{
    double r, g, b, y, u, v; 

    // YCrCb2YUV conversion
    y = (255 / 219) * (Y - 16);
    u = (127 / 112) * (Cb - 128);
    v = (127 / 112) * (Cr - 128);

    // YUV2RGB conversion
    r = y + 1.402 * v;
    g = y - 0.344 * u - 0.714 * v;
    b = y + 1.772 * u;

    buffer[COLOR_RED] = CheckRGBVal(r);
    buffer[COLOR_GREEN] = CheckRGBVal(g);
    buffer[COLOR_BLUE] = CheckRGBVal(b);
}

BOOL ConvertYUV2RGB(PVOID pBuffer, BYTE *pBGR_Buffer, FRAME_INFO frameInfo)
{
    BYTE *pYCrCb_Buffer = (BYTE *)pBuffer;
    WORD LineBytes;
    BYTE dat0, dat1, Y0, Cr0, Y1, Cb0;
    int i, col;
    
    // Reading the header of the frame
    dat0 = *(pYCrCb_Buffer++);
    dat1 = *(pYCrCb_Buffer++);
    if ((dat0 != 0x19) && (dat1 != 0x68))
    {
        printf("The buffer you are tring to read dosn't contain data from CPIA device.\n");
        return FALSE;
    }
    pYCrCb_Buffer += 62;

    // Converting data
    for (i=0; i<frameInfo.frameRowNum; i++) 
    {
        dat0 = *(pYCrCb_Buffer++);
        dat1 = *(pYCrCb_Buffer++);
        LineBytes = (dat1 << 8) | dat0;
        if (LineBytes != frameInfo.dwBytesInLine)
        {
            printf("Error while reading Line Header \n"); 
            return FALSE;
        }

        for (col=0; col<(LineBytes/2); col+=2) 
        {
            Y0  = *(pYCrCb_Buffer++);
            Cb0 = *(pYCrCb_Buffer++);
            Y1  = *(pYCrCb_Buffer++);
            Cr0 = *(pYCrCb_Buffer++);
            YUV2RGB_Calc(Y0, Cb0, Cr0, &pBGR_Buffer[3*(i*frameInfo.frameColNum+col)]);
            YUV2RGB_Calc(Y1, Cb0, Cr0, &pBGR_Buffer[3*(i*frameInfo.frameColNum+col+1)]);
        }
        dat0 = *(pYCrCb_Buffer++);
        if (dat0 == 0xFF) break;
    }

    return TRUE;
}

BYTE CheckRGBVal(double val)
{
    if (val > 255)
        return 255;
    if (val < 0)
        return 0;
    return (BYTE) val;
}

void StoreFrameOnFile(PVOID pBuffer, DWORD dwBytes, char *fileName)
{
    PBYTE pData = (PBYTE) pBuffer;
    BYTE pHex[HEX_STOP_POS+1];
    DWORD offset;
    DWORD i;
    FILE *fp;

    if (!dwBytes)
        return;

    fp = fopen(fileName, "w");
    if (fp == NULL)
    {
        printf("Can't store frame on disk !\n");
        return;
    }

    for (offset=0; offset<dwBytes; offset++)
    {
        DWORD line_offset = offset%BYTES_IN_LINE;
        if (offset && !line_offset)
            fprintf(fp, "%s\n", pHex);
        
        sprintf(pHex+line_offset*HEX_CHARS_PER_BYTE, "%02X ", pData[offset]);
    }

    // Print the last line. fill with blanks if needed
    if (offset%BYTES_IN_LINE)
    {
        for (i=(offset%BYTES_IN_LINE)*HEX_CHARS_PER_BYTE; i<BYTES_IN_LINE*HEX_CHARS_PER_BYTE; i++)
            pHex[i] = ' ';
        pHex[i] = '\0';
    }
    fprintf(fp, "%s\n", pHex);
    fclose(fp);
}

// WinDriver functions
DWORD CAMERA_CountDevices (DWORD dwVendorID, DWORD dwProductID)
{
    WD_VERSION ver;
    WD_USB_SCAN_DEVICES usbScan;
    HANDLE hWD;

    hWD = WD_Open();
     // Check whether the handle is valid & the version is OK
    if (hWD==INVALID_HANDLE_VALUE) 
    {
        sprintf (CAMERA_ErrorString, "Failed opening WinDriver device\n");
        return 0;
    }

    BZERO(ver);
    WD_Version(hWD, &ver);
    if (ver.dwVer<WD_VER) 
    {
        sprintf (CAMERA_ErrorString, "Incorrect WinDriver version\n");
        WD_Close (hWD);
        return 0;
    }

    BZERO(usbScan);
    usbScan.searchId.dwVendorId = dwVendorID;
    usbScan.searchId.dwProductId = dwProductID;  
    WD_UsbScanDevice(hWD,&usbScan);
    WD_Close (hWD);
    if (usbScan.dwDevices==0)
        sprintf (CAMERA_ErrorString, "No devices found\n");
    return usbScan.dwDevices;
}

CAMERA_HANDLE CAMERA_Open(DWORD uniqueId, DWORD configIndex, DWORD dwInterface, DWORD dwAlternate)
{
    CAMERA_HANDLE hCAMERA = (CAMERA_HANDLE) malloc (sizeof (CAMERA_STRUCT));

    WD_VERSION ver;
    WD_USB_DEVICE_REGISTER devReg;

    BZERO(*hCAMERA);

    hCAMERA->hDevice = 0;
    hCAMERA->hWD = WD_Open();

    // Check whether the handle is valid & the version is OK
    if (hCAMERA->hWD==INVALID_HANDLE_VALUE)
    {
        sprintf (CAMERA_ErrorString, "Failed opening WinDriver device\n");
        free(hCAMERA);
        return NULL;
    }

    BZERO(ver);
    WD_Version(hCAMERA->hWD, &ver);
    if (ver.dwVer<WD_VER)
    {
        sprintf (CAMERA_ErrorString, "Incorrect WinDriver version\n");
        WD_Close(hCAMERA->hWD);
        free(hCAMERA);
        return NULL;
    }

    BZERO(devReg);
    devReg.uniqueId = uniqueId;
    devReg.dwConfigurationIndex = configIndex;
    devReg.dwInterfaceNum = dwInterface;
    devReg.dwInterfaceAlternate = dwAlternate;
    WD_UsbDeviceRegister(hCAMERA->hWD, &devReg);
    if (!devReg.hDevice)
    {
        sprintf (CAMERA_ErrorString, "Could not open device\n");
        WD_Close(hCAMERA->hWD);
        free(hCAMERA);
        return NULL;
    }
    hCAMERA->hDevice = devReg.hDevice;
    hCAMERA->deviceInfo = devReg.Device;

        // Open finished OK
        return hCAMERA;
    }

void CAMERA_Close(CAMERA_HANDLE hCAMERA)
{
    WD_USB_DEVICE_REGISTER devReg;

    BZERO(devReg);
    devReg.hDevice = hCAMERA->hDevice;
    WD_UsbDeviceUnregister(hCAMERA->hWD, &devReg);

    // Close WinDriver
    WD_Close(hCAMERA->hWD);
    free (hCAMERA);
}

void CAMERA_GetDeviceInfo(CAMERA_HANDLE hCAMERA, WD_USB_DEVICE_INFO *pDevInfo)
{
    *pDevInfo = hCAMERA->deviceInfo;
}

DWORD CAMERA_ReadPipe00(CAMERA_HANDLE hCAMERA, PVOID pBuffer, DWORD dwSize, CHAR setupPacket[8])
{
    WD_USB_TRANSFER transfer;
    DWORD i;

    BZERO(transfer);
    transfer.dwPipe = 0x00;
    transfer.dwBytes = dwSize;
    transfer.fRead = TRUE;
    for (i=0; i<8; i++)
        transfer.SetupPacket[i] = setupPacket[i];
    transfer.pBuffer = pBuffer;
    transfer.hDevice = hCAMERA->hDevice;
    WD_UsbTransfer(hCAMERA->hWD, &transfer);

    if (transfer.fOK)
        return transfer.dwBytesTransfered;
    return 0xffffffff;
}

DWORD CAMERA_WritePipe00(CAMERA_HANDLE hCAMERA, PVOID pBuffer, DWORD dwSize, CHAR setupPacket[8])
{
    WD_USB_TRANSFER transfer;
    DWORD i;

    BZERO(transfer);
    transfer.dwPipe = 0x00;
    transfer.dwBytes = dwSize;
    for (i=0; i<8; i++)
        transfer.SetupPacket[i] = setupPacket[i];
    transfer.pBuffer = pBuffer;
    transfer.hDevice = hCAMERA->hDevice;
    WD_UsbTransfer(hCAMERA->hWD, &transfer);

    if (transfer.fOK)
        return transfer.dwBytesTransfered;
    return 0xffffffff;
}

BOOL CAMERA_ReadPipe81(CAMERA_HANDLE hCAMERA, PVOID pBuffer, DWORD dwSize, DWORD *pdwTransfered)
{
    WD_USB_TRANSFER transfer;

    BZERO(transfer);
    transfer.dwPipe = 0x81;
    transfer.dwBytes = dwSize;
    transfer.fRead = TRUE;
    transfer.pBuffer = pBuffer;
    transfer.hDevice = hCAMERA->hDevice;
    WD_UsbTransfer(hCAMERA->hWD, &transfer);

    *pdwTransfered = transfer.dwBytesTransfered;
    return transfer.fOK;
}

CAMERA_HANDLE CAMERA_LocateAndOpenDevice(DWORD uniqueId)
{
    CAMERA_HANDLE hCAMERA;
    DWORD configNum = 0, dwInterface = 0, dwAlternate = 0;
    HANDLE hWD;

    if (!USB_Get_WD_handle(&hWD))
        return NULL;
    
    configNum = CAMERA_DEFAULT_CONFIG;
    dwInterface = CAMERA_DEFAULT_INTERFACE;
    dwAlternate = CAMERA_DEFAULT_ALTERNATE;

    hCAMERA = CAMERA_Open (uniqueId, configNum, dwInterface, dwAlternate);
    if (!hCAMERA)
    {
        printf ("%s\n", CAMERA_ErrorString);
        WD_Close(hWD);
        return NULL;
    }

    WD_Close(hWD);
    return hCAMERA;
}