/* This file is part of SyncContest.
   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */

#include "simulation_loop.h"

#include <SDL.h>

#include "mymath.h"
#include "challenge.h"
#include "cutils.h"
#include "map.h"

#ifndef ASSET_DIR_PATH
#define ASSET_DIR_PATH "./assets"
#endif

void find_absolute_asset_path(char *path, size_t path_size,
                              const char *filename) {
  snprintf(path, path_size, "%s/%s", ASSET_DIR_PATH, filename);
}

void load_asset_wav(const char *filename, asset_wav_t *wav) {
  assert (filename);
  assert (wav);

  char filepath[512];

  find_absolute_asset_path(filepath, sizeof filepath, filename);
  if (!SDL_LoadWAV(filepath, &wav->spec, &wav->buffer, &wav->size))
    log_fatal("[sdl] could not open WAV file %s (%s)\n",
              filepath, SDL_GetError());
}

void free_asset_wav(asset_wav_t *wav) {
  assert (wav);
  SDL_FreeWAV(wav->buffer);
  wav->buffer = NULL;
}

SDL_Surface *load_asset_bmp_surface(const char *filename) {
  SDL_Surface *r;
  char filepath[512];

  find_absolute_asset_path(filepath, sizeof filepath, filename);

  if ((r = SDL_LoadBMP(filepath)) == NULL)
    log_fatal("[sdl] could not load %s\n", filepath);

  return r;
}

SDL_Texture *load_asset_bmp_texture(const char *filename, SDL_Renderer *r,
                                    int *texture_width, int *texture_height) {
  SDL_Surface *s = load_asset_bmp_surface(filename);
  SDL_Texture *t = SDL_CreateTextureFromSurface(r, s);

  if (!t)
    log_fatal("[sdl] could not load texture from surface %s (%s)\n",
              filename, SDL_GetError());
  SDL_FreeSurface(s);

  if (texture_width && texture_height
      && SDL_QueryTexture(t, NULL, NULL, texture_width, texture_height))
    log_fatal("[sdl] could not query texture %s (%s)\n",
              filename, SDL_GetError());

  log_info("[sdl] loaded texture %s\n", filename);
  return t;
}

/* Our simulation assumes the origin is on the bottom-left corner of the window,
   unlike SDL which assumes it is on the top-left corner. The function below
   transforms a point from geometric space into SDL space. */
void sdl_space_of_position(Globals__position *position, int *x, int *y) {
  *x = position->x;
  *y = MAX_Y - position->y;
}

void sdl_point_of_position(Globals__position *position, SDL_Point *point) {
  sdl_space_of_position(position, &point->x, &point->y);
}

int draw_point(SDL_Renderer *rd, Globals__position *p) {
  int x, y;
  sdl_space_of_position(p, &x, &y);
  return SDL_RenderDrawPoint(rd, x, y);
}

int draw_line(SDL_Renderer *rd,
              Globals__position *startp, Globals__position *endp) {
  int x1, y1, x2, y2;
  sdl_space_of_position(startp, &x1, &y1);
  sdl_space_of_position(endp, &x2, &y2);
  return SDL_RenderDrawLine(rd, x1, y1, x2, y2);
}

void draw_rectangle(SDL_Renderer *rd, Globals__position *center, size_t l,
                    uint32_t r, uint32_t g, uint32_t b) {
  SDL_Rect rect = (SDL_Rect){ 0, 0, l, l };
  /* Compute coordinates. */
  sdl_space_of_position(center, &rect.x, &rect.y);
  rect.x -= l / 2;
  rect.y -= l / 2;
  /* Render rectangle. */
  SDL_SetRenderDrawColor(rd, r, g, b, 0x00);
  if (SDL_RenderFillRect(rd, &rect) < 0)
    log_fatal("[sdl] could not draw rectangle (%s)\n", SDL_GetError());
  SDL_SetRenderDrawColor(rd, 0xFF, 0xFF, 0xFF, 0);
  if (draw_point(rd, center) < 0)
    log_fatal("[sdl] could not draw point (%s)\n", SDL_GetError());
}

void draw_tile(SDL_Renderer *rd,
               SDL_Texture *texture,
               Globals__position *p,
               double angle,
               int w,
               int h) {
  SDL_Rect dst_rect = { 0, 0, w, h };
  Globals__position center = { p->x - w / 2, p->y + h / 2 };
  sdl_space_of_position(&center, &dst_rect.x, &dst_rect.y);

  SDL_RenderCopyEx(rd,             /* renderer */
                   texture,        /* texture */
                   NULL,           /* entire texture */
                   &dst_rect,      /* destination */
                   angle,          /* angle (cw) */
                   NULL,           /* center dst_rec */
                   SDL_FLIP_NONE); /* no flipping */
}

race_result_t simulation_loop(bool show_guide,
                              int initial_top,
                              float sps,
                              bool headless,
                              bool audio,
                              size_t max_synchronous_steps) {
  SDL_Window *w;
  bool quit = false;                /* Shall we quit? */
  race_result_t res = RACE_TIMEOUT; /* Did we complete the race? */
  int top = initial_top;            /* Has the race started? */
  bool verbose = false, debug = false;
  int car_w, car_h, obs_w, obs_h;
  SDL_Texture *bg, *car, *obs;
  SDL_Renderer *r;
  size_t current_tick = 0;

  /* Initialize SDL and acquire resources. */

  if (!headless) {
    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO) < 0)
      log_fatal("[sdl] could not initialize SDL library (%s)\n",
                SDL_GetError());

    if ((w = SDL_CreateWindow("Synchronous Contest " YEAR " v" VERSION,
                              SDL_WINDOWPOS_UNDEFINED,
                              SDL_WINDOWPOS_UNDEFINED,
                              MAX_X,
                              MAX_Y,
                              SDL_WINDOW_SHOWN|SDL_WINDOW_RESIZABLE)) == NULL)
      log_fatal("[sdl] could not open window (%s)\n", SDL_GetError());

    r = SDL_CreateRenderer(w, -1, SDL_RENDERER_ACCELERATED);
    if (!r)
      log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError());

    if (!r)
      log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError());

    bg = load_asset_bmp_texture(show_guide ? map->guide : map->graphics,
                                r,
                                NULL,
                                NULL);
    car = load_asset_bmp_texture("orange.bmp",
                                 r,
                                 &car_w,
                                 &car_h);
    obs = load_asset_bmp_texture("obst.bmp",
                                 r,
                                 &obs_w,
                                 &obs_h);

    /* Load sounds and setup audio device. */

    load_asset_wav("collision.wav", &collision);
    load_asset_wav("direction.wav", &wrong_dir);
    load_asset_wav("exit.wav", &exit_road);
    load_asset_wav("light.wav", &light_run);
    load_asset_wav("speed.wav", &speed_excess);

    if (audio) {
      if (!(audio_device =
            SDL_OpenAudioDevice(NULL, 0, &collision.spec, NULL, 0))) {
        log_info("[sdl] could not open audio device\n");
      } else
        SDL_PauseAudioDevice(audio_device, 0);
    }
  }

  /* Initialize synchronous state. */

  Challenge__the_challenge_out out;
  Challenge__the_challenge_mem mem;
  Challenge__the_challenge_reset(&mem);

  /* Setup time counters. */

  const uint32_t sync_dt_ms = 1000.f * Globals__timestep;
  const uint32_t simulation_dt_ms = (1. / (double)sps) * 1000.;
  uint32_t time_budget_ms = sync_dt_ms; /* Enough to do one initial step. */

  log_info("[simulation] starting (%zu ms/cycle)\n", simulation_dt_ms);

  while (!quit
         && (!max_synchronous_steps || current_tick < max_synchronous_steps)) {
    SDL_Event e;
    uint32_t start_time_ms = SDL_GetTicks();

    /* Perform as many synchronous steps as possible within our budget. */
    while (time_budget_ms >= sync_dt_ms
           && (!max_synchronous_steps
               || current_tick < max_synchronous_steps)) {
      time_budget_ms -= sync_dt_ms;
      Challenge__the_challenge_step(map->init_phase, top, &out, &mem);
      current_tick++;
      /* Check robot status once simulation has started. */
      if (top) {
        switch (out.sta) {
        case Globals__Preparing:
        case Globals__Running:
          break;
        case Globals__Arrived:
          log_info("[simulation %08zu] race finished\n", current_tick);
          res = RACE_SUCCESS;
          quit = true;
          break;
        case Globals__Stopped:
          log_info("[simulation %08zu] car stopped\n", current_tick);
          res = RACE_CRASH;
          quit = true;
          break;
        }
      }
      if (!debug && !verbose && !quit) {
        printf("\e[?25l");  /* Disable cursor */
        printf("H %06.2f\tV %06.2f\tT %06.2f\tS %09d\r",
               out.ph.ph_head, out.ph.ph_vel, out.time, out.scoreA);
        printf("\e[?25h");  /* Re-enable cursor */
      }
    }

    if (!headless) {
      /* Process events, including key presses. */
      while (SDL_PollEvent(&e) != 0) {
        switch (e.type) {
        case SDL_QUIT:
          quit = true;
          break;
        case SDL_WINDOWEVENT:
          if (e.window.event == SDL_WINDOWEVENT_RESIZED) {
            float newwidth = e.window.data1;
            float newheight = e.window.data2;
            SDL_RenderSetScale(r, newwidth / MAX_X, newheight / MAX_Y);
          }
          break;
        case SDL_KEYDOWN:
          switch (e.key.keysym.sym) {
          case SDLK_q:
            quit = true;
            break;
          case SDLK_t:
            top = true;
            break;
          case SDLK_d:
            debug = !debug;
            break;
          case SDLK_v:
            if (verbose)
              log_set_verbosity_level(LOG_INFO);
            else
              log_set_verbosity_level(LOG_DEBUG);
            verbose = !verbose;
            break;
          case SDLK_UP:
            map->init_phase.ph_head += 2;
            break;
          case SDLK_DOWN:
            map->init_phase.ph_head -= 2;
            break;
          }
          break;
        }
      }

      /* Render the scene, which includes the background as well as the car. */
      SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0xFF);

      SDL_RenderClear(r);
      SDL_RenderCopy(r, bg, NULL, NULL);

      if (!debug)
        draw_tile(r, car, &out.ph.ph_pos, 360.f - out.ph.ph_head, car_w, car_h);
      else {
        /* In debug mode, render the car as a plain square. */
        Globals__phase *ph; Globals__position endp; float f = 20.0;
        if (top) {
          ph = &out.ph;
          f *= ph->ph_vel / SPEED_MAX;
        } else
          ph = &map->init_phase;
        draw_rectangle(r, &ph->ph_pos, 5, 0x00, 0x00, 0x00);
        /* Draw direction vector. */
        endp.x = ph->ph_pos.x + f * cos(ph->ph_head / 360. * 2. * M_PI);
        endp.y = ph->ph_pos.y + f * sin(ph->ph_head / 360. * 2. * M_PI);
        SDL_SetRenderDrawColor(r, 0x00, 0x00, 0x00, 0x00);
        draw_line(r, &ph->ph_pos, &endp);
        /* Draw detected map vector. */
        Map__lookup_pos_out out;
        Map__lookup_pos_step(ph->ph_pos, &out);
        endp.x = ph->ph_pos.x + out.data.dir_x;
        endp.y = ph->ph_pos.y + out.data.dir_y;
        SDL_SetRenderDrawColor(r, 0xAE, 0xB4, 0xC0, 0x00);
        draw_line(r, &ph->ph_pos, &endp);
        /* Reset color. */
        SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0);
      }

      /* We draw the signalization info, when relevant. */
      if (!debug) {
        for (size_t i = 0; i < MAX_OBST_COUNT; i++) {
          Globals__obstacle *o = &out.sign.si_obstacles[i];
          if (o->o_pres)
            draw_tile(r, obs, &o->o_pos, 0.f, obs_w, obs_h);
        }

        for (size_t i = 0; i < MAX_TL_COUNT; i++) {
          Utilities__encode_color_out enc;
          Globals__traflight *t = &out.sign.si_tlights[i];
          Utilities__encode_color_step(t->tl_color, &enc);
          draw_rectangle(r, &t->tl_pos, 10, enc.a.red, enc.a.green, enc.a.blue);
        }
      }

      /* In debug mode, we also render the raw information coming from the
         map. This is useful to understand the map file contents. */
      if (debug) {
        SDL_SetRenderDrawColor(r, 0x00, 0x00, 0xFF, 0xFF);
        for (size_t i = 0; i < map->road_sz; i++) {
          switch (map->road_arr[i].kind) {
          case RD_LINE_1:
            draw_line(r,
                      &map->road_arr[i].u.line.startp,
                      &map->road_arr[i].u.line.endp);
            break;
          default:
            /* TODO draw curved roads. */
            break;
          }
        }

        /* Draw waypoints. */
        for (size_t i = 0; i < map->wayp_sz; i++)
          draw_rectangle(r, &map->wayp_arr[i].position,
                         10, 0xFF, 0x00, 0x00);

        /* Draw traffic lights. */
        for (size_t i = 0; i < map->tlight_sz; i++)
          draw_rectangle(r, &map->tlight_arr[i].tl.ptl_pos,
                         10, 0x00, 0x00, 0xFF);

        /* Draw stops. */
        for (size_t i = 0; i < map->stop_sz; i++)
          draw_rectangle(r, &map->stop_arr[i].position,
                         10, 0x84, 0x21, 0xFF);

        /* Draw obstacles. */
        for (size_t i = 0; i < map->obst_sz; i++)
          draw_rectangle(r, &map->obst_arr[i].pot_pos,
                         10, 0x12, 0xAE, 0x00);
      }

      SDL_RenderPresent(r);
    }

    /* Sleep for our remaining per-simulation time. */
    uint32_t stop_time_ms = SDL_GetTicks();
    uint32_t frame_time_ms = stop_time_ms - start_time_ms;
    if (frame_time_ms < simulation_dt_ms) {
      log_debug("[simulation %08zu] %zu elapsed, sleeping for %zu ms\n",
                current_tick, frame_time_ms, simulation_dt_ms - frame_time_ms);
      SDL_Delay(simulation_dt_ms - frame_time_ms);
    }

    /* Accumulate time for the synchronous step. */
    time_budget_ms += fmax(simulation_dt_ms, frame_time_ms);
  }
  log_info("[simulation %08zu] shutting down, score = %zu, time = %f\n",
           current_tick, out.scoreA, out.time);

  /* Wait for audio queue to be empty. */
  if (audio_device) {
    Uint32 audio_buffered;
    while ((audio_buffered = SDL_GetQueuedAudioSize(audio_device)) != 0)
      SDL_Delay(50);
  }

  free_asset_wav(&collision);
  free_asset_wav(&wrong_dir);
  free_asset_wav(&exit_road);
  free_asset_wav(&light_run);
  free_asset_wav(&speed_excess);

  SDL_DestroyTexture(obs);
  SDL_DestroyTexture(car);
  SDL_DestroyTexture(bg);
  SDL_DestroyRenderer(r);
  SDL_DestroyWindow(w);
  SDL_Quit();

  return res;
}