c++ custom debugger to steal register's data

#include <windows.h>
#include <tlhelp32.h>
#include <tchar.h>
#include <stdio.h>
#include <stdint.h>
#include <string>
#include <Winnt.h>

//#define MA2P_NAME "gma2onpc.exe"
//#define MA2P_NAME "calculator.exe"
#define MA2P_NAME "debugThis.exe"

struct BreakpointList {
    char *version;
    DWORD64 address;
};
#define SUPPORTED_VERSION_NO 2
const BreakpointList SUPPORTED_VERSION[] = {
    {"3.3.4.3", 0x14016ade4},
    {"c:\\Users\\Luca\\Desktop\\shit\\programming\\java\\grandMhAck2\\hack\\grandMhAck2", 0x4015cd} //0x4015AD
};

struct ThreadInfo {
    DWORD tid;
    CONTEXT ctx;
    HANDLE hThread;
    BOOL stillValid;
};
BOOL doActionsOnThreads(DWORD dwOwnerPID, void (*action)(DWORD));
DWORD64 getBreakpointAddr(char *version);
ThreadInfo getThreadInfo(DWORD tid);
DWORD getPidByPName(LPCSTR name);
DWORD getPid(LPCSTR name);
BOOL setPermissions(BOOL enable);
void printError(TCHAR *msg);
void printDWORD(DWORD n);
void countThreads(DWORD tid);
void createThreadObj(DWORD tid);
void addBreakPoint(ThreadInfo ti, DWORD address);
void realoadPointer(char *version);
void addBreakPoint(ThreadInfo ti, DWORD64 address);
void removeBreakPoint(ThreadInfo ti);
void debugMainLoop(DWORD64 address);
void printDbgEvent(DEBUG_EVENT dbgEvent);
void elaborateBreakpointData(CONTEXT ctx);

//BOOL firstRun = TRUE;
uint16_t threadsNo;
ThreadInfo *threads;

int main(int argc, char *argv[]) {
    if(argc < 2){
        printf("arg[0] should be the grandma version!\n");
        //DWORD64 asd = 0x14016ade4;
        //printf("0x%llx\n%llu", asd, asd);
        //return 1;
    }
    printf("grandMhAck2 is starting\n");
    if(!setPermissions(true))
        return 1;
    realoadPointer(argv[0]);
    printf("end");
    return 0;
}

void realoadPointer(char *version){
    printf("Reloading dmx buffer pointer\n");
    DWORD pID = getPidByPName(MA2P_NAME);
    printf("\tPid obtained: %lu\n", pID);
    printf("Attaching debugger to process\n");
    DebugActiveProcess(pID);
    DebugSetProcessKillOnExit(false);
    threadsNo = 0;
    doActionsOnThreads(pID, countThreads);
    printf("Total threads found: %u\nObtaining thread info\n", threadsNo);
    //if(!firstRun)
    //  free(threads);
    threads = (ThreadInfo *) malloc(threadsNo * sizeof(ThreadInfo));
    uint16_t tmp = threadsNo;
    doActionsOnThreads(pID, createThreadObj);
    threadsNo = tmp;
    printf("\nGetting correct breakpoint address for version: '%s'\n", version);
    DWORD64 address = getBreakpointAddr(version);
    printf("Setting breakpoint at *0x%llx for threads:", address);
    for(uint16_t i = 0; i < threadsNo; i++)
        if(threads[i].stillValid)
            addBreakPoint(threads[i], address);
    printf("\nEntering debug main loop, waiting breakpoint to be hitted\n");
    debugMainLoop(address);
    printf("Removing breakpoint *0x%llx for threads:", address);
    for(uint16_t i = 0; i < threadsNo; i++)
        if(threads[i].stillValid)
            removeBreakPoint(threads[i]);
    free(threads);
    printf("\nDetaching debugger from process\n");
    DebugActiveProcessStop(pID);
    //firstRun = FALSE;
}

void elaborateBreakpointData(CONTEXT ctx){
    printf("RAX: %d\n", ctx.Rax); // eax get
}

void debugMainLoop(DWORD64 address){
    DEBUG_EVENT dbgEvent;
    BOOL run = TRUE;
    while (run){
        fflush(stdout);
        if (WaitForDebugEvent(&dbgEvent, INFINITE) == 0) {
            printf("\tWaitForDebugEvent returned 0\n");
            break;
        }
        if (dbgEvent.dwDebugEventCode == EXCEPTION_DEBUG_EVENT){
            DWORD tid = dbgEvent.dwThreadId;
            ThreadInfo ti = getThreadInfo(tid);
            //printf("Doneasd\n");
            //fflush(stdout);
            if (dbgEvent.u.Exception.ExceptionRecord.ExceptionCode == EXCEPTION_SINGLE_STEP) {
                if (dbgEvent.u.Exception.ExceptionRecord.ExceptionAddress == (LPVOID)address){
                    printf("Breakpoint *0x%llx has been hitted by thread 0x%06X!\n", address, tid);
                    ti.ctx.ContextFlags = CONTEXT_INTEGER;
                    GetThreadContext(ti.hThread, &ti.ctx);
                    elaborateBreakpointData(ti.ctx);
                    removeBreakPoint(ti);
                    printf(" removed from main loop to prevent other breakpoint hits and debugee's death\n");
                    run = FALSE;
                } else printf("\tNot correct address (*0x%llx)\n", address);
            } else if (dbgEvent.u.Exception.ExceptionRecord.ExceptionCode == EXCEPTION_ACCESS_VIOLATION){
                printf("EXCEPTION_ACCESS_VIOLATION: removing thread 0x%06X\n", tid);
                removeBreakPoint(ti);
                printf(" removed\n");
            } else printf("\tDebugged not SINGLE_STEP or ACCESS_VIOLATION event\n");
        } else printf("\tDebugged not EXCEPTION event\n");
        //printDbgEvent(dbgEvent);
        ContinueDebugEvent(dbgEvent.dwProcessId, dbgEvent.dwThreadId, DBG_CONTINUE);
    }
}

DWORD64 getBreakpointAddr(char *version){
    //DWORD64 addr = NULL;
    DWORD64 addr = 0x4015af; //0x4015cd
    uint16_t i = 0;
    while(i < SUPPORTED_VERSION_NO && !addr){
        if(!stricmp(version, SUPPORTED_VERSION[i].version))
            addr = SUPPORTED_VERSION[i].address;
        i++;
    }
    return addr;
}

void addBreakPoint(ThreadInfo ti, DWORD64 address){
    printf(" 0x%06X", ti.tid);
    ti.ctx.ContextFlags = CONTEXT_DEBUG_REGISTERS;
    ti.ctx.Dr0 = address;
    ti.ctx.Dr7 = 0x00000401;
    if(!SetThreadContext(ti.hThread, &ti.ctx))
        printError(TEXT("Error while setting breakpoint\n"));
}
void removeBreakPoint(ThreadInfo ti){
    printf(" 0x%06X", ti.tid);
    ti.stillValid = FALSE;
    ti.ctx.ContextFlags = CONTEXT_DEBUG_REGISTERS;
    ti.ctx.Dr0 = 0;
    ti.ctx.Dr7 = 0;
    SetThreadContext(ti.hThread, &ti.ctx);
}

DWORD getPidByPName(LPCSTR name){
    DWORD pid = 0;
    BOOL alreadyRunning = TRUE;
    pid = getPid(name);
    while(!pid) {
        pid = getPid(name);
        fflush(stdout); //Sleep is shit and I hate this programming language
        alreadyRunning = FALSE;
        Sleep(1000);
    }
    if(!alreadyRunning){
        printf("%s started after this process. Waiting 2000ms, so it will load threads 'n stuff\n", name);
        fflush(stdout);
        Sleep(2000);
    }
    return pid;
}
DWORD getPid(LPCSTR name){
    printf("Trying to obtain pid for '%s'\n", name);
    PROCESSENTRY32 entry;
    entry.dwSize = sizeof(PROCESSENTRY32);
    HANDLE snapshot = CreateToolhelp32Snapshot(TH32CS_SNAPPROCESS, 0);
    DWORD pid = 0;

    if(Process32First(snapshot, &entry))
        while(Process32Next(snapshot, &entry) && !pid)
            if(!stricmp(entry.szExeFile, name))
                pid = entry.th32ProcessID;

    CloseHandle(snapshot);
    return pid;
}

BOOL doActionsOnThreads(DWORD dwOwnerPID, void (*action)(DWORD)){
    HANDLE hThreadSnap = INVALID_HANDLE_VALUE;
    THREADENTRY32 te32;
    hThreadSnap = CreateToolhelp32Snapshot(TH32CS_SNAPTHREAD, 0);
    if(hThreadSnap == INVALID_HANDLE_VALUE) return FALSE;
    te32.dwSize = sizeof(THREADENTRY32);
    if(!Thread32First(hThreadSnap, &te32)){
        printError(TEXT("Thread32First"));
        CloseHandle(hThreadSnap);
        return FALSE;
    }
    printf("Running action 0x%08X on all threads\n\t", action);
    do {
        if(te32.th32OwnerProcessID == dwOwnerPID){
            DWORD tId = te32.th32ThreadID;
            if(action)
                action(tId);
        }
    } while(Thread32Next(hThreadSnap, &te32));
    //printf("\n");
    CloseHandle(hThreadSnap);
    return TRUE;
}

void printError(TCHAR *msg){
    DWORD eNum;
    TCHAR sysMsg[256];
    TCHAR *p;
    eNum = GetLastError();
    FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
                  NULL, eNum,
                  MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
                  sysMsg, 256, NULL);
    p = sysMsg;
    while((*p > 31) || (*p == 9)) ++p;
    do{
        *p-- = 0;
    } while ((p >= sysMsg) && ((*p == '.') || (*p < 33)));
    _tprintf(TEXT("\n  WARNING: %s failed with error %d (%s)"), msg, eNum, sysMsg);
}

ThreadInfo getThreadInfo(DWORD tid){
    printf("Getting threadinfo obj for 0x%06X\n", tid);
    uint16_t i = 0, result = 0xffff;
    while(i < threadsNo && result == 0xffff){
        //printf("%lu %lu\n", threads[i].tid, tid);
        if(threads[i].tid == tid)
            result = i;
        i++;
    }
    //printf("Done %u < %u\n", result, threadsNo);
    return threads[result];
}
void createThreadObj(DWORD tid){ //ik it's not an object lol
    printf("0x%06X ", tid);
    threads[--threadsNo] = {
        tid,
        {0},
        OpenThread(THREAD_ALL_ACCESS, FALSE, tid),
        TRUE
    };
}
void countThreads(DWORD tid){
    threadsNo++;
}
void printDWORD(DWORD n){
    printf("%lu\n", n);
}

void printDbgEvent(DEBUG_EVENT dbgEvent){
    printf("Tid: 0x%06X\n", dbgEvent.dwThreadId);
    printf("Code: %lu\t%B\n", dbgEvent.dwDebugEventCode, dbgEvent.dwDebugEventCode == EXCEPTION_DEBUG_EVENT);
    printf("Exception: %llx\n", dbgEvent.u.Exception.ExceptionRecord.ExceptionCode);
    printf("Record: %llx\n", dbgEvent.u.Exception.ExceptionRecord.ExceptionRecord);
    printf("Flags: %lu\n", dbgEvent.u.Exception.ExceptionRecord.ExceptionFlags);
    printf("Address: %llx\n", dbgEvent.u.Exception.ExceptionRecord.ExceptionAddress);
    printf("Parameters NO: %lu\n", dbgEvent.u.Exception.ExceptionRecord.NumberParameters);
    for(DWORD i = 0; i < 3; i++)
        printf("ExceptionInformation[%lu]: 0x%lx\n", i, dbgEvent.u.Exception.ExceptionRecord.ExceptionInformation[i]);
    printf("\n");
}

BOOL setPermissions(BOOL enable){
    printf("Obtaining permissions\n");
    HANDLE hToken;
    if(!OpenProcessToken(GetCurrentProcess(), TOKEN_ADJUST_PRIVILEGES, &hToken)) {
        printError(TEXT("Error opening current process"));
        return FALSE;
    }
    TOKEN_PRIVILEGES tp;
    LUID luid;
    if(!LookupPrivilegeValue(NULL, SE_DEBUG_NAME, &luid)){
        printf("LookupPrivilegeValue error: %u\n", GetLastError());
        return FALSE;
    }
    tp.PrivilegeCount = 1;
    tp.Privileges[0].Luid = luid;
    if(enable)
        tp.Privileges[0].Attributes = SE_PRIVILEGE_ENABLED;
    else
        tp.Privileges[0].Attributes = 0;
    if(!AdjustTokenPrivileges(hToken, FALSE, &tp, sizeof(TOKEN_PRIVILEGES), (PTOKEN_PRIVILEGES) NULL, (PDWORD) NULL)){
        printError(TEXT("AdjustTokenPrivileges error\n"));
        return FALSE;
    }
    if(GetLastError() == ERROR_NOT_ALL_ASSIGNED) {
        printf("ERROR: The token does not have the specified privilege.\n");
        return FALSE;
    }
    CloseHandle(hToken);
    return TRUE;
}