// OpenGL ES 2.0 code
#include <jni.h>
#include <android/log.h>

#include <db_utilities_camera.h>
#include "mosaic/ImageUtils.h"
#include "mosaic_renderer/FrameBuffer.h"
#include "mosaic_renderer/WarpRenderer.h"
#include "mosaic_renderer/SurfaceTextureRenderer.h"
#include "mosaic_renderer/YVURenderer.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "mosaic_renderer_jni.h"

#define  LOG_TAG    "MosaicRenderer"
#define  LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define  LOGV(...)  __android_log_print(ANDROID_LOG_VERBOSE,LOG_TAG,__VA_ARGS__)
#define  LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)

// Texture handle
GLuint gSurfaceTextureID[1];

bool gWarpImage = true;

// Low-Res input image frame in YUVA format for preview rendering and processing
// and high-res YUVA input image for processing.
unsigned char* gPreviewImage[NR];
// Low-Res & high-res preview image width
int gPreviewImageWidth[NR];
// Low-Res & high-res preview image height
int gPreviewImageHeight[NR];

// Semaphore to protect simultaneous read/writes from gPreviewImage
sem_t gPreviewImage_semaphore;

// Off-screen preview FBO width (large enough to store the entire
// preview mosaic).
int gPreviewFBOWidth;
// Off-screen preview FBO height (large enough to store the entire
// preview mosaic).
int gPreviewFBOHeight;

// gK is the transformation to map the canonical {-1,1} vertex coordinate system
// to the {0,gPreviewImageWidth[LR]} input image frame coordinate system before
// applying the given affine transformation trs. gKm is the corresponding
// transformation for going to the {0,gPreviewFBOWidth}.
double gK[9];
double gKinv[9];
double gKm[9];
double gKminv[9];

// Shader to copy input SurfaceTexture into and RGBA FBO. The two shaders
// render to the textures with dimensions corresponding to the low-res and
// high-res image frames.
SurfaceTextureRenderer gSurfTexRenderer[NR];
// Off-screen FBOs to store the low-res and high-res RGBA copied out from
// the SurfaceTexture by the gSurfTexRenderers.
FrameBuffer gBufferInput[NR];

// Shader to convert RGBA textures into YVU textures for processing
YVURenderer gYVURenderer[NR];
// Off-screen FBOs to store the low-res and high-res YVU textures for processing
FrameBuffer gBufferInputYVU[NR];

// Shader to add warped current frame to the preview FBO
WarpRenderer gWarper1;
// Shader to translate the preview FBO
WarpRenderer gWarper2;
// Off-screen FBOs (flip-flop) to store the result of gWarper1 & gWarper2
FrameBuffer gBuffer[2];

// Shader to warp and render the preview FBO to the screen
WarpRenderer gPreview;

// Index of the gBuffer FBO gWarper1 is going to write into
int gCurrentFBOIndex = 0;

// 3x3 Matrices holding the transformation of this frame (gThisH1t) and of
// the last frame (gLastH1t) w.r.t the first frame.
double gThisH1t[9];
double gLastH1t[9];

// Variables to represent the fixed position of the top-left corner of the
// current frame in the previewFBO
double gCenterOffsetX = 0.0f;
double gCenterOffsetY = 0.0f;

// X-Offset of the viewfinder (current frame) w.r.t
// (gCenterOffsetX, gCenterOffsetY). This offset varies with time and is
// used to pan the viewfinder across the UI layout.
double gPanOffset = 0.0f;

// Variables tracking the translation value for the current frame and the
// last frame (both w.r.t the first frame). The difference between these
// values is used to control the panning speed of the viewfinder display
// on the UI screen.
double gThisTx = 0.0f;
double gLastTx = 0.0f;

// These are the scale factors used by the gPreview shader to ensure that
// the image frame is correctly scaled to the full UI layout height while
// maintaining its aspect ratio
double gUILayoutScalingX = 1.0f;
double gUILayoutScalingY = 1.0f;

// State of the viewfinder. Set to false when the viewfinder hits the UI edge.
bool gPanViewfinder = true;

// Affine transformation in GL 4x4 format (column-major) to warp the
// last frame mosaic into the current frame coordinate system.
GLfloat g_dAffinetransGL[16];
double g_dAffinetrans[16];

// Affine transformation in GL 4x4 format (column-major) to translate the
// preview FBO across the screen (viewfinder panning).
GLfloat g_dAffinetransPanGL[16];
double g_dAffinetransPan[16];

// XY translation in GL 4x4 format (column-major) to center the current
// preview mosaic in the preview FBO
GLfloat g_dTranslationToFBOCenterGL[16];
double g_dTranslationToFBOCenter[16];

// GL 4x4 Identity transformation
GLfloat g_dAffinetransIdent[] = {
    1., 0., 0., 0.,
    0., 1., 0., 0.,
    0., 0., 1., 0.,
    0., 0., 0., 1.};

float g_dIdent3x3[] = {
    1.0, 0.0, 0.0,
    0.0, 1.0, 0.0,
    0.0, 0.0, 1.0};

const int GL_TEXTURE_EXTERNAL_OES_ENUM = 0x8D65;

static void printGLString(const char *name, GLenum s) {
    const char *v = (const char *) glGetString(s);
    LOGI("GL %s = %s\n", name, v);
}

// @return false if there was an error
bool checkGlError(const char* op) {
    GLint error = glGetError();
    if (error != 0) {
        LOGE("after %s() glError (0x%x)\n", op, error);
        return false;
    }
    return true;
}

void bindSurfaceTexture(GLuint texId)
{
    glBindTexture(GL_TEXTURE_EXTERNAL_OES_ENUM, texId);

    // Can't do mipmapping with camera source
    glTexParameterf(GL_TEXTURE_EXTERNAL_OES_ENUM, GL_TEXTURE_MIN_FILTER,
            GL_LINEAR);
    glTexParameterf(GL_TEXTURE_EXTERNAL_OES_ENUM, GL_TEXTURE_MAG_FILTER,
            GL_LINEAR);
    // Clamp to edge is the only option
    glTexParameteri(GL_TEXTURE_EXTERNAL_OES_ENUM, GL_TEXTURE_WRAP_S,
            GL_CLAMP_TO_EDGE);
    glTexParameteri(GL_TEXTURE_EXTERNAL_OES_ENUM, GL_TEXTURE_WRAP_T,
            GL_CLAMP_TO_EDGE);
}

void ClearPreviewImage(int mID)
{
    unsigned char* ptr = gPreviewImage[mID];
    for(int j = 0, i = 0;
            j < gPreviewImageWidth[mID] * gPreviewImageHeight[mID] * 4;
            j += 4)
    {
            ptr[i++] = 0;
            ptr[i++] = 0;
            ptr[i++] = 0;
            ptr[i++] = 255;
    }

}

void ConvertAffine3x3toGL4x4(double *matGL44, double *mat33)
{
    matGL44[0] = mat33[0];
    matGL44[1] = mat33[3];
    matGL44[2] = 0.0;
    matGL44[3] = mat33[6];

    matGL44[4] = mat33[1];
    matGL44[5] = mat33[4];
    matGL44[6] = 0.0;
    matGL44[7] = mat33[7];

    matGL44[8] = 0;
    matGL44[9] = 0;
    matGL44[10] = 1.0;
    matGL44[11] = 0.0;

    matGL44[12] = mat33[2];
    matGL44[13] = mat33[5];
    matGL44[14] = 0.0;
    matGL44[15] = mat33[8];
}

// This function computes fills the 4x4 matrices g_dAffinetrans,
// and g_dAffinetransPan using the specified 3x3 affine
// transformation between the first captured frame and the current frame.
// The computed g_dAffinetrans is such that it warps the preview mosaic in
// the last frame's coordinate system into the coordinate system of the
// current frame. Thus, applying this transformation will create the current
// frame mosaic but with the current frame missing. This frame will then be
// pasted in by gWarper2 after translating it by g_dTranslationToFBOCenter.
// The computed g_dAffinetransPan is such that it offsets the computed preview
// mosaic horizontally to make the viewfinder pan within the UI layout.
void UpdateWarpTransformation(float *trs)
{
    double H[9], Hp[9], Htemp1[9], Htemp2[9], T[9];

    for(int i = 0; i < 9; i++)
    {
        gThisH1t[i] = trs[i];
    }

    db_Identity3x3(T);
    T[2] = -gCenterOffsetX;
    T[5] = -gCenterOffsetY;

    // H = ( inv(gThisH1t) * gLastH1t ) * T
    db_Identity3x3(Htemp1);
    db_Identity3x3(Htemp2);
    db_Identity3x3(H);
    db_InvertAffineTransform(Htemp1, gThisH1t);
    db_Multiply3x3_3x3(Htemp2, Htemp1, gLastH1t);
    db_Multiply3x3_3x3(H, Htemp2, T);

    for(int i = 0; i < 9; i++)
    {
        gLastH1t[i] = gThisH1t[i];
    }

    // Move the origin such that the frame is centered in the previewFBO
    // i.e. H = inv(T) * H
    H[2] += gCenterOffsetX;
    H[5] += gCenterOffsetY;

    // Hp = inv(K) * H * K
    // K moves the coordinate system from openGL to image pixels so
    // that the alignment transform H can be applied to them.
    // inv(K) moves the coordinate system back to openGL normalized
    // coordinates so that the shader can correctly render it.
    db_Identity3x3(Htemp1);
    db_Multiply3x3_3x3(Htemp1, H, gKm);
    db_Multiply3x3_3x3(Hp, gKminv, Htemp1);

    ConvertAffine3x3toGL4x4(g_dAffinetrans, Hp);

    ////////////////////////////////////////////////
    ////// Compute g_dAffinetransPan now...   //////
    ////////////////////////////////////////////////

    gThisTx = trs[2];

    if(gPanViewfinder)
    {
        gPanOffset += (gThisTx - gLastTx) * VIEWFINDER_PAN_FACTOR_HORZ;
    }

    gLastTx = gThisTx;

    // Compute the position of the current frame in the screen coordinate system
    // and stop the viewfinder panning if we hit the maximum border allowed for
    // this UI layout
    double normalizedXPositionOnScreenLeft = (2 *
            (gCenterOffsetX + gPanOffset) / gPreviewFBOWidth - 1.0) *
            gUILayoutScalingX;
    double normalizedScreenLimitLeft = -1.0 + VIEWPORT_BORDER_FACTOR_HORZ * 2.0;

    double normalizedXPositionOnScreenRight = (2 *
            ((gCenterOffsetX + gPanOffset) + gPreviewImageWidth[LR]) /
            gPreviewFBOWidth - 1.0) * gUILayoutScalingX;
    double normalizedScreenLimitRight = 1.0 - VIEWPORT_BORDER_FACTOR_HORZ * 2.0;

    if(normalizedXPositionOnScreenRight > normalizedScreenLimitRight ||
            normalizedXPositionOnScreenLeft < normalizedScreenLimitLeft)
        gPanViewfinder = false;

    db_Identity3x3(H);
    H[2] = gPanOffset;

    // Hp = inv(K) * H * K
    db_Identity3x3(Htemp1);
    db_Multiply3x3_3x3(Htemp1, H, gKm);
    db_Multiply3x3_3x3(Hp, gKminv, Htemp1);

    ConvertAffine3x3toGL4x4(g_dAffinetransPan, Hp);
}

void AllocateTextureMemory(int widthHR, int heightHR, int widthLR, int heightLR)
{
    gPreviewImageWidth[HR] = widthHR;
    gPreviewImageHeight[HR] = heightHR;

    gPreviewImageWidth[LR] = widthLR;
    gPreviewImageHeight[LR] = heightLR;

    sem_init(&gPreviewImage_semaphore, 0, 1);

    sem_wait(&gPreviewImage_semaphore);
    gPreviewImage[LR] = ImageUtils::allocateImage(gPreviewImageWidth[LR],
            gPreviewImageHeight[LR], 4);
    ClearPreviewImage(LR);
    gPreviewImage[HR] = ImageUtils::allocateImage(gPreviewImageWidth[HR],
            gPreviewImageHeight[HR], 4);
    ClearPreviewImage(HR);
    sem_post(&gPreviewImage_semaphore);

    gPreviewFBOWidth = PREVIEW_FBO_WIDTH_SCALE * gPreviewImageWidth[LR];
    gPreviewFBOHeight = PREVIEW_FBO_HEIGHT_SCALE * gPreviewImageHeight[LR];

    // The origin is such that the current frame will sit with its center
    // at the center of the previewFBO
    gCenterOffsetX = (gPreviewFBOWidth / 2 - gPreviewImageWidth[LR] / 2);
    gCenterOffsetY = (gPreviewFBOHeight / 2 - gPreviewImageHeight[LR] / 2);

    gPanOffset = 0.0f;

    db_Identity3x3(gThisH1t);
    db_Identity3x3(gLastH1t);

    gPanViewfinder = true;

    int w = gPreviewImageWidth[LR];
    int h = gPreviewImageHeight[LR];

    int wm = gPreviewFBOWidth;
    int hm = gPreviewFBOHeight;

    // K is the transformation to map the canonical [-1,1] vertex coordinate
    // system to the [0,w] image coordinate system before applying the given
    // affine transformation trs.
    gKm[0] = wm / 2.0 - 0.5;
    gKm[1] = 0.0;
    gKm[2] = wm / 2.0 - 0.5;
    gKm[3] = 0.0;
    gKm[4] = hm / 2.0 - 0.5;
    gKm[5] = hm / 2.0 - 0.5;
    gKm[6] = 0.0;
    gKm[7] = 0.0;
    gKm[8] = 1.0;

    gK[0] = w / 2.0 - 0.5;
    gK[1] = 0.0;
    gK[2] = w / 2.0 - 0.5;
    gK[3] = 0.0;
    gK[4] = h / 2.0 - 0.5;
    gK[5] = h / 2.0 - 0.5;
    gK[6] = 0.0;
    gK[7] = 0.0;
    gK[8] = 1.0;

    db_Identity3x3(gKinv);
    db_InvertCalibrationMatrix(gKinv, gK);

    db_Identity3x3(gKminv);
    db_InvertCalibrationMatrix(gKminv, gKm);

    //////////////////////////////////////////
    ////// Compute g_Translation now... //////
    //////////////////////////////////////////
    double T[9], Tp[9], Ttemp[9];

    db_Identity3x3(T);
    T[2] = gCenterOffsetX;
    T[5] = gCenterOffsetY;

    // Tp = inv(K) * T * K
    db_Identity3x3(Ttemp);
    db_Multiply3x3_3x3(Ttemp, T, gK);
    db_Multiply3x3_3x3(Tp, gKinv, Ttemp);

    ConvertAffine3x3toGL4x4(g_dTranslationToFBOCenter, Tp);

    UpdateWarpTransformation(g_dIdent3x3);
}

void FreeTextureMemory()
{
    sem_wait(&gPreviewImage_semaphore);
    ImageUtils::freeImage(gPreviewImage[LR]);
    ImageUtils::freeImage(gPreviewImage[HR]);
    sem_post(&gPreviewImage_semaphore);

    sem_destroy(&gPreviewImage_semaphore);
}

extern "C"
{
    JNIEXPORT jint JNICALL Java_com_android_camera_panorama_MosaicRenderer_init(
            JNIEnv * env, jobject obj);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_reset(
            JNIEnv * env, jobject obj,  jint width, jint height);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_preprocess(
            JNIEnv * env, jobject obj, jfloatArray stMatrix);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_transferGPUtoCPU(
            JNIEnv * env, jobject obj);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_step(
            JNIEnv * env, jobject obj);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_ready(
            JNIEnv * env, jobject obj);
    JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_setWarping(
            JNIEnv * env, jobject obj, jboolean flag);
};

JNIEXPORT jint JNICALL Java_com_android_camera_panorama_MosaicRenderer_init(
        JNIEnv * env, jobject obj)
{
    gSurfTexRenderer[LR].InitializeGLProgram();
    gSurfTexRenderer[HR].InitializeGLProgram();
    gYVURenderer[LR].InitializeGLProgram();
    gYVURenderer[HR].InitializeGLProgram();
    gWarper1.InitializeGLProgram();
    gWarper2.InitializeGLProgram();
    gPreview.InitializeGLProgram();
    gBuffer[0].InitializeGLContext();
    gBuffer[1].InitializeGLContext();
    gBufferInput[LR].InitializeGLContext();
    gBufferInput[HR].InitializeGLContext();
    gBufferInputYVU[LR].InitializeGLContext();
    gBufferInputYVU[HR].InitializeGLContext();

    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    glGenTextures(1, gSurfaceTextureID);
    // bind the surface texture
    bindSurfaceTexture(gSurfaceTextureID[0]);

    return (jint) gSurfaceTextureID[0];
}


JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_reset(
        JNIEnv * env, jobject obj,  jint width, jint height)
{
    gUILayoutScalingX = (PREVIEW_FBO_WIDTH_SCALE / PREVIEW_FBO_HEIGHT_SCALE) *
        (gPreviewImageWidth[LR] / gPreviewImageHeight[LR]) / (width / height) *
        PREVIEW_FBO_HEIGHT_SCALE;
    gUILayoutScalingY = PREVIEW_FBO_HEIGHT_SCALE;

    gBuffer[0].Init(gPreviewFBOWidth, gPreviewFBOHeight, GL_RGBA);
    gBuffer[1].Init(gPreviewFBOWidth, gPreviewFBOHeight, GL_RGBA);

    gBufferInput[LR].Init(gPreviewImageWidth[LR],
            gPreviewImageHeight[LR], GL_RGBA);

    gBufferInput[HR].Init(gPreviewImageWidth[HR],
            gPreviewImageHeight[HR], GL_RGBA);

    gBufferInputYVU[LR].Init(gPreviewImageWidth[LR],
            gPreviewImageHeight[LR], GL_RGBA);

    gBufferInputYVU[HR].Init(gPreviewImageWidth[HR],
            gPreviewImageHeight[HR], GL_RGBA);

    sem_wait(&gPreviewImage_semaphore);
    ClearPreviewImage(LR);
    ClearPreviewImage(HR);
    sem_post(&gPreviewImage_semaphore);

    // bind the surface texture
    bindSurfaceTexture(gSurfaceTextureID[0]);

    gSurfTexRenderer[LR].SetupGraphics(&gBufferInput[LR]);
    gSurfTexRenderer[LR].Clear(0.0, 0.0, 0.0, 1.0);
    gSurfTexRenderer[LR].SetViewportMatrix(1, 1, 1, 1);
    gSurfTexRenderer[LR].SetScalingMatrix(1.0f, -1.0f);
    gSurfTexRenderer[LR].SetInputTextureName(gSurfaceTextureID[0]);
    gSurfTexRenderer[LR].SetInputTextureType(GL_TEXTURE_EXTERNAL_OES_ENUM);

    gSurfTexRenderer[HR].SetupGraphics(&gBufferInput[HR]);
    gSurfTexRenderer[HR].Clear(0.0, 0.0, 0.0, 1.0);
    gSurfTexRenderer[HR].SetViewportMatrix(1, 1, 1, 1);
    gSurfTexRenderer[HR].SetScalingMatrix(1.0f, -1.0f);
    gSurfTexRenderer[HR].SetInputTextureName(gSurfaceTextureID[0]);
    gSurfTexRenderer[HR].SetInputTextureType(GL_TEXTURE_EXTERNAL_OES_ENUM);

    gYVURenderer[LR].SetupGraphics(&gBufferInputYVU[LR]);
    gYVURenderer[LR].Clear(0.0, 0.0, 0.0, 1.0);
    gYVURenderer[LR].SetInputTextureName(gBufferInput[LR].GetTextureName());
    gYVURenderer[LR].SetInputTextureType(GL_TEXTURE_2D);

    gYVURenderer[HR].SetupGraphics(&gBufferInputYVU[HR]);
    gYVURenderer[HR].Clear(0.0, 0.0, 0.0, 1.0);
    gYVURenderer[HR].SetInputTextureName(gBufferInput[HR].GetTextureName());
    gYVURenderer[HR].SetInputTextureType(GL_TEXTURE_2D);

    // gBuffer[1-gCurrentFBOIndex] --> gWarper1 --> gBuffer[gCurrentFBOIndex]
    gWarper1.SetupGraphics(&gBuffer[gCurrentFBOIndex]);
    gWarper1.Clear(0.0, 0.0, 0.0, 1.0);
    gWarper1.SetViewportMatrix(1, 1, 1, 1);
    gWarper1.SetScalingMatrix(1.0f, 1.0f);
    gWarper1.SetInputTextureName(gBuffer[1 - gCurrentFBOIndex].GetTextureName());
    gWarper1.SetInputTextureType(GL_TEXTURE_2D);

    // gBufferInput[LR] --> gWarper2 --> gBuffer[gCurrentFBOIndex]
    gWarper2.SetupGraphics(&gBuffer[gCurrentFBOIndex]);
    gWarper2.Clear(0.0, 0.0, 0.0, 1.0);
    gWarper2.SetViewportMatrix(gPreviewImageWidth[LR],
            gPreviewImageHeight[LR], gBuffer[gCurrentFBOIndex].GetWidth(),
            gBuffer[gCurrentFBOIndex].GetHeight());
    gWarper2.SetScalingMatrix(1.0f, 1.0f);
    gWarper2.SetInputTextureName(gBufferInput[LR].GetTextureName());
    gWarper2.SetInputTextureType(GL_TEXTURE_2D);

    gPreview.SetupGraphics(width, height);
    gPreview.Clear(0.0, 0.0, 0.0, 1.0);
    gPreview.SetViewportMatrix(1, 1, 1, 1);
    // Scale the previewFBO so that the viewfinder window fills the layout height
    // while maintaining the image aspect ratio
    gPreview.SetScalingMatrix(gUILayoutScalingX, -1.0f * gUILayoutScalingY);
    gPreview.SetInputTextureName(gBuffer[gCurrentFBOIndex].GetTextureName());
    gPreview.SetInputTextureType(GL_TEXTURE_2D);
}

JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_preprocess(
        JNIEnv * env, jobject obj, jfloatArray stMatrix)
{
    jfloat *stmat = env->GetFloatArrayElements(stMatrix, 0);

    gSurfTexRenderer[LR].SetSTMatrix((float*) stmat);
    gSurfTexRenderer[HR].SetSTMatrix((float*) stmat);

    env->ReleaseFloatArrayElements(stMatrix, stmat, 0);

    gSurfTexRenderer[LR].DrawTexture(g_dAffinetransIdent);
    gSurfTexRenderer[HR].DrawTexture(g_dAffinetransIdent);
}

#ifndef now_ms
#include <time.h>
static double
now_ms(void)
{
    //struct timespec res;
    struct timeval res;
    //clock_gettime(CLOCK_REALTIME, &res);
    gettimeofday(&res, NULL);
    return 1000.0*res.tv_sec + (double)res.tv_usec/1e3;
}
#endif


JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_transferGPUtoCPU(
        JNIEnv * env, jobject obj)
{
    double t0, t1, time_c;

    t0 = now_ms();

    gYVURenderer[LR].DrawTexture();
    gYVURenderer[HR].DrawTexture();

    t1 = now_ms();
    time_c = t1 - t0;
    LOGV("YVU Rendering: %g ms", time_c);

    sem_wait(&gPreviewImage_semaphore);
    // Bind to the input LR FBO and read the Low-Res data from there...
    glBindFramebuffer(GL_FRAMEBUFFER, gBufferInputYVU[LR].GetFrameBufferName());
    t0 = now_ms();
    glReadPixels(0,
                 0,
                 gBufferInput[LR].GetWidth(),
                 gBufferInput[LR].GetHeight(),
                 GL_RGBA,
                 GL_UNSIGNED_BYTE,
                 gPreviewImage[LR]);

    checkGlError("glReadPixels LR");
    t1 = now_ms();
    time_c = t1 - t0;
    LOGV("glReadPixels LR: %g ms", time_c);

    // Bind to the input HR FBO and read the high-res data from there...
    glBindFramebuffer(GL_FRAMEBUFFER, gBufferInputYVU[HR].GetFrameBufferName());
    t0 = now_ms();
    glReadPixels(0,
                 0,
                 gBufferInput[HR].GetWidth(),
                 gBufferInput[HR].GetHeight(),
                 GL_RGBA,
                 GL_UNSIGNED_BYTE,
                 gPreviewImage[HR]);

    checkGlError("glReadPixels HR");
    t1 = now_ms();
    time_c = t1 - t0;
    LOGV("glReadPixels HR: %g ms", time_c);

    sem_post(&gPreviewImage_semaphore);
}

JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_step(
        JNIEnv * env, jobject obj)
{
    if(!gWarpImage) // ViewFinder
    {
        gWarper2.SetupGraphics(&gBuffer[gCurrentFBOIndex]);
        gPreview.SetInputTextureName(gBuffer[gCurrentFBOIndex].GetTextureName());

        gWarper2.DrawTexture(g_dTranslationToFBOCenterGL);
        gPreview.DrawTexture(g_dAffinetransIdent);
    }
    else
    {
        gWarper1.SetupGraphics(&gBuffer[gCurrentFBOIndex]);
        // Clear the destination so that we can paint on it afresh
        gWarper1.Clear(0.0, 0.0, 0.0, 1.0);
        gWarper1.SetInputTextureName(
                gBuffer[1 - gCurrentFBOIndex].GetTextureName());
        gWarper2.SetupGraphics(&gBuffer[gCurrentFBOIndex]);
        gPreview.SetInputTextureName(gBuffer[gCurrentFBOIndex].GetTextureName());

        gWarper1.DrawTexture(g_dAffinetransGL);
        gWarper2.DrawTexture(g_dTranslationToFBOCenterGL);
        gPreview.DrawTexture(g_dAffinetransPanGL);

        gCurrentFBOIndex = 1 - gCurrentFBOIndex;
    }
}

JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_setWarping(
        JNIEnv * env, jobject obj, jboolean flag)
{
    // TODO: Review this logic
    if(gWarpImage != (bool) flag) //switching from viewfinder to capture or vice-versa
    {
        // Clear gBuffer[0]
        gWarper1.SetupGraphics(&gBuffer[0]);
        gWarper1.Clear(0.0, 0.0, 0.0, 1.0);
        // Clear gBuffer[1]
        gWarper1.SetupGraphics(&gBuffer[1]);
        gWarper1.Clear(0.0, 0.0, 0.0, 1.0);
        // Clear the screen to black.
        gPreview.Clear(0.0, 0.0, 0.0, 1.0);

        gLastTx = 0.0f;
        gPanOffset = 0.0f;
        gPanViewfinder = true;

        db_Identity3x3(gThisH1t);
        db_Identity3x3(gLastH1t);
    }

    gWarpImage = (bool)flag;
}


JNIEXPORT void JNICALL Java_com_android_camera_panorama_MosaicRenderer_ready(
        JNIEnv * env, jobject obj)
{
    for(int i=0; i<16; i++)
    {
        g_dAffinetransGL[i] = g_dAffinetrans[i];
        g_dAffinetransPanGL[i] = g_dAffinetransPan[i];
        g_dTranslationToFBOCenterGL[i] = g_dTranslationToFBOCenter[i];
    }
}