#include <cassert>
#include <cmath>
#include <bcm_host.h>
#include <GLES/gl.h>
#include <GLES/glext.h>
#include <EGL/egl.h>
//#include <EGL/eglext.h>
#include "Rpi.h"

static inline unsigned get_alignment(unsigned pitch)
{
   if (pitch & 1)
      return 1;
   if (pitch & 2)
      return 2;
   if (pitch & 4)
      return 4;
   return 8;
}

static const GLfloat default_vertices[] = {
   0, 0,
   0, 1,
   1, 1,
   1, 0
};

Rpi::Rpi(const rarch_video_info_t *info):
    mTextureWidth(0),
    mTextureHeight(0),
    mRenderWidth(0),
    mRenderHeight(0)
{
    int32_t success;
    EGLBoolean result;
    EGLint num_config;

    static EGL_DISPMANX_WINDOW_T nativewindow;

    DISPMANX_ELEMENT_HANDLE_T dispman_element;
    DISPMANX_DISPLAY_HANDLE_T dispman_display;
    DISPMANX_UPDATE_HANDLE_T dispman_update;
    DISPMANX_MODEINFO_T dispman_modeinfo;
    VC_RECT_T dst_rect;
    VC_RECT_T src_rect;

    static const EGLint attribute_list[] =
    {
        EGL_RED_SIZE, 8,
        EGL_GREEN_SIZE, 8,
        EGL_BLUE_SIZE, 8,
        EGL_ALPHA_SIZE, 8,
        EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
        EGL_NONE
    };

    EGLConfig config;

    // get an EGL display connection
    mDisplay = eglGetDisplay(EGL_DEFAULT_DISPLAY);
    assert(mDisplay != EGL_NO_DISPLAY);

    // initialize the EGL display connection
    result = eglInitialize(mDisplay, nullptr, nullptr);
    assert(result != EGL_FALSE);

    // get an appropriate EGL frame buffer configuration
    result = eglChooseConfig(mDisplay, attribute_list, &config, 1, &num_config);
    assert(result != EGL_FALSE);

    // create an EGL rendering context
    mContext = eglCreateContext(mDisplay, config, EGL_NO_CONTEXT, nullptr);
    assert(mContext != EGL_NO_CONTEXT);

    // create an EGL window surface
    success = graphics_get_display_size(0 /* LCD */, &mScreenWidth, &mScreenHeight);
    assert(success >= 0);

    dst_rect.x = 0;
    dst_rect.y = 0;
    dst_rect.width = mScreenWidth;
    dst_rect.height = mScreenHeight;

    src_rect.x = 0;
    src_rect.y = 0;
    src_rect.width = mScreenWidth << 16;
    src_rect.height = mScreenHeight << 16;

    dispman_display = vc_dispmanx_display_open(0 /* LCD */);
    dispman_update = vc_dispmanx_update_start(0);

    vc_dispmanx_display_get_info(dispman_display, &dispman_modeinfo);

    dispman_element = vc_dispmanx_element_add ( dispman_update, dispman_display,
        0/*layer*/, &dst_rect, 0/*src*/,
        &src_rect, DISPMANX_PROTECTION_NONE, 0 /*alpha*/, 0/*clamp*/, DISPMANX_NO_ROTATE);

    nativewindow.element = dispman_element;
    nativewindow.width = mScreenWidth;
    nativewindow.height = mScreenHeight;
    vc_dispmanx_update_submit_sync(dispman_update);

    mSurface = eglCreateWindowSurface(mDisplay, config, &nativewindow, nullptr);
    assert(mSurface != EGL_NO_SURFACE);

    // connect the context to the surface
    result = eglMakeCurrent(mDisplay, mSurface, mSurface, mContext);
    assert(EGL_FALSE != result);

    mBpp = info->color_format == RARCH_COLOR_FORMAT_XRGB1555 ? 2 : 4;
    mTexType = info->color_format == RARCH_COLOR_FORMAT_XRGB1555 ? GL_UNSIGNED_SHORT_5_5_5_1 : GL_UNSIGNED_INT;

    // Set background color and clear buffers
    glClearColor(0, 0, 0, 1);
    glClear(GL_COLOR_BUFFER_BIT);
    glClear(GL_DEPTH_BUFFER_BIT);
    glShadeModel(GL_SMOOTH);

    // set viewport for aspect ratio, taken from RetroArch
    if (info->force_aspect == RARCH_TRUE)
    {
        float desired_aspect = info->aspect_ratio;
        float device_aspect = (float) dispman_modeinfo.width / dispman_modeinfo.height;

        // If the aspect ratios of screen and desired aspect ratio are sufficiently equal (floating point stuff),
        // assume they are actually equal.
        if (fabs(device_aspect - desired_aspect) < 0.0001)
        {
            glViewport(0, 0, mScreenWidth, mScreenHeight);
        }
        else if (device_aspect > desired_aspect)
        {
            float delta = (desired_aspect / device_aspect - 1.0) / 2.0 + 0.5;
            glViewport(mScreenWidth * (0.5 - delta), 0, 2.0 * mScreenWidth * delta, mScreenHeight);
            mScreenWidth = 2.0 * mScreenWidth * delta;
        }
        else
        {
            float delta = (device_aspect / desired_aspect - 1.0) / 2.0 + 0.5;
            glViewport(0, mScreenHeight * (0.5 - delta), mScreenWidth, 2.0 * mScreenHeight * delta);
            mScreenHeight = 2.0 * mScreenHeight * delta;
        }
    }
    else
    {
        glViewport(0, 0, mScreenWidth, mScreenHeight);
    }

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    glOrtho(0, 1, 0, 1, -1, 1);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    mTextureWidth = mTextureHeight = info->input_scale * RARCH_INPUT_SCALE_BASE;

    mEmptyBuf = new uint8_t[mTextureWidth * mTextureHeight * info->color_format == RARCH_COLOR_FORMAT_XRGB1555 ? 2 : 4]();

    glEnable(GL_TEXTURE_2D);
    glGenTextures(1, &mTexture);
    glBindTexture(GL_TEXTURE_2D, mTexture);
    //glPixelStorei(GL_UNPACK_ROW_LENGTH, mTextureWidth);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, mTextureWidth, mTextureHeight, 0, GL_RGBA, mTexType, mEmptyBuf);

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, info->smooth ? GL_LINEAR : GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, info->smooth ? GL_LINEAR : GL_NEAREST);

    glEnableClientState(GL_VERTEX_ARRAY);
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);

    mTexVertices[0] = 0;
    mTexVertices[1] = 1;
    mTexVertices[2] = 0;
    mTexVertices[3] = 0;
    mTexVertices[4] = 1;
    mTexVertices[5] = 0;
    mTexVertices[6] = 1;
    mTexVertices[7] = 1;

    glVertexPointer(2, GL_FLOAT, 0, default_vertices);
    glTexCoordPointer(2, GL_FLOAT, 0, mTexVertices);

    set_nonblock_state(info->vsync);
}

Rpi::~Rpi()
{
    delete [] mEmptyBuf;

    // clear screen
    glClear(GL_COLOR_BUFFER_BIT);
    eglSwapBuffers(mDisplay, mSurface);

    // Release OpenGL resources
    eglMakeCurrent(mDisplay, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT);
    eglDestroySurface(mDisplay, mSurface);
    eglDestroyContext(mDisplay, mContext);
    eglTerminate(mDisplay);
}

int Rpi::frame(const void *frame, unsigned width, unsigned height, unsigned pitch, const char *msg)
{
    if (width != mRenderWidth || height != mRenderHeight)
    {
        mRenderWidth = width;
        mRenderHeight = height;
        glPixelStorei(GL_UNPACK_ALIGNMENT, get_alignment(pitch));
    }

    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, mTexType, frame);

    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    eglSwapBuffers(mDisplay, mSurface);

    return RARCH_OK;
}

void Rpi::set_nonblock_state(int state)
{
    eglSwapInterval(mDisplay, state == RARCH_TRUE ? 1 : 0);
}

int Rpi::alive() const
{
    // assume alive
    return RARCH_TRUE;
}

int Rpi::focus() const
{
    // assume focus
    return RARCH_TRUE;
}