#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#include <raylib.h>
#include <raymath.h>

#include "types.h"
#include "game.h"

MachineDef machine_dict[] = {
  {{},               0, R_Coal,  4}, // MT_CoalMine
  {{},               0, R_Iron,  4}, // MT_IronMine
  {{R_Coal, R_Iron}, 2, R_Steel, 0}, // MT_Furnace 
};

int init_machines(Machines *machines){
  machines->machine_n = 0;
  machines->machine_id = 0;

  machines->machine_stack = malloc(sizeof(Worker)*W_ALLOC_BLOCK);
  if(!machines->machine_stack)
    return 1;

  return 0;
}

int init_workers(Workers *workers){
  workers->worker_n = 0;

  workers->worker_stack = malloc(sizeof(Worker)*W_ALLOC_BLOCK);
  if(!workers->worker_stack)
    return 1;

  return 0;
}

int init_game(Game *game){
  int err = init_machines(&game->machines);
  err |= init_workers(&game->workers);
  if(err){
    printf("Failed to init game. Error : %x\n", err);
    return 1;
  }
  return 0;
}

void clean_machines(Machines *machines){
  free(machines->machine_stack);
}

void clean_workers(Workers *workers){
  free(workers->worker_stack);
}

void clean_game(Game *game){
  clean_machines(&game->machines);
  clean_workers(&game->workers);
}

Worker *add_worker(Workers *workers, V2d pos){
  if((workers->worker_n%W_ALLOC_BLOCK)+1 >= W_ALLOC_BLOCK){
    void *tmp = realloc(workers->worker_stack, 
                          (workers->worker_n+W_ALLOC_BLOCK)*sizeof(Worker));
    if(!tmp){
      printf("Failed to extend worker stack !\n");
      return NULL;
    }

    workers->worker_stack = tmp;
  }

  Worker *work = &workers->worker_stack[workers->worker_n++];
  work->pos = pos;
  work->ai = (AiInternal){S_Idle, 0, 0, 0, -1, -1, {.x=0,.y=0}};

  return work;
}

Machine *add_machine(Machines *machines, V2d pos){
  if((machines->machine_n%M_ALLOC_BLOCK)+1 >= M_ALLOC_BLOCK){
    void *tmp = realloc(machines->machine_stack, 
                          (machines->machine_n+W_ALLOC_BLOCK)*sizeof(Machine));
    if(!tmp){
      printf("Failed to extend machine stack !\n");
      return NULL;
    }

    machines->machine_stack = tmp;
  }

  Machine *mach = &machines->machine_stack[machines->machine_n++];
  mach->id = machines->machine_id++;
  mach->pos = pos;
  mach->assign_workers = 0;

  return mach;
}

int id_cmpfunc(const void *a_, const void *b_){
  Machine *a = a_;
  Machine *b = b_;
  return a->id - b->id;
}

Machine *get_machine_from_id(Machines *machines, int id){
  Machine tmach = {.id = id};
  Machine *match = bsearch(&tmach, machines->machine_stack, machines->machine_n,
                            sizeof(Machine), &id_cmpfunc);
  return match;
}

int get_machine_localres(Machine *machine, int globalres){
  MachineDef *def = &machine_dict[machine->type];
  for(int i = 0; i < 4; i++){
    if(def->inputs[i] == globalres)
      return i;
  }
  return -1;
}

void update_worker(Game *game, Worker *worker){
  AiInternal *ai = &worker->ai;
  if(ai->assigned_machine < 0 && ai->assigned_machine1 < 0)
   ai->status = S_Idle;

  Machine *mach[2];
  MachineDef *defs[2];

  if(ai->assigned_machine > -1){
    mach[0] = get_machine_from_id(&game->machines, ai->assigned_machine);
    if(!mach[0])
      ai->assigned_machine = -1;
    else
      defs[0] = &machine_dict[mach[0]->type];
  }
  if(ai->assigned_machine1 > -1){
    mach[1] = get_machine_from_id(&game->machines, ai->assigned_machine1);
    if(!mach[1])
      ai->assigned_machine1 = -1;
    else
      defs[1] = &machine_dict[mach[1]->type];
  }

  if(ai->assigned_machine < 0 && ai->assigned_machine1 < 0)
    ai->status = S_Idle;

  //TODO : Move !
  switch(ai->status){
    default:
      break;
    case S_Loading:
      if(!Vector2Equals(worker->pos,mach[0]->pos)){
        ai->status = S_Transporting;
        ai->going_to = 0;
        ai->dest = mach[0]->pos;
        break;
      }
      if(ai->carrying <= 50 && mach[0]->storage[4]){
        ai->carrying++;
        mach[0]->storage[4]--;
      }
      else {
        ai->status = S_Transporting;
        ai->dest = mach[1]->pos;
        ai->going_to = 1;
      }
      break;
    case S_Unloading:
      int localres = get_machine_localres(mach[1], ai->carry_type);
      if(localres != (int)ai->carry_type){
        ai->status = S_Idle;
        break;
      }
      if(!Vector2Equals(worker->pos, mach[1]->pos)){
        ai->status = S_Transporting;
        ai->dest = mach[1]->pos;
        ai->going_to = 1;
        break;
      }
      if(ai->carrying && mach[1]->storage[localres] <= 200){
        ai->carrying--;
        mach[1]->storage[localres]++;
      }
      else{
        ai->status = S_Transporting;
        ai->dest = mach[0]->pos;
        ai->going_to = 0;
      }
      break;
    case S_Operating:
      worker->pos = mach[0]->pos;
      break;
    case S_Transporting:
    case S_Walking:
      worker->pos = ai->dest;
      if(Vector2Equals(worker->pos, mach[0]->pos))
        ai->status = S_Loading;
      else if(Vector2Equals(worker->pos, mach[1]->pos))
        ai->status = S_Unloading;
      break;
  }
}

void update_workers(Game *game){
  for(int i = 0; i < game->workers.worker_n; i++){
    update_worker(game, &game->workers.worker_stack[i]);
  }
}

void update_machine(Game *game, Machine *machine){
  MachineDef *def = &machine_dict[machine->type];
  bool can_output = machine->assign_workers == def->workers_required;
  can_output = can_output && (machine->storage[4] <= 200);
  for(int i = 0; i < def->input_n; i++){
    if(!machine->storage[i])
      can_output = false;
  }
  if(can_output){
    for(int i = 0; i < def->input_n; i++){
      machine->storage[i]--;
    }
    machine->storage[4]++;
  }
}

void update_machines(Game *game){
  for(int i = 0; i < game->machines.machine_n; i++){
    update_machine(game, &game->machines.machine_stack[i]);
  }
}

void update(Game *game){
  update_machines(game);
  update_workers(game);
}

int assign_worker_machine(Machine *machine, Worker *worker){
  machine->assign_workers++;
  AiInternal *ai = &worker->ai;
  ai->assigned_machine = machine->id;
  ai->assigned_machine1 = -1;
  ai->status = S_Operating;
  return 0;
}

int assign_worker_fetch(Machine *a, Machine *b, Worker *worker){
  AiInternal *ai = &worker->ai;
  ai->carry_type = machine_dict[a->id].output;
  ai->assigned_machine = a->id;
  ai->assigned_machine1 = b->id;
  ai->status = S_Loading;
  return 0;
}