#include <stdint.h>
#include <stdio.h>
#include <stdarg.h>
#include <sys/types.h>

#define debug(format, ...) printf("DEBUG - " format "\n",##__VA_ARGS__)

volatile u_int32_t ganimede_read_data[1024] __attribute__ ((aligned (1024)));
volatile u_int32_t ganimede_write_data[1024] __attribute__ ((aligned (1024)));

const uint32_t size_bias = 10;

enum OpType{ WRITE_ONE_SHOT, WRITE_LOOP, READ_ONE_SHOT, READ_LOOP };
int ceil_log2(unsigned long long x)
{
  static const unsigned long long t[6] = {
    0xFFFFFFFF00000000ull,
    0x00000000FFFF0000ull,
    0x000000000000FF00ull,
    0x00000000000000F0ull,
    0x000000000000000Cull,
    0x0000000000000002ull
  };

  int y = (((x & (x - 1)) == 0) ? 0 : 1);
  int j = 32;
  int i;

  for (i = 0; i < 6; i++) {
    int k = (((x & t[i]) == 0) ? 0 : j);
    y += k;
    x >>= k;
    j >>= 1;
  }

  return y;
}
// Address will forcibly floored to be aligned to KB.
// byteSize must be smaller than 
u_int32_t create_ganimede_instruction(u_int32_t address, enum OpType optype, u_int32_t byteSize){
  u_int32_t addressPart = (address & 0xfffffc00);
  u_int32_t optypePart = 0x00000000;
  u_int32_t byteSizePart = 0;

  byteSize = byteSize > 1 << 25? 1<<25 :byteSize;
  byteSizePart = ceil_log2(byteSize) - size_bias;
  byteSizePart = (byteSizePart & 0xf) << 6;
  switch (optype) {
    case WRITE_ONE_SHOT: optypePart |= 0b001000; break;
    case WRITE_LOOP:     optypePart |= 0b011000; break;
    case READ_ONE_SHOT:  optypePart |= 0b000000; break;
    case READ_LOOP:      optypePart |= 0b010000; break;
  }
  return addressPart | optypePart | byteSizePart;
}

char* show_optype(u_int32_t optype){
  switch ((optype & 0b111000) >> 3)  {
    case 0b001: return "WRITE_ONE_SHOT"; break;
    case 0b011: return "WRITE_LOOP"; break;
    case 0b000: return "READ_ONE_SHOT"; break;
    case 0b010: return "READ_LOOP"; break;
  }
  return "UNKNOWN";
}

void print_gan_inst(u_int32_t inst){
  int size = ((inst & 0b1111000000) >> 6) + size_bias;
  if (size >= 30){
    debug(        "command vector is |    address: %#011x\n"
          "                          |  operation: %s\n" 
          "                          |       size: %d MB\n"
          , inst & 0xfffffc00, show_optype(inst), 1 << (size - 30));
  } else if (size >= 20){
    debug(        "command vector is |    address: %#011x\n"
          "                          |  operation: %s\n" 
          "                          |       size: %d MB\n"
          , inst & 0xfffffc00, show_optype(inst), 1 << (size - 20));
  } else if(size >= 10){
    debug(        "command vector is |    address: %#011x\n"
          "                          |  operation: %s\n" 
          "                          |       size: %d KB\n"
          , inst & 0xfffffc00, show_optype(inst), 1 << (size - 10));
  } else {
    debug(        "command vector is |    address: %#011x\n"
          "                          |  operation: %s\n" 
          "                          |       size: %d B\n"
          , inst & 0xfffffc00, show_optype(inst), 1 << size);
  }
}

int main(){
  volatile u_int32_t* data_ptr = (u_int32_t*) 0x10000000;
  volatile u_int32_t* cfg_ptr = (u_int32_t*) 0x80000d00;
  //for(int i = 0; i < 10; i++){
  //  ((int*) (0x80000000))[i] = 0x12345678 + i;
  //}
  printf("\n");
  debug("Initializing socbridge and GANIMEDE");
  debug("setting socbridge config");
  // Setting omaxsz = 7 and orxqen = true
  cfg_ptr[0] = 0x0001bfff;
  cfg_ptr[1] = 0x00000000;
  debug("setting parif config.");
  cfg_ptr[4] = 0x00000071;
  cfg_ptr[5] = 0x00001002;

  debug("initializing test data to read");
  for(int i = 0; i < sizeof(ganimede_read_data)/sizeof(u_int32_t); i++){
    ganimede_read_data[i] = i;
  }

  debug("Finished initializing\n");



  debug("Sending commands to GANIMEDE");
  debug("creating and sending a read instruction");
  int readInst = create_ganimede_instruction((u_int32_t) &ganimede_read_data[0], READ_ONE_SHOT, 1024 );
  print_gan_inst(readInst);
  //data_ptr[0] = readInst;

  debug("creating and sending a write instruction");
  int writeInst = create_ganimede_instruction((u_int32_t) &ganimede_write_data[0], WRITE_ONE_SHOT, 1024);
  print_gan_inst(writeInst);
  //data_ptr[1] = writeInst;

  debug("Finished sending commands\n");
  return 0;
}