/*---------------------------------------------------------------------------*
  Project:  TwlSDK - demos.TWL - camera - camera-1
  File:     main.c

  Copyright 2007-2008 Nintendo. All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law. They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Date:: 2009-06-15#$
  $Rev: 10750 $
  $Author: kitase_hirotake $
 *---------------------------------------------------------------------------*/

#include <twl.h>
#include <twl/camera.h>
#include "DEMOBitmap.h"

#define NDMA_NO      1           // The NDMA number (0 to 3) to use
#define WIDTH       256         // Image width
#define HEIGHT      192         // Image height

#define LINES_AT_ONCE   CAMERA_GET_MAX_LINES(WIDTH)     // Number of lines transferred in one cycle
#define BYTES_PER_LINE  CAMERA_GET_LINE_BYTES(WIDTH)    // Number of bytes in one line's transfer

static void VBlankIntr(void);
static BOOL CameraInit(void);
static void CameraIntrVsync(CAMERAResult result);
static void CameraIntrError(CAMERAResult result);
static void CameraIntrReboot(CAMERAResult result);
static void CameraDmaRecvIntr(void* arg);

static CAMERASelect current;
static CAMERAFlip flipIn, flipOut;
static BOOL flipFlag = FALSE;
static BOOL switchFlag = FALSE;
static u32 stabilizedCount;

static int wp;  // Buffer while capturing data from camera
static int rp;  // Buffer most recently copied to VRAM
static BOOL wp_pending; // Data capture was cancelled (recapture to same buffer)
static u16 buffer[2][WIDTH*HEIGHT] ATTRIBUTE_ALIGN(32);
static void PendingCapture(void)
{
    wp_pending = TRUE;
}

// Character display
static void PutString( char *format, ... )
{
    u16             *dest = G2_GetBG1ScrPtr();
    char            temp[32+1];
    int             i;
    va_list         va;

    va_start(va, format);
    (void)OS_VSNPrintf(temp, sizeof(temp), format, va);
    va_end(va);

    for (i = 0; i < 32 && temp[i]; i++)
    {
        dest[i] = (u16)((u8)temp[i] | (1 << 12));
    }
}

/*---------------------------------------------------------------------------*
  Name:         TwlMain

  Description:  Main

  Arguments:    None.

  Returns:      None.
 *---------------------------------------------------------------------------*/
void TwlMain()
{
    CAMERAResult result;

    // Initialization
    OS_Init();
    OS_InitThread();
    GX_Init();
    OS_InitTick();
    OS_InitAlarm();

    // DMA is not used in GX (the old DMA conflicts with camera DMA)
    (void)GX_SetDefaultDMA(GX_DMA_NOT_USE);

    // Clear VRAM
    GX_SetBankForLCDC(GX_VRAM_LCDC_A);
    GX_SetBankForLCDC(GX_VRAM_LCDC_B);
    MI_CpuClearFast((void*)HW_LCDC_VRAM_A, 128 * 1024);
    MI_CpuClearFast((void*)HW_LCDC_VRAM_B, 128 * 1024);

    // Direct bitmap display mode and text display
    GX_SetBankForBG(GX_VRAM_BG_256_AB);         // Allocate VRAM-A, B banks to BG
    GX_SetGraphicsMode(GX_DISPMODE_GRAPHICS, GX_BGMODE_4, GX_BG0_AS_2D);
    GX_SetVisiblePlane(GX_PLANEMASK_BG1 | GX_PLANEMASK_BG3);

    G2_SetBG1Control(GX_BG_SCRSIZE_TEXT_256x256, GX_BG_COLORMODE_16,
                     GX_BG_SCRBASE_0x0000, GX_BG_CHARBASE_0x04000, GX_BG_EXTPLTT_01);
    G2_SetBG1Priority(1);
    G2_BG1Mosaic(FALSE);

    G2_SetBG3ControlDCBmp(GX_BG_SCRSIZE_DCBMP_256x256, GX_BG_AREAOVER_XLU, GX_BG_BMPSCRBASE_0x20000);
    G2_SetBG3Priority(3);
    G2_BG3Mosaic(FALSE);

    // Load text
    {
        static const GXRgb pal[16] = { GX_RGB(0, 0, 0), GX_RGB(31, 31, 31), };
        GX_LoadBG1Char(DEMOAsciiChr, 0x00000, sizeof(DEMOAsciiChr));
        GX_LoadBGPltt(pal, 0x0000, sizeof(pal));
    }
    wp = 0;
    rp = 1;
    wp_pending = TRUE;
    stabilizedCount = 0;

    // V-Blank interrupt settings
    OS_SetIrqFunction(OS_IE_V_BLANK, VBlankIntr);
    (void)OS_EnableIrqMask(OS_IE_V_BLANK);
    (void)OS_EnableIrq();
    (void)GX_VBlankIntr(TRUE);
    (void)OS_EnableInterrupts();

    OS_WaitVBlankIntr();
    GX_DispOn();

    // Initialize camera
    current = CAMERA_SELECT_IN;
    flipIn  = CAMERA_FLIP_HORIZONTAL;
    flipOut = CAMERA_FLIP_NONE;
    flipFlag = FALSE;

    (void)CameraInit();

    CAMERA_SetOutputFormat(CAMERA_OUTPUT_RGB);
    CAMERA_SetTransferLines(CAMERA_GET_MAX_LINES(WIDTH));

    // Configure DMA interrupt
    (void)OS_EnableIrqMask(OS_IE_NDMA1);

    // Camera VSYNC interrupt callback
    CAMERA_SetVsyncCallback(CameraIntrVsync);

    // Camera error interrupt callback
    CAMERA_SetBufferErrorCallback(CameraIntrError);

    // Camera restart completion callback
    CAMERA_SetRebootCallback(CameraIntrReboot);

    // Prepare the DMA to use for the capture
    MI_StopNDma(NDMA_NO);
    CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);

    // Camera start
    CAMERA_ClearBuffer();
    CAMERA_StartCapture();
    OS_TPrintf("Camera is shooting a movie...\n");

    while (1)
    {
        u16 pad;
        u16 trg;
        static u16 old = 0xffff;    // Ignore the trigger by first data
        static int camera_end_flag = FALSE;
        static int camera_stop_capture_flag = FALSE;
        static int camera_notactivate_flag = FALSE;

        OS_WaitVBlankIntr();

        pad = PAD_Read();
        trg = (u16)(pad & ~old);
        old = pad;

        if (camera_end_flag == FALSE)
        {
            if (trg & PAD_BUTTON_Y)
            {
                flipFlag = TRUE;
            }
            if (trg & PAD_BUTTON_X)
            {
                switchFlag = TRUE;
            }
            if (camera_stop_capture_flag == FALSE)
            {
                if (trg & PAD_BUTTON_B)
                {
                    static BOOL standby = FALSE;
                    standby ^= 1;
                    if (standby)
                    {
                        // Temporarily halt use of the camera. If the camera will be used again in the next cycle, all you need to do is set "Activate NONE", instead of calling the CAMERA_End function.
                        // If you set Activate to NONE prior to StopCapture, there is a risk that IsBusy will be TRUE until the next time you set "Activate ON".
                        CAMERA_StopCapture();
                        while(CAMERA_IsBusy())
                        {
                        }
                        MI_StopNDma(NDMA_NO);
                        result = CAMERA_I2CActivate(CAMERA_SELECT_NONE);
                        if(result == CAMERA_RESULT_FATAL_ERROR)
                            OS_Panic("CAMERA FATAL ERROR\n");

                        if(result == CAMERA_RESULT_ILLEGAL_STATUS)
                            continue;

                        camera_notactivate_flag = TRUE;
                    }
                    else
                    {
                        result = CAMERA_I2CActivate(current);
                        if(result == CAMERA_RESULT_FATAL_ERROR)
                            OS_Panic("CAMERA FALTAL ERROR\n");
                        if(result == CAMERA_RESULT_ILLEGAL_STATUS)
                            continue;
                        stabilizedCount = 0;

                        CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);
                        CAMERA_ClearBuffer();
                        CAMERA_StartCapture();
                        while (CAMERA_IsBusy() == FALSE)
                        {
                        }
                        camera_notactivate_flag = FALSE;
                    }
                    OS_TPrintf("%s\n", result == CAMERA_RESULT_SUCCESS ? "SUCCESS" : "FAILED");
                }
            }
            if (camera_notactivate_flag == FALSE)
            {
                if (trg & PAD_BUTTON_START)
                {
                    if (CAMERA_IsBusy())
                    {
                        // If you want to change parameters, you need to first start up the camera and then stop capturing. This makes the CAMERA_IsBusy function return FALSE.
                        // 
                        // If you are about to stop using the camera, then it's all right to stop capturing with the camera in standby mode.
                        result = CAMERA_I2CActivate(current);
                        if(result == CAMERA_RESULT_FATAL_ERROR)
                            OS_Panic("CAMERA FATAL ERROR\n");
                        if(result == CAMERA_RESULT_ILLEGAL_STATUS)
                            continue;

                        OS_TPrintf("%s\n", result == CAMERA_RESULT_SUCCESS ? "SUCCESS" : "FAILED");
                        OS_TPrintf("call CAMERA_Stop()... ");
                        CAMERA_StopCapture();
                        while (CAMERA_IsBusy())
                        {
                        }
                        OS_TPrintf("Camera was stopped.\n");
                        MI_StopNDma(NDMA_NO);
                        stabilizedCount = 0;
                        camera_stop_capture_flag = TRUE;
                    }
                    else
                    {
                        CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);
                        CAMERA_ClearBuffer();
                        CAMERA_StartCapture();
                        OS_TPrintf("Camera is shooting a movie...\n");
                        while (CAMERA_IsBusy() == FALSE)
                        {
                        }
                        camera_stop_capture_flag = FALSE;
                    }
                }
            }
        }
        // If you are not going to use the camera, just call CAMERA_I2CActivate(CAMERA_SELECT_NONE)
        if (trg & PAD_BUTTON_SELECT)
        {
            if (camera_end_flag == FALSE)
            {
                OS_TPrintf("call CAMERA_End()... ");
                CAMERA_StopCapture();
                while(CAMERA_IsBusy())
                {
                }
                MI_StopNDma(NDMA_NO);
                CAMERA_End();
                OS_TPrintf("Done.\n");
                camera_stop_capture_flag = TRUE;
                camera_notactivate_flag = TRUE;
                camera_end_flag = TRUE;
            }
            else
            {
                if(CameraInit() == TRUE)
                {
                    // Reregister the various callbacks that were registered at the first Init
                    CAMERA_SetVsyncCallback(CameraIntrVsync);
                    CAMERA_SetBufferErrorCallback(CameraIntrError);
                    CAMERA_SetRebootCallback(CameraIntrReboot);

                    CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);
                    CAMERA_ClearBuffer();
                    CAMERA_StartCapture();
                    OS_TPrintf("Camera is shooting a movie...\n");
                    while (CAMERA_IsBusy() == FALSE)
                    {
                    }
                    camera_end_flag = FALSE;
                    camera_notactivate_flag = FALSE;
                    camera_stop_capture_flag = FALSE;
                }
            }
        }
    }
}

//--------------------------------------------------------------------------------
//    V-Blank interrupt process
//
void VBlankIntr(void)
{
    OS_SetIrqCheckFlag(OS_IE_V_BLANK);
    if (wp == rp)
    {
        rp ^= 1;
        GX_LoadBG3Scr(buffer[rp], 0, BYTES_PER_LINE * HEIGHT);
        DC_InvalidateRange(buffer[rp], BYTES_PER_LINE * HEIGHT);
    }
}

//--------------------------------------------------------------------------------
//    Camera initialization (only the Init- and I2C-related initialization)
//
BOOL CameraInit(void)
{
    CAMERAResult result;
    OSTick begin = OS_GetTick();

    result = CAMERA_Init();
    if(result == CAMERA_RESULT_FATAL_ERROR)
        OS_TPanic("CAMERA_Init was failed.");
    if(result == CAMERA_RESULT_ILLEGAL_STATUS)
        return FALSE;

    OS_TPrintf("CAMERA_Init took %d msec\n", (int)OS_TicksToMilliSeconds(OS_GetTick()-begin));

    result = CAMERA_I2CFlip(CAMERA_SELECT_IN, flipIn);
    if (result == CAMERA_RESULT_FATAL_ERROR)
        OS_TPanic("CAMERA_I2CFlip was failed. (%d)\n", result);
    if(result == CAMERA_RESULT_ILLEGAL_STATUS)
        return FALSE;

    result = CAMERA_I2CFlip(CAMERA_SELECT_OUT, flipOut);
    if (result == CAMERA_RESULT_FATAL_ERROR)
        OS_TPanic("CAMERA_I2CFlip was failed. (%d)\n", result);
    if(result == CAMERA_RESULT_ILLEGAL_STATUS)
        return FALSE;

    result = CAMERA_I2CActivate(current);
    if (result == CAMERA_RESULT_FATAL_ERROR)
        OS_TPanic("CAMERA_I2CActivate was failed. (%d)\n", result);
    if(result == CAMERA_RESULT_ILLEGAL_STATUS)
        return FALSE;
    stabilizedCount = 0;

    return TRUE;
}

//--------------------------------------------------------------------------------
//    Camera interrupt process (Generated when there is an error and for Vsync)
//
void CameraIntrError(CAMERAResult result)
{
#pragma unused(result)
    OS_TPrintf("Error was occurred.\n");
    // Camera Stop processing
    CAMERA_StopCapture();
    MI_StopNDma(NDMA_NO);
    CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);
    CAMERA_ClearBuffer();
    wp_pending = TRUE;      // Also use same frame next time
    CAMERA_StartCapture();
}

// Restart completion callback
void CameraIntrReboot(CAMERAResult result)
{
    if(result == CAMERA_RESULT_FATAL_ERROR)
    {
        return; // Restore was not possible, even after restarting camera
    }
    CameraIntrError(result); // DMA synchronization might have drifted, so realign
}

static void DoneCallback(CAMERAResult result, void* arg)
{
    const char* str = arg;
    if (result != CAMERA_RESULT_SUCCESS)
    {
        OS_TPrintf("%s was failed. (%d)\n", arg, result);
    }
}

void CameraIntrVsync(CAMERAResult result)
{
#pragma unused(result)
    if(stabilizedCount <= 30)
        stabilizedCount++;
}

#define FPS_SAMPLES 4
void CameraDmaRecvIntr(void* arg)
{
#pragma unused(arg)
    CAMERAResult result;

    MI_StopNDma(NDMA_NO);

    if(flipFlag == TRUE)
    {
        CAMERAFlip *pFlip = (current == CAMERA_SELECT_IN ? &flipIn : &flipOut);
        *pFlip = (CAMERAFlip)((*pFlip + 1) % CAMERA_FLIP_MAX);
        result = CAMERA_I2CFlipAsync(current, *pFlip, DoneCallback, "CAMERA_I2CFlipAsync");
        if (result == CAMERA_RESULT_FATAL_ERROR)
            OS_TPanic("CAMERA_I2CFlipAsync was failed. (%d)\n", result);
        if(result == CAMERA_RESULT_ILLEGAL_STATUS)
            return;

        flipFlag = FALSE;
    }
    else if(switchFlag == TRUE)
    {
        current = (current == CAMERA_SELECT_IN ? CAMERA_SELECT_OUT : CAMERA_SELECT_IN);
        result = CAMERA_I2CActivateAsync(current, DoneCallback, "CAMERA_I2CActivateAsync");
        if (result == CAMERA_RESULT_FATAL_ERROR)
            OS_TPanic("CAMERA_I2CActivateAsync was failed. (%d)\n", result);
        if(result == CAMERA_RESULT_ILLEGAL_STATUS)
            return;
        stabilizedCount = 0;
        switchFlag = FALSE;
    }

    // Start the next frame capture
    if (wp_pending)
    {
        wp_pending = FALSE;
    }
    else
    {
        // Capture results are not displayed on screen until camera is stable
        // This demo waits the minimum of four camera frames required to avoid chromatic aberration, but if you want to wait until auto-exposure is stable, you need to wait 14 frames for an indoor shot or 30 for an outdoor one, as indicated in the Function Reference.
        // 
        if(stabilizedCount > 4)
        {
            wp ^= 1;
        }
    }
    CAMERA_DmaRecvAsync(NDMA_NO, buffer[wp], CAMERA_GetBytesAtOnce(WIDTH), CAMERA_GET_FRAME_BYTES(WIDTH, HEIGHT), CameraDmaRecvIntr, NULL);

    // Display frame rate
    {
        static OSTick begin = 0;
        static int uspf[FPS_SAMPLES] = { 0 };
        static int count = 0;
        int i;
        int sum = 0;
        OSTick end = OS_GetTick();
        if (begin)  // Leave out the first time
        {
            uspf[count] = (int)OS_TicksToMicroSeconds(end - begin);
            count = (count + 1) % FPS_SAMPLES;
        }
        begin = end;
        // Calculate average value
        for (i = 0; i < FPS_SAMPLES; i++)
        {
            if (uspf[i] == 0)  break;
            sum +=  uspf[i];
        }
        if (sum)
        {
            int mfps = (int)(1000000000LL * i / sum);
            PutString("%2d.%03d fps", mfps / 1000, mfps % 1000);
        }
    }
}
