#include <stdio.h>
#include <stdlib.h>
#include <gccore.h>
#include <wiiuse/wpad.h>
#include <inttypes.h>
#include <string.h>

#include "term.h"
#include "state.h"

static void *xfb = NULL;
static GXRModeObj *rmode = NULL;

#define TARGET_IOS 58

static void initialize(int again) {
  VIDEO_Init();

  rmode = VIDEO_GetPreferredMode(NULL);
  xfb = MEM_K0_TO_K1(SYS_AllocateFramebuffer(rmode));

  console_init(xfb, 20, 20, rmode->fbWidth - 20, rmode->xfbHeight - 20, rmode->fbWidth * VI_DISPLAY_PIX_SZ);

  VIDEO_Configure(rmode);
  VIDEO_SetNextFramebuffer(xfb);
  VIDEO_ClearFrameBuffer(rmode, xfb, COLOR_BLACK);
  VIDEO_SetBlack(FALSE);
  VIDEO_Flush();
  VIDEO_WaitVSync();
  if (rmode->viTVMode & VI_NON_INTERLACE) {
    VIDEO_WaitVSync();
  }

  fputs(TERM_CLEAR TERM_CUR_POS(2, 5), stdout);
  printf("USB Gecko (newnewexi) test tool by figboot\n\n");

  s32 iosver = IOS_GetVersion();
  s32 iosrev = IOS_GetRevision();
  printf("You're running IOS%" PRId32 " rev %" PRId32 ".\n", iosver, iosrev);

  if (iosver != TARGET_IOS) {
    if (again) {
      printf(TERM_CSI("93m") "Failed to set up IOS" STR(TARGET_IOS) "! Bailing out.\n");
      exit(1);
    }

    printf("Trying to load IOS" STR(TARGET_IOS) "...\n");

    s32 res;
    if ((res = IOS_ReloadIOS(TARGET_IOS)) < 0) {
      printf(TERM_CSI("93m") "Failed to set up IOS" STR(TARGET_IOS) "! IOS_ReloadIOS returned %" PRId32 "\n", res);
      exit(1);
    }

    free(MEM_K1_TO_K0(xfb));
    initialize(1);
  }
}

static void __attribute__((noreturn)) fatal_error(void) {
  printf("The program will now abort due to the fatal error.\n");
  exit(1);
}

int main(int argc, char **argv)
{
  void *state_data = NULL;
  size_t state_data_size = 0;
  struct et_next_state next;
  const struct et_state *cur_state = NULL;
  int stateres = 0;

  u32 wpad_pressed;
  u32 pad_pressed;

  initialize(0);

  WPAD_Init();
  PAD_Init();

  printf("Done initializing.\n");

  /* Set up initial state */
  cur_state = et_state_setup;

  if (cur_state->private_size > 0) {
    state_data = malloc(cur_state->private_size);
    state_data_size = cur_state->private_size;
    if (!state_data) {
      printf("Failed to allocate private space for setup state %p!", cur_state);
      fatal_error();
    }
  }

  if ((stateres = et_state_init(cur_state, state_data, et_init_data_null)) < 0) {
    printf("Failed to init setup state %p: %d\n", cur_state, stateres);
    fatal_error();
  }

  while (SYS_MainLoop()) {
    WPAD_ScanPads();
    PAD_ScanPads();

    wpad_pressed = WPAD_ButtonsDown(0);
    pad_pressed = PAD_ButtonsDown(0);

    if (wpad_pressed & WPAD_BUTTON_HOME || pad_pressed & PAD_BUTTON_START) {
      printf("Exit button pressed. Goodbye!\n");
      exit(0);
    }

    stateres = et_state_tick(cur_state, state_data, &next);
    if (stateres < 0) {
      printf("Failed to tick state %p: %d\n", cur_state, stateres);
      fatal_error();
    }

    if (stateres > 0) {
      /* tear down the old state first... */
      et_state_cleanup(cur_state, state_data);

      cur_state = next.state;

      if (state_data_size < cur_state->private_size) {
        void *new_data = realloc(state_data, cur_state->private_size);
        if (!new_data) {
          printf("Failed to allocate %zu bytes for new state %p\n", cur_state->private_size, cur_state);
          fatal_error();
        }

        state_data = new_data;
        state_data_size = cur_state->private_size;
      }

      if ((stateres = et_state_init(cur_state, state_data, next.init_data)) < 0) {
        printf("Failed to init new state %p: %d\n", cur_state, stateres);
        fatal_error();
      }
    }

    VIDEO_WaitVSync();
  }

  return 0;
}

#if 0

//---------------------------------------------------------------------------------
int main(int argc, char **argv) {
//---------------------------------------------------------------------------------

	// Initialise the video system
	VIDEO_Init();

	// This function initialises the attached controllers
	WPAD_Init();

	// Obtain the preferred video mode from the system
	// This will correspond to the settings in the Wii menu
	rmode = VIDEO_GetPreferredMode(NULL);

	// Allocate memory for the display in the uncached region
	xfb = MEM_K0_TO_K1(SYS_AllocateFramebuffer(rmode));

	// Initialise the console, required for printf
	console_init(xfb,20,20,rmode->fbWidth,rmode->xfbHeight,rmode->fbWidth*VI_DISPLAY_PIX_SZ);

	// Set up the video registers with the chosen mode
	VIDEO_Configure(rmode);

	// Tell the video hardware where our display memory is
	VIDEO_SetNextFramebuffer(xfb);

	// Make the display visible
	VIDEO_SetBlack(false);

	// Flush the video register changes to the hardware
	VIDEO_Flush();

	// Wait for Video setup to complete
	VIDEO_WaitVSync();
	if(rmode->viTVMode&VI_NON_INTERLACE) VIDEO_WaitVSync();


	// The console understands VT terminal escape codes
	// This positions the cursor on row 2, column 0
	// we can use variables for this with format codes too
	// e.g. printf ("\x1b[%d;%dH", row, column );
	printf("\x1b[2;0H");


	printf("Hello World!\n");

#define UGCHAN EXI_CHANNEL_1 /* slot B */
#define UGDEV EXI_DEVICE_0   /* memory card */

        if (EXI_Lock(UGCHAN, UGDEV, NULL) == 1) {
          EXI_Select(UGCHAN, UGDEV, EXI_SPEED32MHZ);
          uint32_t cmd = 0x90000000;
          int res = 0;
          if (EXI_Imm(UGCHAN, &cmd, sizeof(cmd), EXI_READWRITE, NULL) == 0) res |= 0x01;
          if (EXI_Sync(UGCHAN) == 0) res |= 0x02;

          if (res == 0) {
            printf("Got: 0x%08x\n", cmd);
            if (cmd != 0x04700000) {
              printf("This is probably not a USB gecko...\n");
            }
          } else {
            printf("EXI read/write error :( %#04x\n", res);
          }

          EXI_Deselect(UGCHAN);
          EXI_Unlock(UGCHAN);
        } else {
          printf("Failed to lock EXI channel %d %d\n", UGCHAN, UGDEV);
          goto waitexit;
        }

        for (int i = 0; i < 10; ++i) {
          if (EXI_Lock(UGCHAN, UGDEV, NULL) == 0) {
            printf("Failed to lock EXI for LED\n");
            break;
          }

          int res = 0;

          if (EXI_Select(UGCHAN, UGDEV, EXI_SPEED32MHZ) == 0) res |= 0x01;
          uint32_t cmd = (i & 1) ? 0x80000000 : 0x70000000;
          if (EXI_Imm(UGCHAN, &cmd, sizeof(cmd), EXI_WRITE, NULL) == 0) res |= 0x02;
          if (EXI_Sync(UGCHAN) == 0) res |= 0x04;
          if (EXI_Deselect(UGCHAN) == 0) res |= 0x08;

          if (EXI_Unlock(UGCHAN) == 0) res |= 0x10;

          printf("Error for LED: 0x%02x\n", res);

          for (int j = 0; j < 60; ++j) {
            VIDEO_WaitVSync();
          }
        }


        for (int j = 0; j < 10; ++j) {
        if (EXI_Lock(UGCHAN, UGDEV, NULL) == 1) {
          printf("Sending a zillion bytes to your so-called \"usb gecko\"...\n");

          for (int i = 0; i < 4096; ++i) {
            EXI_Select(UGCHAN, UGDEV, EXI_SPEED32MHZ);

            uint16_t cmd = 0xB000 | (uint16_t)(((i & 63) + 32) << 4);
            if (EXI_Imm(UGCHAN, &cmd, sizeof(cmd), EXI_READWRITE, NULL) == 0) {
              printf("Failed when writing %d\n", i);
              goto error;
            }

            if (EXI_Sync(UGCHAN) == 0) {
              printf("Failed sync when writing %d\n", i);
              goto error;
            }

            if ((cmd & 0x0400) == 0) {
              printf("Out of FIFO space at %d\n", i);
              goto error;
            }

            EXI_Deselect(UGCHAN);
            continue;
error:
            EXI_Deselect(UGCHAN);
            break;
          }

          EXI_Unlock(UGCHAN);
        }  else {
          printf("Failed to lock EXI channel %d %d\n", UGCHAN, UGDEV);
          goto waitexit;
        }
        }

waitexit:
        printf("That's all! Press HOME to exit.\n");

	while(SYS_MainLoop()) {

		// Call WPAD_ScanPads each loop, this reads the latest controller states
		WPAD_ScanPads();

		// WPAD_ButtonsDown tells us which buttons were pressed in this loop
		// this is a "one shot" state which will not fire again until the button has been released
		u32 pressed = WPAD_ButtonsDown(0);

		// We return to the launcher application via exit
		if ( pressed & WPAD_BUTTON_HOME ) exit(0);

		// Wait for the next frame
		VIDEO_WaitVSync();
	}

	return 0;
}
#endif