#include <3ds.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "screen_dump_1bpp_bin.h"
#include "screen_dump_4bpp_bin.h"
#include "screen_dump_8bpp_bin.h"

#include "screen_dump_4bpp_pal_bin.h"
#include "screen_dump_8bpp_pal_bin.h"

#define TimesToRun 100

u32 Keys_Held = 0;
u32 Keys_Down = 0;
u32 Keys_Up = 0;

static u32 FBConvTable[ 256 ][ 8 ];
static u32 ScreenOut[ 512 * 512 * 4 ];

static u16 CLUT_reds[ 256 ];
static u16 CLUT_greens[ 256 ];
static u16 CLUT_blues[ 256 ];

s32 MainThreadPriority = 0;

void LoadCLUT4BPP( void ) {
    int i = 0;

    for ( i = 0; i < 16; i++ ) {
        CLUT_reds[ i ] = screen_dump_4bpp_pal_bin[ ( i * 3 ) + 0 ] << 8;
        CLUT_greens[ i ] = screen_dump_4bpp_pal_bin[ ( i * 3 ) + 1 ] << 8;
        CLUT_blues[ i ] = screen_dump_4bpp_pal_bin[ ( i * 3 ) + 2 ] << 8;
    }
}

void LoadCLUT8BPP( void ) {
    int i = 0;

    for ( i = 0; i < 256; i++ ) {
        CLUT_reds[ i ] = screen_dump_8bpp_pal_bin[ ( i * 3 ) + 0 ] << 8;
        CLUT_greens[ i ] = screen_dump_8bpp_pal_bin[ ( i * 3 ) + 1 ] << 8;
        CLUT_blues[ i ] = screen_dump_8bpp_pal_bin[ ( i * 3 ) + 2 ] << 8;
    }
}

void MakeTable1BPP( void ) {
    int i = 0;
    int j = 0;

    for ( i = 0; i < 256; i++ ) {
        for ( j = 0; j <= 7; j++ ) {
            FBConvTable[ i ][ j ] = ( i & BIT( j ) ) ? 0xFF : 0xFFFFFFFF;
        }
    }
}

void MakeTable4BPP( void ) {
    int r, g, b = 0;
    int l, h = 0;
    int i = 0;

    LoadCLUT4BPP( );

    for ( i = 0; i < 256; i++ ) {
        l = ( i >> 4 );
        h = ( i & 0x0F );

        r = ( CLUT_reds[ l ] >> 8 ) & 0xFF;
        g = ( CLUT_greens[ l ] >> 8 ) & 0xFF;
        b = ( CLUT_blues[ l ] >> 8 ) & 0xFF;

        FBConvTable[ i ][ 0 ] = ( r << 24 ) | ( g << 16 ) | ( b << 8 ) | 0xFF;

        r = ( CLUT_reds[ h ] >> 8 ) & 0xFF;
        g = ( CLUT_greens[ h ] >> 8 ) & 0xFF;
        b = ( CLUT_blues[ h ] >> 8 ) & 0xFF;

        FBConvTable[ i ][ 1 ] = ( r << 24 ) | ( g << 16 ) | ( b << 8 ) | 0xFF;
    }
}

void MakeTable8BPP( void ) {
    int i = 0;

    LoadCLUT8BPP( );

    for ( i = 0; i < 256; i++ ) {
        FBConvTable[ i ][ 0 ] = \
        ( ( ( CLUT_reds[ i ] >> 8 ) & 0xFF ) << 24 ) | \
        ( ( ( CLUT_greens[ i ] >> 8 ) & 0xFF ) << 16 ) | \
        ( ( ( CLUT_blues[ i ] >> 8 ) & 0xFF ) << 8 ) | \
        0xFF;
    }
}

struct ConvertStruct {
    u8* Src;
    u32* Dest;
    int Size;
};

void Convert1BPP_8BPP( u8* Src, u32* Dest, int SrcSizeInBytes ) {
    while ( SrcSizeInBytes-- ) {
        memcpy( Dest, &FBConvTable[ *Src ], 8 * sizeof( u32 ) );

        Dest+= 8;
        Src++;
    }
}

void ConvertIRGB4_To_RGBA8( u8* Src, u32* Dest, int SrcSizeInBytes ) {
    while ( SrcSizeInBytes-- ) {
        *Dest++ = FBConvTable[ *Src ][ 0 ];
        *Dest++ = FBConvTable[ *Src++ ][ 1 ];
    }
}

void Convert4BPPThread( void* Param ) {
    struct ConvertStruct* CS = ( struct ConvertStruct* ) Param;

    ConvertIRGB4_To_RGBA8( CS->Src, CS->Dest, CS->Size );
}

int IRGB4_To_RGBA8( u8* Src, u32* Dest, int Size ) {
    struct ConvertStruct CS;
    Thread T = NULL;

    CS.Src = Src;
    CS.Dest = Dest;
    CS.Size = Size / 2;

    T = threadCreate( Convert4BPPThread, ( void* ) &CS, 4096, MainThreadPriority - 2, 1, false );

    ConvertIRGB4_To_RGBA8( &Src[ Size / 2 ], &Dest[ Size / 8 ], Size / 2 );

    if ( T != NULL ) {
        threadJoin( T, UINT64_MAX );
        threadFree( T );
    } else {
        printf( "threadCreate: Error\n" );
    }

    return T == NULL ? 0 : 1;
}

void ConvertIRGB8_To_RGBA8( u8* Src, u32* Dest, int SrcSizeInBytes ) {
    while ( SrcSizeInBytes-- ) {
        *Dest++ = FBConvTable[ *Src++ ][ 0 ];
    }
}

void Convert8BPPThread( void* Param ) {
    struct ConvertStruct* CS = ( struct ConvertStruct* ) Param;

    ConvertIRGB8_To_RGBA8( CS->Src, CS->Dest, CS->Size );
}

/*
 * Converts an indexed RGB888 image to an RGBA888 image and
 * splits the workload between syscore and appcore.
 */
int IRGB8_To_RGBA8( u8* Src, u32* Dest, int Size ) {
    struct ConvertStruct CS;
    Thread T = NULL;

    /* Set up the params for the syscore to use the 2nd
     * half of the buffer.
     */
    CS.Src = Src;
    CS.Dest = Dest;
    CS.Size = Size / 2;

    /* Create a syscore thread */
    T = threadCreate( Convert8BPPThread, ( void* ) &CS, 4096, MainThreadPriority - 1, 1, false );

    /* While syscore is running the thread, run this normally for
     * the first half of the buffer.
     */
    ConvertIRGB8_To_RGBA8( &Src[ Size / 2 ], &Dest[ Size / 8 ], Size / 2 );

    if ( T != NULL ) {
        /* Wait for syscore thread to finish */
        threadJoin( T, U64_MAX );
        threadFree( T );
    } else {
        printf( "threadCreate: Error\n" );
    }

    return T == NULL ? 0 : 1;
}

void Test1BPPSpeed( void ) {
    u64 StartTime = 0;
    u64 EndTime = 0;
    int i = 0;

    MakeTable1BPP( );

    StartTime = osGetTime( );

    for ( i = 0; i < TimesToRun; i++ ) {
        Convert1BPP_8BPP( ( u8* ) screen_dump_1bpp_bin, ScreenOut, screen_dump_1bpp_bin_size );
    }

    EndTime = osGetTime( );

    printf( "1BPP Conversion took %.1fms\n", ( ( float ) ( EndTime - StartTime ) ) / ( ( float ) TimesToRun ) );
}

void Test4BPPSpeed( void ) {
    u64 StartTime = 0;
    u64 EndTime = 0;
    int i = 0;

    MakeTable4BPP( );

    StartTime = osGetTime( );

    for ( i = 0; i < TimesToRun; i++ ) {
        if ( IRGB4_To_RGBA8( ( u8* ) screen_dump_4bpp_bin, ScreenOut, screen_dump_4bpp_bin_size ) == 0 )
            break;
    }

    EndTime = osGetTime( );

    printf( "4BPP Conversion took %.1fms\n", ( ( float ) ( EndTime - StartTime ) ) / ( ( float ) TimesToRun ) );
}

void Test8BPPSpeed( void ) {
    u64 StartTime = 0;
    u64 EndTime = 0;
    int i = 0;

    MakeTable8BPP( );

    StartTime = osGetTime( );

    for ( i = 0; i < TimesToRun; i++ ) {
        if ( IRGB8_To_RGBA8( ( u8* ) screen_dump_8bpp_bin, ScreenOut, screen_dump_8bpp_bin_size ) == 0 )
            break;
    }

    EndTime = osGetTime( );

    printf( "8BPP Conversion took %.1fms\n", ( ( float ) ( EndTime - StartTime ) ) / ( ( float ) TimesToRun ) );
}

int main( void ) {
    gfxInitDefault( );
    consoleInit( GFX_TOP, NULL );

    printf( "Ready...\n" );

    svcGetThreadPriority( &MainThreadPriority, CUR_THREAD_HANDLE );
    APT_SetAppCpuTimeLimit( 80 );

    Test1BPPSpeed( );
    Test4BPPSpeed( );
    Test8BPPSpeed( );

    while ( aptMainLoop( ) ) {
        hidScanInput( );

        Keys_Held = hidKeysHeld( );
        Keys_Down = hidKeysDown( );
        Keys_Up = hidKeysUp( );

        if ( Keys_Down & KEY_START )
            break;

        gfxSwapBuffers( );
        gfxFlushBuffers( );
        gspWaitForVBlank( );
    }

    gfxExit( );

    return 0;
}