#include "parameters.h"

#include <azur/azur.h>
#include <azur/gint/render.h>
#include <gint/drivers/r61524.h>
#include <gint/rtc.h>

#include <gint/clock.h>
#include <gint/kmalloc.h>

#include <gint/usb-ff-bulk.h>
#include <gint/usb.h>
#include <libprof.h>

#include <cstdint>
#include <fxlibc/printf.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <num/num.h>

#include "extrakeyboard.h"
#include "pinball_entities.h"
#include "stdint-gcc.h"
#include "utilities.h"
#include "vector2D.h"

#include <vector>

#include <math.h>

bool screenshot = false;
bool record = false;
bool textoutput = false;
bool exitToOS = false;

uint8_t texttodraw = 1;

#define SCALE_PIXEL 1
#define X_RESOL (DWIDTH / SCALE_PIXEL)
#define Y_RESOL (DHEIGHT / SCALE_PIXEL)

float elapsedTime = 0.0f;
uint32_t time_update = 0, time_render = 0;
prof_t perf_update, perf_render;

static kmalloc_arena_t extended_ram = {0};
static kmalloc_arena_t *_uram;
kmalloc_gint_stats_t *_uram_stats;
kmalloc_gint_stats_t *extram_stats;

KeyboardExtra MyKeyboard;

Scene MyPinball;

libnum::num32 flipperHeight;
libnum::num32 cScale;
libnum::num32 simWidth;
libnum::num32 simHeight;

/* return the scaled x component of a vector */
uint16_t CX(Vector2D pos) { return (int) (pos.x * cScale); }

/* return the scaled y component of a vector */
uint16_t CY(Vector2D pos) { return (int) (libnum::num32(azrp_height) - pos.y * cScale); }

/* create the pinball board */
void SetupScene(void) {
  libnum::num32 offset = libnum::num32(0.02);

  flipperHeight = libnum::num32(1.7);
  cScale = libnum::num32(azrp_height) / flipperHeight;
  simWidth = libnum::num32(azrp_width) / cScale;
  simHeight = libnum::num32(azrp_height) / cScale;

  MyPinball.score = 0;

  MyPinball.borders.clear();
  MyPinball.borders.push_back( Vector2D( libnum::num32(0.74)      , libnum::num32(0.25)));
  MyPinball.borders.push_back( Vector2D( libnum::num32(1.0)-offset, libnum::num32(0.4)));
  MyPinball.borders.push_back( Vector2D( libnum::num32(1.0)-offset, flipperHeight - offset));
  MyPinball.borders.push_back( Vector2D( offset                   , flipperHeight - offset));
  MyPinball.borders.push_back( Vector2D( offset                   , libnum::num32(0.4)));
  MyPinball.borders.push_back( Vector2D( libnum::num32(0.26)      , libnum::num32(0.25)));
  MyPinball.borders.push_back( Vector2D( libnum::num32(0.26)      , libnum::num32(offset)));
  MyPinball.borders.push_back( Vector2D( libnum::num32(0.74)      , libnum::num32(offset)));

  
  MyPinball.balls.clear();
  libnum::num32 ball_radius = libnum::num32(0.03);
  libnum::num32 ball_mass = libnum::num32( PI ) * ball_radius * ball_radius;
  Vector2D ball_pos1( libnum::num32(0.92),  libnum::num32(0.5) );
  Vector2D ball_vel1( libnum::num32(0.2) ,  libnum::num32(3.5) );

  MyPinball.balls.push_back( Ball( ball_radius, ball_mass, ball_pos1, ball_vel1, 0.2 ) );
  Vector2D ball_pos2( libnum::num32(0.08),  libnum::num32(0.5) );
  Vector2D ball_vel2( libnum::num32(0.2) ,  libnum::num32(3.5) );
  MyPinball.balls.push_back( Ball( ball_radius, ball_mass, ball_pos2, ball_vel2, 0.2 ) );


	MyPinball.obstacles.clear();
	MyPinball.obstacles.push_back( Obstacle( 0.1 , Vector2D( libnum::num32(0.25), libnum::num32(0.6) ), 2.0 ) );
	MyPinball.obstacles.push_back( Obstacle( 0.1 , Vector2D( libnum::num32(0.75), libnum::num32(0.5) ), 2.0 ) );
	MyPinball.obstacles.push_back( Obstacle( 0.12, Vector2D( libnum::num32(0.7 ), libnum::num32(1.0) ), 2.0 ) );
	MyPinball.obstacles.push_back( Obstacle( 0.1 , Vector2D( libnum::num32(0.2 ), libnum::num32(1.2) ), 2.0 ) );
  

	libnum::num32 flip_radius = libnum::num32( 0.03);
	libnum::num32 flip_length = libnum::num32( 0.2);
	libnum::num32 flip_maxRotation = libnum::num32(1.0);
	libnum::num32 flip_restAngle = libnum::num32(0.5);
	libnum::num32 flip_angularVelocity = libnum::num32(10.0);
	libnum::num32 flip_restitution = libnum::num32(0.0);
	Vector2D flip_pos1( libnum::num32(0.26), libnum::num32(0.22) );
	Vector2D flip_pos2( libnum::num32(0.74), libnum::num32(0.22) );
	MyPinball.flippers.push_back( Flipper( flip_radius, flip_pos1, flip_length, -flip_restAngle, flip_maxRotation, flip_angularVelocity, flip_restitution) );
	MyPinball.flippers.push_back( Flipper( flip_radius, flip_pos2, flip_length, libnum::num32(PI) + flip_restAngle, -flip_maxRotation, flip_angularVelocity, flip_restitution) );
}

void HandleBallBallCollision( Ball ball1, Ball ball2 )
{
  libnum::num32 restitution = MIN( ball1.restitution, ball2.restitution );
  Vector2D dir = ball2.pos - ball1.pos;
  libnum::num32 d = dir.Length();
  if ( d==libnum::num32(0) || d > (ball1.radius + ball2.radius) ) return;

  dir.Normalise();

  libnum::num32 corr = (ball1.radius + ball2.radius-d) / libnum::num32(2);
  ball1.pos.Add(dir, -corr);
  ball2.pos.Add(dir, corr);

  libnum::num32 v1 = ball1.vel.Dot(dir);
  libnum::num32 v2 = ball2.vel.Dot(dir);

  libnum::num32 m1 = ball1.mass;
  libnum::num32 m2 = ball2.mass;

  libnum::num32 newV1 = (m1*v1 + m2*v2 - m2*(v1-v2)*restitution / (m1+m2) );
  libnum::num32 newV2 = (m1*v1 + m2*v2 - m1*(v2-v1)*restitution / (m1+m2) );

  ball1.vel.Add(dir, newV1-v1 );
  ball2.vel.Add(dir, newV2-v2 );
}

void HandleBallOsbtacleCollision( Ball ball, Obstacle obstacle )
{
  Vector2D dir = ball.pos - obstacle.pos;
  libnum::num32 d = dir.Length();
  if ( d==libnum::num32(0) || d > (ball.radius + obstacle.radius) ) return;

  dir.Normalise();

  libnum::num32 corr = ball.radius + obstacle.radius-d;
  ball.pos.Add(dir, corr);

  libnum::num32 v = ball.vel.Dot(dir);
  ball.vel.Add(dir, obstacle.pushVel-v );
  
  MyPinball.score++;
}


static void hook_prefrag(int id, void *fragment, int size) {
  if (!screenshot && !record)
    return;

  /* Screenshot takes precedence */
  char const *type = screenshot ? "image" : "video";

  int pipe = usb_ff_bulk_output();

  if (id == 0) {
    usb_fxlink_header_t h;
    usb_fxlink_image_t sh;
    int size = azrp_width * azrp_height * 2;

    usb_fxlink_fill_header(&h, "fxlink", type, size + sizeof sh);
    sh.width = htole32(azrp_width);
    sh.height = htole32(azrp_height);
    sh.pixel_format = htole32(USB_FXLINK_IMAGE_RGB565);

    usb_write_sync(pipe, &h, sizeof h, false);
    usb_write_sync(pipe, &sh, sizeof sh, false);
  }

  usb_write_sync(pipe, fragment, size, false);

  if (id == azrp_frag_count - 1) {
    usb_commit_sync(pipe);
    screenshot = false;
  }
}

static void update(float dt) {}

static void render(void) {

  azrp_clear(C_BLACK);

  int mod = MyPinball.borders.size();
  for( int i=0; i<MyPinball.borders.size(); i++ )     azrp_line( CX(MyPinball.borders[i]), CY(MyPinball.borders[i]), CX(MyPinball.borders[(i+1) % mod]), CY(MyPinball.borders[(i+1) % mod]), C_WHITE );

  for( int i=0; i<MyPinball.obstacles.size(); i++ )   azrp_filledcircle( CX(MyPinball.obstacles[i].pos), CY(MyPinball.obstacles[i].pos), (int) (MyPinball.obstacles[i].radius*cScale), C_BLUE );

  for( int i=0; i<MyPinball.balls.size(); i++ )       azrp_filledcircle( CX(MyPinball.balls[i].pos), CY(MyPinball.balls[i].pos), (int) (MyPinball.balls[i].radius*cScale), C_RED );

  for( int i=0; i<MyPinball.flippers.size(); i++ )
  {
    libnum::num32 angleflip = MyPinball.flippers[i].restAngle + MyPinball.flippers[i].sign * MyPinball.flippers[i].rotation;
    azrp_filledcircle( CX(MyPinball.flippers[i].pos), CY(MyPinball.flippers[i].pos), (int) (MyPinball.flippers[i].radius*cScale), C_GREEN );
    Vector2D extremity = MyPinball.flippers[i].pos.Clone();
    extremity.x += COS(angleflip)*MyPinball.flippers[i].length;
    extremity.y += SIN(angleflip)*MyPinball.flippers[i].length;
    azrp_filledcircle( CX(extremity), CY(extremity), (int) (MyPinball.flippers[i].radius*cScale), C_GREEN );
    azrp_line( CX(MyPinball.flippers[i].pos), CY(MyPinball.flippers[i].pos), CX(extremity), CY(extremity), C_GREEN );

  }
  

  azrp_draw_text(150, 01, "FPS  = %.0f - Mem Free = %d", (float)(1000.0f / elapsedTime), _uram_stats->free_memory + extram_stats->free_memory);
}

static void get_inputs(float dt) {
  if (MyKeyboard.IsKeyPressed(MYKEY_SHIFT) &&
      MyKeyboard.IsKeyHoldPressed(MYKEY_EXIT)) {
    exitToOS = true;
  };

#if (DEBUG_MODE)
  if (MyKeyboard.IsKeyPressed(MYKEY_OPTN) &&
      MyKeyboard.IsKeyPressedEvent(MYKEY_7) && usb_is_open()) {
    screenshot = true;
  };
  if (MyKeyboard.IsKeyPressed(MYKEY_OPTN) &&
      MyKeyboard.IsKeyPressedEvent(MYKEY_8) && usb_is_open()) {
    record = true;
  };
  if (MyKeyboard.IsKeyPressed(MYKEY_OPTN) &&
      MyKeyboard.IsKeyPressedEvent(MYKEY_9) && usb_is_open()) {
    record = false;
  };

  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F1)) {
  }
  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F2)) {
  }
  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F3)) {
  }
  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F4)) {
  }
  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F5)) {
  }
  if (MyKeyboard.IsKeyPressedEvent(MYKEY_F6)) {
  }
#endif

  /* we can have either LEFT or RIGHT or NONE OF THEM pressed for the direction
   */
}

bool AddMoreRAM(void) {
  /* allow more RAM */
  char const *osv = (char *)0x80020020;

  if ((!strncmp(osv, "03.", 3) && osv[3] <= '8') &&
      gint[HWCALC] == HWCALC_FXCG50) // CG-50
  {
    extended_ram.name = "extram";
    extended_ram.is_default = true;
    extended_ram.start = (void *)0x8c200000;
    extended_ram.end = (void *)0x8c4e0000;

    kmalloc_init_arena(&extended_ram, true);
    kmalloc_add_arena(&extended_ram);
    return true;
  } else if (gint[HWCALC] == HWCALC_PRIZM) // CG-10/20
  {

    extended_ram.name = "extram";
    extended_ram.is_default = true;

    uint16_t *vram1, *vram2;
    dgetvram(&vram1, &vram2);
    dsetvram(vram1, vram1);

    extended_ram.start = vram2;
    extended_ram.end = (char *)vram2 + 396 * 224 * 2;

    kmalloc_init_arena(&extended_ram, true);
    kmalloc_add_arena(&extended_ram);
    return true;

  } else if (gint[HWCALC] == HWCALC_FXCG_MANAGER) // CG-50 EMULATOR
  {

    extended_ram.name = "extram";
    extended_ram.is_default = true;
    extended_ram.start = (void *)0x88200000;
    extended_ram.end = (void *)0x884e0000;

    kmalloc_init_arena(&extended_ram, true);
    kmalloc_add_arena(&extended_ram);
    return true;
  } else {
    return false;
  }
}

void FreeMoreRAM(void) {
  memset(extended_ram.start, 0,
         (char *)extended_ram.end - (char *)extended_ram.start);
}

int main(void) {
  exitToOS = false;

  _uram = kmalloc_get_arena("_uram");

  bool canWeAllocate3Mb = AddMoreRAM();

  __printf_enable_fp();
  __printf_enable_fixed();

  azrp_config_scale(SCALE_PIXEL);

  azrp_shader_clear_configure();
  azrp_shader_image_rgb16_configure();
  azrp_shader_image_p8_configure();
  azrp_shader_image_p4_configure();
  azrp_shader_line_configure();
  azrp_shader_circle_configure();

  azrp_hook_set_prefrag(hook_prefrag);

  usb_interface_t const *interfaces[] = {&usb_ff_bulk, NULL};
  usb_open(interfaces, GINT_CALL_NULL);

  SetupScene();

  prof_init();

  do {
    perf_update = prof_make();
    prof_enter(perf_update);

    {
      // all the stuff to be update should be put here
      MyKeyboard.Update(elapsedTime);
      get_inputs(elapsedTime);

      update(elapsedTime);

      // update the RAM consumption status
      _uram_stats = kmalloc_get_gint_stats(_uram);
      extram_stats = kmalloc_get_gint_stats(&extended_ram);
    }

    prof_leave(perf_update);
    time_update = prof_time(perf_update);

    perf_render = prof_make();
    prof_enter(perf_render);

    {
      // all the stuff to be rendered should be put here
      render();

      azrp_update();
    }

    prof_leave(perf_render);
    time_render = prof_time(perf_render);

    elapsedTime = ((float)(time_update + time_render)) / 1000.0f;

  } while (exitToOS == false);

  prof_quit();
  usb_close();

  if (canWeAllocate3Mb)
    FreeMoreRAM();

  return 1;
}