#include "main.h"

USHORT B2LW(USHORT num)
{
    return ((num&0xFF)<<8) | ((num&0xFF00)>>8);
}

UINT B2L(UINT num)
{
    return ((num>>24)&0xFF) | ((num<<8)&0xFF0000) | ((num>>8)&0xFF00) | ((num<<24)&0xFF000000);
}

void rotateVec(double *x, double *y, double a)
{
    double oldX = *x;
    *x = *x * cos(a) - *y * sin(a);
    *y = oldX * sin(a) + *y * cos(a);
}

double atof_s(char *str)
{
    UINT len = 0;
    char *pstr = str;
    for (; isdigit(*pstr) || *pstr=='.' || *pstr=='+'
        || *pstr=='-'; *pstr++, len++);
    if (len > 32) return 0.0f;
    char *safe = (char*)malloc(len + 1);
    memset(safe, 0, len + 1);
    pstr = str; memcpy(safe, pstr, len);
    double res = atof(safe);
    free((void*)safe);
    return res;
}

DWORD prng_step()
{
    static DWORD prng_lfsr = 0x9D0A31FC;
    prng_lfsr = (prng_lfsr >> 1) ^ (UINT)(0 - (prng_lfsr & 1) & 0xD0000001);
    return prng_lfsr;
}

UINT mapWidth = 0, mapHeight = 0; BYTE *world = 0;
#define WORLD(x,y) (world[x+(y*mapWidth)])
BYTE *texs[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

BYTE *fb = 0, *pb = 0;
UINT width = 640, height = 480, bpp = 32;
UINT pmultx = 0, pmulty = 0;

BYTE NETWORK_IPADDR[4] = {10, 10, 0, 86};
BYTE NETWORK_MACADDR[6] = {0x00, 0x50, 0xF2, 0xFB, 0x1F, 0xB1};

void PutPixel(UINT x, UINT y, DWORD color)
{
    pb[(y*pmulty)+(x*pmultx)] = GetB(color); //b
    pb[(y*pmulty)+(x*pmultx)+1] = GetG(color); //g
    pb[(y*pmulty)+(x*pmultx)+2] = GetR(color); //r
}

void XBoxStartup()
{
    XVideoSetMode(width, height, bpp, 60);
    XVideoSetSoftenFilter(0);
    XVideoSetFlickerFilter(0);

    Pktdrv_Init();
    Pktdrv_GetEthernetAddr(NETWORK_MACADDR);

    /*Open World*/
    int worldfh = 0; UINT spawni = 0; WORLDNINFO worldinfo;
    XCreateFile(&worldfh, "d:\\worlds\\test.bin", GENERIC_READ,
        FILE_SHARE_READ, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL);
    XReadFile(worldfh, &worldinfo, sizeof(WORLDNINFO), 0);

    worldinfo.magic = (DWORD)B2L((UINT)worldinfo.magic);
    if (worldinfo.magic != WORLDINFO_MAGIC) {
        XCloseHandle(worldfh);
        HalReturnToFirmware(ReturnFirmwareQuickReboot);
    }
    debugPrint("    World Magic: %08X\n", worldinfo.magic);
    debugPrint("    World Name: %s\n", worldinfo.name);
    debugPrint("    PRNG value: %08X\n", prng_step());

    worldinfo.width = (UINT)B2L((UINT)worldinfo.width);
    worldinfo.height = (UINT)B2L((UINT)worldinfo.height);
    worldinfo.mapoff = (DWORD)B2L((UINT)worldinfo.mapoff);
    mapWidth = worldinfo.width; mapHeight = worldinfo.height;
    debugPrint("    World Size: %d, %d\n", worldinfo.width, worldinfo.height);

    worldinfo.t1_spawncnt = (UINT)B2L((UINT)worldinfo.t1_spawncnt);
    worldinfo.t1_spawnoff = (DWORD)B2L((UINT)worldinfo.t1_spawnoff);
    SPAWNSTR *t1_spawnstrs = (SPAWNSTR*)malloc(worldinfo.t1_spawncnt * sizeof(SPAWNSTR));
    XSetFilePointer(worldfh, worldinfo.t1_spawnoff, 0, FILE_BEGIN);
    XReadFile(worldfh, t1_spawnstrs, worldinfo.t1_spawncnt * sizeof(SPAWNSTR), 0);
    SPAWN *t1_spawns = (SPAWN*)malloc(worldinfo.t1_spawncnt * sizeof(SPAWN));
    for (spawni = 0; spawni < worldinfo.t1_spawncnt; spawni++)
    {
        t1_spawns[spawni].pX = atof_s(t1_spawnstrs[spawni].pX);
        t1_spawns[spawni].pY = atof_s(t1_spawnstrs[spawni].pY);
        t1_spawns[spawni].dX = atof_s(t1_spawnstrs[spawni].dX);
        t1_spawns[spawni].dY = atof_s(t1_spawnstrs[spawni].dY);
    }
    free(t1_spawnstrs);

    worldinfo.t2_spawncnt = (UINT)B2L((UINT)worldinfo.t2_spawncnt);
    worldinfo.t2_spawnoff = (DWORD)B2L((UINT)worldinfo.t2_spawnoff);
    SPAWNSTR *t2_spawnstrs = (SPAWNSTR*)malloc(worldinfo.t2_spawncnt * sizeof(SPAWNSTR));
    XSetFilePointer(worldfh, worldinfo.t2_spawnoff, 0, FILE_BEGIN);
    XReadFile(worldfh, t2_spawnstrs, worldinfo.t2_spawncnt * sizeof(SPAWNSTR), 0);
    SPAWN *t2_spawns = (SPAWN*)malloc(worldinfo.t2_spawncnt * sizeof(SPAWN));
    for (spawni = 0; spawni < worldinfo.t2_spawncnt; spawni++)
    {
        t2_spawns[spawni].pX = atof_s(t2_spawnstrs[spawni].pX);
        t2_spawns[spawni].pY = atof_s(t2_spawnstrs[spawni].pY);
        t2_spawns[spawni].dX = atof_s(t2_spawnstrs[spawni].dX);
        t2_spawns[spawni].dY = atof_s(t2_spawnstrs[spawni].dY);
    }
    free(t1_spawnstrs);

    world = (BYTE*)malloc(worldinfo.width * worldinfo.height);
    memset((void*)world, 0, worldinfo.width * worldinfo.height);
    XSetFilePointer(worldfh, worldinfo.mapoff, 0, FILE_BEGIN);
    XReadFile(worldfh, world, worldinfo.width * worldinfo.height, 0);
    UINT dbgX = 0, dbgY = 0;
    for (dbgY = 0; dbgY < worldinfo.height; dbgY++) {
        debugPrint("    ");
        for (dbgX = 0; dbgX < worldinfo.width; dbgX++)
            debugPrint("%01d", WORLD(dbgX, dbgY));
        debugPrint("\n");
    }
    XCloseHandle(worldfh);
    /*Open World*/

    XInput_Init();

    pmultx = bpp / 8;
    pmulty = width * (bpp / 8);
    fb = XVideoGetFB();
    pb = (BYTE*)malloc(width * height * pmultx);

    int x, y; BYTE reverseRot = 0;
    double posX = t1_spawns[0].pX, posY = t1_spawns[0].pY;  //x and y start position
    double dirX = t1_spawns[0].dX, dirY = t1_spawns[0].dY; //initial direction vector
    if (dirX < 0) reverseRot = 1; else reverseRot = 0;
    double planeX = atof_s(worldinfo.planeX); //the 2d raycaster version of camera plane
    double planeY = atof_s(worldinfo.planeY); //x = 0.00, y = 0.66 default
    double frameTime = 0, lastTickCount = 0;
    double rotSpeed = 0, moveSpeed = 0;

    GAME_SNAPSHOT _snapshot;
    memset((void*)&_snapshot, 0, sizeof(GAME_SNAPSHOT));
    GAME_SNAPSHOT *snapshot = (GAME_SNAPSHOT*)&_snapshot;
    snapshot->PacketNum = 0;
    snapshot->team = ((prng_step() >> 19) & 0x1);
    debugPrint("    TEAM: [%d] %s\n",
        snapshot->team, (snapshot->team)?"BLUE":"RED");
    sprintf(snapshot->name, "%x", prng_step());
    debugPrint("    NANE: %s\n", snapshot->name);
    snapshot->pX = posX; snapshot->pY = posY;
    snapshot->dX = dirX; snapshot->dY = dirY;
    snapshot->planeX = planeX; snapshot->planeY = planeY;
    switch (snapshot->team) {
        case 0: snapshot->color = RGB(255,0,0); break;
        case 1: snapshot->color = RGB(0,255,0); break;
    }
    XInput_GetEvents();
    snapshot->btns = g_DefaultPad.CurrentButtons;
    NetworkSendSnapshot(snapshot);

    debugPrint("    Press START to being.\n");
    while ((g_DefaultPad.PressedButtons.usDigitalButtons & XPAD_START)==0) {
        XInput_GetEvents(); prng_step();
    }

    int yoffset = 0;
    BYTE running = 1;
    while (running == 1)
    {
        /*Handle Input*/
        XInput_GetEvents();
        while (g_DefaultPad.CurrentButtons.usDigitalButtons&XPAD_DPAD_DOWN)
            XInput_GetEvents();
        snapshot->btns = g_DefaultPad.CurrentButtons;
        if (g_DefaultPad.PressedButtons.ucAnalogButtons[XPAD_WHITE]) {
            running = 0;
        }
        if (g_DefaultPad.PressedButtons.ucAnalogButtons[XPAD_BLACK]) {
            SPAWN *spawn = 0;
            if (snapshot->team == 0) spawn = &t1_spawns[(prng_step() >> 4) & 0x1];
            else if (snapshot->team == 1) spawn = &t2_spawns[(prng_step() >> 14) & 0x1];
            posX = spawn->pX; posY = spawn->pY;
            dirX = spawn->dX; dirY = spawn->dY;
            planeX = atof_s(worldinfo.planeX);
            planeY = atof_s(worldinfo.planeY);
            if (dirX < 0) reverseRot = 1; else reverseRot = 0;
            snapshot->pX = posX; snapshot->pY = posY;
            snapshot->dX = dirX; snapshot->dY = dirY;
            snapshot->planeX = planeX; snapshot->planeY = planeY;
        }
        if (g_DefaultPad.CurrentButtons.usDigitalButtons & XPAD_LEFT_THUMB) {
            moveSpeed *= 1.5;
        }
        if (g_DefaultPad.CurrentButtons.usDigitalButtons & XPAD_RIGHT_THUMB) {
            rotSpeed *= 1.25;
        }
        if (XPAD_AXISINUP(g_DefaultPad.sLThumbY)) { //MOVE FORWARD
            if(WORLD((int)(posX + dirX * moveSpeed), (int)(posY)) == 0) posX += dirX * moveSpeed;
            if(WORLD((int)(posX), (int)(posY + dirY * moveSpeed)) == 0) posY += dirY * moveSpeed;
            snapshot->pX = posX; snapshot->pY = posY;
        }
        if (XPAD_AXISINDOWN(g_DefaultPad.sLThumbY)) { //MOVE BACKWARD
            if(WORLD((int)(posX - dirX * moveSpeed), (int)(posY)) == 0) posX -= dirX * moveSpeed;
            if(WORLD((int)(posX), (int)(posY - dirY * moveSpeed)) == 0) posY -= dirY * moveSpeed;
            snapshot->pX = posX; snapshot->pY = posY;
        }
        if (XPAD_AXISINLEFT(g_DefaultPad.sLThumbX)) { //STRAFE LEFT
            if(WORLD((int)(posX - planeX * moveSpeed), (int)(posY)) == 0) posX -= planeX  * moveSpeed;
            if(WORLD((int)(posX), (int)(posY - planeY * moveSpeed)) == 0) posY -= planeY * moveSpeed;
            snapshot->pX = posX; snapshot->pY = posY;
        }
        if (XPAD_AXISINRIGHT(g_DefaultPad.sLThumbX)) { //STRAFE RIGHT
            if(WORLD((int)(posX + planeX * moveSpeed), (int)(posY)) == 0) posX += planeX * moveSpeed;
            if(WORLD((int)(posX), (int)(posY + planeY* moveSpeed)) == 0) posY += planeY * moveSpeed;
            snapshot->pX = posX; snapshot->pY = posY;
        }
        if (reverseRot?XPAD_AXISINRIGHT(g_DefaultPad.sRThumbX):XPAD_AXISINLEFT(g_DefaultPad.sRThumbX)) { //ROTATE LEFT
            rotateVec(&dirX, &dirY, -rotSpeed);
            rotateVec(&planeX, &planeY, -rotSpeed);
            snapshot->dX = dirX; snapshot->dY = dirY;
            snapshot->planeX = planeX; snapshot->planeY = planeY;
        }
        if (reverseRot?XPAD_AXISINLEFT(g_DefaultPad.sRThumbX):XPAD_AXISINRIGHT(g_DefaultPad.sRThumbX)) { //ROTATE RIGHT
            rotateVec(&dirX, &dirY, rotSpeed);
            rotateVec(&planeX, &planeY, rotSpeed);
            snapshot->planeX = planeX; snapshot->planeY = planeY;
        }

        /* Networking */
        NetworkSendSnapshot(snapshot);
        Pktdrv_ReceivePackets();

        /*Render/
        for (x = 0; x < width; x++)
        {
            //calculate ray position and direction
            double cameraX = 2 * x / (double)width - 1; //x-coordinate in camera space
            double rayPosX = posX;
            double rayPosY = posY;
            double rayDirX = dirX + planeX * cameraX;
            double rayDirY = dirY + planeY * cameraX;

            //which box of the map we're in
            int mapX = (int)rayPosX;
            int mapY = (int)rayPosY;

            //length of ray from current position to next x or y-side
            double sideDistX;
            double sideDistY;

            //length of ray from one x or y-side to next x or y-side
            double deltaDistX = sqrt(1 + (rayDirY * rayDirY) / (rayDirX * rayDirX));
            double deltaDistY = sqrt(1 + (rayDirX * rayDirX) / (rayDirY * rayDirY));
            double perpWallDist;

            //what direction to step in x or y-direction (either +1 or -1)
            int stepX;
            int stepY;

            int hit = 0; //was there a wall hit?
            int side; //was a NS or a EW wall hit?

            //calculate step and initial sideDist
            if (rayDirX < 0) {
                stepX = -1;
                sideDistX = (rayPosX - mapX) * deltaDistX;
            } else {
                stepX = 1;
                sideDistX = (mapX + 1.0 - rayPosX) * deltaDistX;
            }
            if (rayDirY < 0) {
                stepY = -1;
                sideDistY = (rayPosY - mapY) * deltaDistY;
            } else {
                stepY = 1;
                sideDistY = (mapY + 1.0 - rayPosY) * deltaDistY;
            }

            //perform DDA
            while (hit == 0)
            {
                //jump to next map square, OR in x-direction, OR in y-direction
                if (sideDistX < sideDistY) {
                    sideDistX += deltaDistX;
                    mapX += stepX;
                    side = 0;
                } else {
                    sideDistY += deltaDistY;
                    mapY += stepY;
                    side = 1;
                }
                //Check if ray has hit a wall
                if (WORLD(mapX, mapY) > 0) hit = 1;
            }

            //Calculate distance projected on camera direction (oblique distance will give fisheye effect!)
            if (side == 0) perpWallDist = fabs((mapX - rayPosX + (1 - stepX) / 2) / rayDirX);
            else perpWallDist = fabs((mapY - rayPosY + (1 - stepY) / 2) / rayDirY);

            //Calculate height of line to draw on screen
            int lineHeight = abs((int)(height / perpWallDist));

            //calculate lowest and highest pixel to fill in current stripe
            int drawStart = -lineHeight / 2 + height / 2;
            if(drawStart < 0)drawStart = 0;
            int drawEnd = lineHeight / 2 + height / 2;
            if(drawEnd >= height)drawEnd = height - 1;
            drawStart += yoffset; drawEnd += yoffset;

            //choose wall color
            DWORD color = 0;
            switch(WORLD(mapX, mapY))
            {
                case 1: color = RGB(255, 0, 0); break; //RED
                case 2: color = RGB(0, 255, 0); break; //GREEN
                case 3: color = RGB(0, 0, 255); break; //BLUE
                case 4: color = RGB(255, 255, 255); break; //WHITE
                default: color = RGB(255, 255, 0); break; //YELLOW
            }

            //give x and y sides different brightness
            if (side == 1) {
                BYTE r = GetR(color), g = GetG(color), b = GetB(color);
                if (r >= 255) r -= 16; if (r >= 128) r -= 32; if (r >= 64) r -= 12;
                if (r >= 32) r -= 8; if (r >= 16) r -= 6; if (r >= 8) r -= 2;
                if (g >= 255) g -= 16; if (g >= 128) g -= 32; if (g >= 64) g -= 12;
                if (g >= 32) g -= 8; if (g >= 16) g -= 6; if (g >= 8) g -= 2;
                if (b >= 255) b -= 16; if (b >= 128) b -= 32; if (b >= 64) b -= 12;
                if (b >= 32) b -= 8; if (b >= 16) b -= 6; if (b >= 8) b -= 2;
                color = RGB(r,g,b);
            }

            //draw the pixels of the stripe as a vertical line
            int tmpy;
            for (tmpy=0; tmpy<drawStart; tmpy++) PutPixel(x, tmpy, RGB(0, 191, 255));
            for (tmpy=drawStart; tmpy<drawEnd; tmpy++) PutPixel(x, tmpy, color);
            for (tmpy=drawEnd; tmpy<height; tmpy++) PutPixel(x, tmpy, RGB(100, 100, 100));
        }
        memcpy((void*)fb, (void*)pb, width*height*pmultx);

        /*FPS (skip time to benchmark)*/
        frameTime = XGetTickCount() - lastTickCount;
        lastTickCount = XGetTickCount();
        if (frameTime < 10) frameTime = 10;
        moveSpeed = frameTime * 0.005f;
        rotSpeed = frameTime * 0.0030f;
    }

    snapshot->team = TEAM_QUIT;
    NetworkSendSnapshot(snapshot);

    free((void*)pb);
    free((void*)world);
    XInput_Quit();
    Pktdrv_Quit();

    HalReturnToFirmware(ReturnFirmwareQuickReboot);
}

void NetworkSendSnapshot(GAME_SNAPSHOT *ss)
{
    //debugPrint("    [SS] Send snapshot #%d\n", ss->PacketNum);
    ss->PacketNum++;
}

int Pktdrv_Callback(unsigned char *packet, unsigned int sz)
{
    char pktbuffer[4096];
    memset(pktbuffer, 0, 4096);
    memcpy(pktbuffer, packet, (sz<4096)?sz:4096);

    ETH *ethhdr = (ETH*)pktbuffer;
    debugPrint("    [RECV PACKET]\n");
    debugPrint("        Target MAC: %02X%02X%02X%02X%02X%02X\n",
        ethhdr->EthDst[0], ethhdr->EthDst[1],
        ethhdr->EthDst[2], ethhdr->EthDst[3],
        ethhdr->EthDst[4], ethhdr->EthDst[5]);
    debugPrint("        Sender MAC: %02X%02X%02X%02X%02X%02X\n",
        ethhdr->EthSrc[0], ethhdr->EthSrc[1],
        ethhdr->EthSrc[2], ethhdr->EthSrc[3],
        ethhdr->EthSrc[4], ethhdr->EthSrc[5]);
    debugPrint("        ProtocolType: 0x%04X\n", ethhdr->ProtocolType);

    ARP *arphdr = (ARP*)pktbuffer;
    if (arphdr->ProtocolType==0x0608 && arphdr->Operation==0x0100)
    {
        DWORD dstip = *((DWORD*)arphdr->TargetProtocolAddr);
        DWORD myip = *((DWORD*)&NETWORK_IPADDR[0]);
        if (dstip == myip)
        {
            memcpy(arphdr->EthDst, arphdr->EthSrc, 6);
            memcpy(arphdr->EthSrc, NETWORK_MACADDR, 6);
            arphdr->Operation = 0x0200;
            memcpy(arphdr->TargetHardwareAddr, arphdr->SenderHardwareAddr, 6);
            memcpy(arphdr->SenderHardwareAddr, NETWORK_MACADDR, 6);
            memcpy(arphdr->TargetProtocolAddr, arphdr->SenderProtocolAddr, 4);
            memcpy(arphdr->SenderProtocolAddr, NETWORK_IPADDR, 4);
            debugPrint("    [ARP] Answering with:\n");
            debugPrint("        Target MAC: %02X%02X%02X%02X%02X%02X\n",
                arphdr->TargetHardwareAddr[0], arphdr->TargetHardwareAddr[1],
                arphdr->TargetHardwareAddr[2], arphdr->TargetHardwareAddr[3],
                arphdr->TargetHardwareAddr[4], arphdr->TargetHardwareAddr[5]);
            debugPrint("        Sender MAC: %02X%02X%02X%02X%02X%02X\n",
                arphdr->SenderHardwareAddr[0], arphdr->SenderHardwareAddr[1],
                arphdr->SenderHardwareAddr[2], arphdr->SenderHardwareAddr[3],
                arphdr->SenderHardwareAddr[4], arphdr->SenderHardwareAddr[5]);
            debugPrint("        Target IP: %d.%d.%d.%d\n",
                arphdr->TargetProtocolAddr[0], arphdr->TargetProtocolAddr[1],
                arphdr->TargetProtocolAddr[2], arphdr->TargetProtocolAddr[3]);
            debugPrint("        Sender IP: %d.%d.%d.%d\n",
                arphdr->SenderProtocolAddr[0], arphdr->SenderProtocolAddr[1],
                arphdr->SenderProtocolAddr[2], arphdr->SenderProtocolAddr[3]);
            while (Pktdrv_GetQueuedTxPkts() >= 1);
            Pktdrv_SendPacket(pktbuffer, sizeof(ARP));
        }
    } else if (arphdr->ProtocolType==0x0608) {
            debugPrint("    [ARP] Operation 0x%04X\n", arphdr->Operation);
    }

    IP *iphdr = (IP*)pktbuffer;
    if (iphdr->ProtocolType == 0x0008)
    {
        debugPrint("    [IPV4] Received\n");
        debugPrint("        Protocol: %d\n", iphdr->Protocol);
        debugPrint("        Scr IP: %d.%d.%d.%d\n",
            iphdr->SrcIPAddr[0], iphdr->SrcIPAddr[1],
            iphdr->SrcIPAddr[2], iphdr->SrcIPAddr[3]);
        debugPrint("        Dst IP: %d.%d.%d.%d\n",
            iphdr->DstIPAddr[0], iphdr->DstIPAddr[1],
            iphdr->DstIPAddr[2], iphdr->DstIPAddr[3]);
        if (iphdr->Protocol == 17)
        {
            DWORD dstip = *((DWORD*)iphdr->DstIPAddr);
            DWORD myip = *((DWORD*)&NETWORK_IPADDR[0]);
            if (dstip == myip)
            {
                UDP *udp = (UDP*)pktbuffer;
                udp->SrcPort = B2LW(udp->SrcPort);
                udp->DstPort = B2LW(udp->DstPort);
                udp->Length = B2LW(udp->Length);
                debugPrint("    [UDP] Packed Received\n");
                debugPrint("        Src port: %d", udp->SrcPort);
                debugPrint("        Dst port: %d", udp->DstPort);
                debugPrint("        Length: %d", udp->Length);
            }
        }
        else if (iphdr->Protocol == 1)
        {
            debugPrint("    [PING] Received\n");
        }
    }

    return 1; //0 to keep
}