#include <stdio.h>
#include <conio.h> // Erforderlich fuer getch()
#include <stdlib.h>
#include <windows.h> // Entfernen oder ersetzen, falls die Kompilierung unter Linux
#include "ftd2xx.h"  // Offizielle Header-Datei FTDI
#include <assert.h>

#define FT232H_INDEX 0 // Geraeteindex (0 - erster angeschlossener Chip FTDI)

// Funktion zum Senden von Befehlen an den Puffer MPSSE
FT_STATUS Send_Commands(FT_HANDLE ftHandle, BYTE* buffer, DWORD size) {
    DWORD bytesWritten = 0;
    FT_STATUS status = FT_Write(ftHandle, buffer, size, &bytesWritten);
    if (status != FT_OK || bytesWritten != size) {
        printf("[!] MPSSE Aktivierung fehlgeschlagen.\n");
        return FT_OTHER_ERROR;
    }
    return FT_OK;
}

int main() {
    FT_HANDLE ftHandle;
    FT_STATUS ftStatus;
    BYTE txBuffer[64];
    BYTE rxBuffer[4];
    DWORD dwBytesRead = 0;
    DWORD idx = 0;
    DWORD           driverVersion = 0;
    FT_DEVICE_LIST_INFO_NODE* devInfo;
    DWORD numDevs;

    printf("=== Reading JTAG IDCODE via FT232H (MPSSE) ===\n\n");

    // 1. oeffnen des Geraets FT232H
    ftStatus = FT_Open(FT232H_INDEX, &ftHandle);
    if (ftStatus != FT_OK) {
        printf("[!] Fehler: FT232H konnte nicht geoeffnet werden. (Status: %d)\n", (int)ftStatus);
        return 1;
    }
    assert(ftHandle != NULL);

    ftStatus = FT_GetDriverVersion(ftHandle, &driverVersion);
    if (ftStatus != FT_OK)
    {
        printf("Failure.  FT_GetDriverVersion returned %d.\n",
            (int)ftStatus);
        //goto exit;
        return 1;
    }

    printf("D2XX version : %x.%x.%x\n",
        (unsigned int)((driverVersion & 0x00FF0000) >> 16),
        (unsigned int)((driverVersion & 0x0000FF00) >> 8),
        (unsigned int)(driverVersion & 0x000000FF)
    );

    // create the device information list
    ftStatus = FT_CreateDeviceInfoList(&numDevs);
    if (ftStatus == FT_OK) {
        printf("Number of devices is %d\n", numDevs);
    }
    if (numDevs > 0) {
        // allocate storage for list based on numDevs
        devInfo = (FT_DEVICE_LIST_INFO_NODE*)malloc(sizeof(FT_DEVICE_LIST_INFO_NODE) * numDevs);
        // get the device information list
        ftStatus = FT_GetDeviceInfoList(devInfo, &numDevs);
        if (ftStatus == FT_OK) {
            for (int i = 0; i < numDevs; i++) {
                printf("Dev %d:\n", i);
                printf(" Flags=0x%x\n", devInfo[i].Flags);
                printf(" Type=0x%x\n", devInfo[i].Type);
                printf(" ID=0x%x\n", devInfo[i].ID);
                printf(" LocId=0x%x\n", devInfo[i].LocId);
                printf(" SerialNumber=%s\n", devInfo[i].SerialNumber);
                printf(" Description=%s\n\n", devInfo[i].Description);
            }
        }
    }

    // 2. USB-Reset und Basiskonfiguration
    FT_ResetDevice(ftHandle);
    FT_SetUSBParameters(ftHandle, 65536, 65536); // Puffergroeße 64КБ
    FT_SetChars(ftHandle, 0, 0, 0, 0);           // Deaktivieren von Sonderzeichen
    FT_SetTimeouts(ftHandle, 1000, 1000);        // Lese-/Schreib-Timeouts: 1 Sek.
    FT_SetLatencyTimer(ftHandle, 1);             // Minimale Latenz: 1 ms

    // 3. Modusaktivierung MPSSE
    FT_SetBitMode(ftHandle, 0x00, 0x00); // Pin-Controller-Reset
    Sleep(50);
    ftStatus = FT_SetBitMode(ftHandle, 0x00, 0x02); // Aktivieren des Modus MPSSE (0x02)
    if (ftStatus != FT_OK) {
        printf("[!] MPSSE Aktivierung fehlgeschlagen.\n");
        FT_Close(ftHandle);
        return 1;
    }
    Sleep(50); // Warten auf Stabilisierung MPSSE

    // 4. Echotest zur Motorsynchronisation MPSSE
    idx = 0;
    txBuffer[idx++] = 0xAB; // Senden eines ungueltigen Befehls MPSSE
    if (Send_Commands(ftHandle, txBuffer, idx) != FT_OK) goto cleanup;
    
    Sleep(10);
    FT_GetQueueStatus(ftHandle, &dwBytesRead);
    if (dwBytesRead > 0) {
        FT_Read(ftHandle, rxBuffer, dwBytesRead, &dwBytesRead);
        // Wir ueberprüfen, ob das MPSSE den Fehler 0xFA sowie unser Befehlsbyte 0xAB zurückgegeben hat.
        if (rxBuffer[0] != 0xFA || rxBuffer[1] != 0xAB) {
            printf("[!] MPSSE Echo Test Synchronization Error.\n");
            goto cleanup;
        }
    } else {
        printf("[!] The MPSSE engine is unresponsive. Check the device driver.\n");
        goto cleanup;
    }

    // 5. Konfiguration der Pin-Richtung JTAG
    // AD0(TCK)=Output(1), AD1(TDI)=Output(1), AD2(TDO)=Input(0), AD3(TMS)=Output(1)
    // Richtung: 0x0B (00001011in binaerer Form)
    idx = 0;
    txBuffer[idx++] = 0x80; // Befehl: Downstream-Port konfigurieren (ADBUS)
    txBuffer[idx++] = 0x00; // Anfaengliche Pin-Werte (alle in 0)
    txBuffer[idx++] = 0x0B; // Pin-Richtung
    
    // Taktfrequenzkonfiguration TCK (Installation eines 6-MHz-Teilers zur Stabilisierung)
    txBuffer[idx++] = 0x8A; // Deaktivieren Sie den internen ÷5-Teiler (Basisfrequenz: 60 MHz).
    txBuffer[idx++] = 0x86; // Befehl: TCK-Frequenzteiler setzen
    txBuffer[idx++] = 0x04; // Teiler fuer das untere Byte: Frequenz = 60МГц / ((1 + 4) * 2) = 6 МГц
    txBuffer[idx++] = 0x00; // Teiler High Byte
    if (Send_Commands(ftHandle, txBuffer, idx) != FT_OK) goto cleanup;

    // 6. JTAG-TAP-Controller-Reset (Übergang in den Zustand „Test-Logic-Reset“)
    // Gemaeß dem JTAG-Standard wird der Schutzschalter zurueckgesetzt, wenn TMS=1 für 5 TCK-Zyklen gehalten wird.
    // und der Zielchip wird seinen IDCODE automatisch in das Datenregister (DR) übernehmen.
    idx = 0;
    txBuffer[idx++] = 0x4B; // MPSSE-Befehl: Bits auf der TMS-Leitung uebertragen (ohne Auslesen)
    txBuffer[idx++] = 0x04; // Anzahl der Bits minus 1 (0x04 bedeutet 5 Bits)
    txBuffer[idx++] = 0xFF; // Bitwerte (TMS = 1,1,1,1,1)
    if (Send_Commands(ftHandle, txBuffer, idx) != FT_OK) goto cleanup;

    // 7. Übergang des endlichen Automaten in den Zustand Shift-DR zum Auslesen des Datenregisters.
    // Bitsequenz TMS: 0 (in Run-Test/Idle), 1 (in Select-DR-Scan), 0 (in Capture-DR), 0 (in Shift-DR)
    // TMS-Binaersequenz (vom Ende her): 0010 -> 0x02 (Uebertragung von 4 Bits)
    idx = 0;
    txBuffer[idx++] = 0x4B; 
    txBuffer[idx++] = 0x03; // Количество бит минус 1 (4 бита)
    txBuffer[idx++] = 0x02; // Значение бит TMS
    if (Send_Commands(ftHandle, txBuffer, idx) != FT_OK) goto cleanup;

    // 8. Auslesen des 32-Bit- (4-Byte-) IDCODE aus dem Datenregister (TDO)
    // Wir verwenden den Befehl fuer gleichzeitiges Takten und Byte-Lesen.
    idx = 0;
    txBuffer[idx++] = 0x24; // MPSSE-Befehl: Bytes an der fallenden Flanke von TCK lesen (MSB/LSB haengt vom Chip ab; typischerweise LSB zuerst).
    txBuffer[idx++] = 0x03; // Anzahl der Bytes minus 1 (0x03 bedeutet 4 Bytes)
    txBuffer[idx++] = 0x00; 
    if (Send_Commands(ftHandle, txBuffer, idx) != FT_OK) goto cleanup;

    // Warten auf den Puffer vom PC und Auslesen des Puffers
    dwBytesRead = 0;
    int timeout = 100;
    while (dwBytesRead < 4 && timeout-- > 0) {
        FT_GetQueueStatus(ftHandle, &dwBytesRead);
        Sleep(5);
    }

    if (dwBytesRead >= 4) {
        FT_Read(ftHandle, rxBuffer, 4, &dwBytesRead);
        
        // Wandle die Bytes in eine 32-Bit-Zahl um.
        unsigned int idcode = (rxBuffer[3] << 24) | (rxBuffer[2] << 16) | (rxBuffer[1] << 8) | rxBuffer[0];
        
        if (idcode == 0x00000000 || idcode == 0xFFFFFFFF) {
            printf("[!] Error: Invalid value read. IDCODE: 0x%08X\n", idcode);
            printf("Check the quality of the connections, the wire lengths, and the power supply to the target board.\n");
        } else {
            printf("========================================\n");
            printf("SUCCESS! Device responded via JTAG.\n");
            printf("========================================\n");
            printf("Read IDCODE: 0x%08X\n", idcode);
            printf("----------------------------------------\n");
            printf("Die Revision (Version): 0x%X\n", (idcode >> 28) & 0xF);
            printf("Device Code (Part ID):   0x%04X\n", (idcode >> 12) & 0xFFFF);
            printf("ID manufacturer (MFG ID):   0x%03X\n", (idcode >> 1) & 0x7FF);
            printf("========================================\n");
        }
    } else {
        printf("[!] Failed to read 4 bytes of IDCODE from the device. (Таймаут).\n");
    }

cleanup:
    // Eine Sitzung beenden mit FTDI
    FT_SetBitMode(ftHandle, 0x00, 0x00); // Zuruecksetzen des MPSSE-Modus auf Standard-UART
    FT_Close(ftHandle);

    printf("\nThe program has finished. Press any key to exit...");
    getch(); // Warte auf Tastendruck...
    return 0;
}