#include <io.h>
#include <stdio.h>
#include <system.h>
#include <sys/time.h>
#include <sys/alt_timestamp.h>

#define VIDEO_BASE 0x080A00
#define CONTROLLER_BASE 0x080808
#define POS_OFFSET 0
#define SCORE_OFFSET 8
#define DOTS_OFFSET 16
#define TOTAL_DOTS 100

#define SLOW_SPEED 1<<11
#define FAST_SPEED 1<<10
#define INIT_POWERUP_COUNTER 350
/*
 * "Hello World" example.
 *
 * This example prints 'Hello from Nios II' to the STDOUT stream. It runs on
 * the Nios II 'standard', 'full_featured', 'fast', and 'low_cost' example
 * designs. It runs with or without the MicroC/OS-II RTOS and requires a STDOUT
 * device in your system's hardware.
 * The memory footprint of this hosted application is ~69 kbytes by default
 * using the standard reference design.
 *
 * For a reduced footprint version of this template, and an explanation of how
 * to reduce the memory footprint for a given application, see the
 * "small_hello_world" template.
 *
 */
 #define IOWR_POS(player, position) \
  IOWR_32DIRECT(VIDEO_BASE+POS_OFFSET, player*4, position)
 #define IOWR_SCORE(player, data) \
  IOWR_32DIRECT(VIDEO_BASE+SCORE_OFFSET, player*4, data)
 #define IOWR_DOTS(line, dots) \
  IOWR_32DIRECT(VIDEO_BASE+DOTS_OFFSET, line*4, dots)
 #define IO_READ_NES(player) \
  IORD_8DIRECT(CONTROLLER_BASE, player)

 #define LEFT 0
 #define UP 1
 #define RIGHT 2
 #define DOWN 3
 #define NONE 4
 
unsigned long maze[30] = {0x0FFFFFFF, 0x08006001, 0x0BDF6FBD,
                          0x0BDF6FBD, 0x08000001, 0x0BDBFDBD,
                          0x0BDBFDBD, 0x08186181, 0x0FDF6FBF,
                          0x0FDF6FBF, 0x0FD801BF, 0x0FDBFDBF,
                          0x0FDBFDBF, 0x0803FC01, 0x0FDBFDBF,
                          0x0FDBFDBF, 0x0FD801BF, 0x0FDBFDBF,
                          0x0FDBFDBF, 0x08006001, 0x0BDF6FBD,
                          0x0BDF6FBD, 0x08C00031, 0x0EDBFDB7,
                          0x0EDBFDB7, 0x08186181, 0x0BFF6FFD,
                          0x0BFF6FFD, 0x08000001, 0x0FFFFFFF};
unsigned long dots[30] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
                          0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF};


int move(int player, int pos_x, int pos_y, int direction) {
    unsigned int maze_x, maze_y;
    unsigned int maze_x2, maze_y2;
    if (direction == LEFT) pos_x--;
    if (direction == UP) pos_y--;
    if (direction == RIGHT) pos_x++;
    if (direction == DOWN) pos_y++;
    maze_x = (pos_x >> 4) - 5;
    maze_y = (pos_y >> 4);
    maze_x2 = ((pos_x+15) >> 4) - 5;
    maze_y2 = ((pos_y+15) >> 4);
    
//    printf(" tile (%d,%d)\n",maze_x,maze_y);
    //printf("new spot: %xu", (maze[maze_y] & (1 << (28-maze_x))));
    if ((maze[maze_y] & (1 << (28-maze_x)))) //if there is a maze tile where we want to go.
        return 0;
    if ((maze[maze_y] & (1 << (28-maze_x2))))
        return 0;
    if ((maze[maze_y2] & (1 << (28-maze_x)))) //if there is a maze tile where we want to go.
        return 0;
    if ((maze[maze_y2] & (1 << (28-maze_x2))))
        return 0;
//    printf("moving to x:%d y%d\n", pos_x, pos_y);
    IOWR_POS(player, pos_x+(pos_y<<16));
    return 1;
}

int transform_board_to_screen_x(board_x) {return (board_x + 6) * 16;}
int transform_screen_to_board_x(screen_x) {return (screen_x / 16) - 6;}
int transform_board_to_screen_y(board_y) {return (board_y) *16;}
int transform_screen_to_board_y(screen_y) {return (screen_y / 16);}

int transform_screen_to_dots_x(screen_x) {return (screen_x/16 - 6);}
int transform_screen_to_dots_y(screen_y) {return (screen_y / 16) - 1;}

int read_direction(int player) {
    int status = IO_READ_NES(player);
    if (status & 0x80) return RIGHT;
    else if (status & 0x20) return DOWN;
    else if (status & 0x40) return LEFT;
    else if (status & 0x10) return UP;
    else return NONE;
}

int reset_condition() {
    int status1 = IO_READ_NES(0), status2 = IO_READ_NES(1);
    if ((status1 & 0x08) && (status2 & 0x08)) return 1;
    return 0;
}

int reset_game() {
    unsigned int x1 = transform_board_to_screen_x(26), y1 = transform_board_to_screen_y(13),
                 x2 = transform_board_to_screen_x(1), y2 = transform_board_to_screen_y(13), i;
    IOWR_SCORE(0, 0x00000000);
    IOWR_SCORE(1, 0x00000000);        
    for (i = 0; i <= 27; i++) {
        IOWR_DOTS(i,0xFFFFFFFF);
        dots[i] = 0xFFFFFFFF;
    }
    IOWR_POS(0, x1+(y1<<16));
    IOWR_POS(1, x2+(y2<<16));
}

int main()
{
  srand();
  int i = 0;
  unsigned int x1 = transform_board_to_screen_x(1), y1 = transform_board_to_screen_y(13),
               x2 = transform_board_to_screen_x(26), y2 = transform_board_to_screen_y(13);
  unsigned int hitx, hity, hitdot;
  unsigned int p1_score = 0, p2_score = 0;
  unsigned int p1_counter = 0, p2_counter = 0, p1_threshold = SLOW_SPEED, p2_threshold = SLOW_SPEED;
  unsigned int p1_powerup_counter = 0, p2_powerup_counter = 0;
  unsigned int last_direction_1 = NONE, last_direction_2 = NONE, temp;
  unsigned int next_direction_1 = NONE, next_direction_2 = NONE;
  int move_result = 0;
    IOWR_SCORE(0, 0x00000000);
    IOWR_SCORE(1, 0x00000000);        
    for (i = 0; i <= 27; i++) {
        IOWR_DOTS(i,dots[i]);
    }
    IOWR_POS(0, x1+(y1<<16));
    while (1) {
        p1_counter++;
        p2_counter++;
        p1_counter %= p1_threshold;
        p2_counter %= p2_threshold;
        //write player_1 score
        if (p1_counter == 0) {
            printf("p1: %x\n", IO_READ_NES(0));
            if (p1_powerup_counter > 0) p1_powerup_counter--;
            else p1_threshold = SLOW_SPEED;
            IOWR_SCORE(0, p1_score%10 + (((p1_score/10)%10)<<4) + (((p1_score/100)%10)<<8));
            if((dots[transform_screen_to_dots_y(y1+6)] & (1<<transform_screen_to_dots_x(x1+8)))) {
               hitx = x1+8;
               hity = y1+6;
               hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+8)] & (1<<transform_screen_to_dots_x(x1+6))) {
                hitx = x1+6;
                hity = y1+8;
                hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+10)] & (1<<transform_screen_to_dots_x(x1+8))) {
                hitx = x1+8;
                hity = y1+10;
                hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+8)] & (1<<transform_screen_to_dots_x(x1+10))) {
                hitx = x1+10;
                hity = y1+8;
                hitdot = 1;
            }
            else hitdot = 0;
//                printf("x1: %d, y1: %d, dots: %d,%d , maze_x: %d, maze_y: %d\n", x1, y1, transform_screen_to_dots_x(x1), transform_screen_to_dots_y(y1), transform_screen_to_board_x(x1), transform_screen_to_board_y(y1));
            if (hitdot == 1) {
                if (transform_screen_to_dots_y(hity) == 1 && transform_screen_to_dots_x(hitx) == 26)
                    {p1_threshold = FAST_SPEED; p1_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(hity) == 24 && transform_screen_to_dots_x(hitx) == 26)
                    {p1_threshold = FAST_SPEED; p1_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(hity) == 1 && transform_screen_to_dots_x(hitx) == 1)
                    {p1_threshold = FAST_SPEED; p1_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(hity) == 24 && transform_screen_to_dots_x(hitx) == 1)
                    {p1_threshold = FAST_SPEED; p1_powerup_counter = INIT_POWERUP_COUNTER;}

                dots[transform_screen_to_dots_y(hity)] = dots[transform_screen_to_dots_y(hity)] & ~(1<<transform_screen_to_dots_x(hitx));
                IOWR_DOTS(transform_screen_to_dots_y(hity), dots[transform_screen_to_dots_y(hity)]);
                p1_score++;
            }
            temp = read_direction(0);
            if (temp != NONE) next_direction_1 = temp;
            move_result = move(0, x1, y1, next_direction_1);
            if (move_result != 0) last_direction_1 = next_direction_1;
            else move_result = move(0, x1, y1, last_direction_1);
            if (move_result != 0) {
                switch (last_direction_1) {
                 case LEFT:
                   x1--;
                   break;
                 case UP:
                   y1--;
                   break;
                 case RIGHT:
                   x1++;
                   break;
                 case DOWN:
                   y1++;
                   break;
                }
            }
            if (reset_condition()) {
                last_direction_1 = NONE;
                last_direction_2 = NONE;
                next_direction_1 = NONE; next_direction_2 = NONE;
                x1 = transform_board_to_screen_x(1); y1 = transform_board_to_screen_y(13);
                x2 = transform_board_to_screen_x(26); y2 = transform_board_to_screen_y(13);
                p1_score = 0; p2_score = 0; p1_threshold = SLOW_SPEED; p2_threshold = SLOW_SPEED;
                p1_powerup_counter = 0; p2_powerup_counter = 0;
                reset_game();
            }
        }
        if (p2_counter == 0) {
            printf("p2: %x\n", IO_READ_NES(1));
            //write player_2 score
            IOWR_SCORE(1, p2_score%10 + (((p2_score/10)%10)<<4) + (((p2_score/100)%10)<<8));
            if (p2_powerup_counter > 0) p2_powerup_counter--;
            else p2_threshold = SLOW_SPEED;
  
            if((dots[transform_screen_to_dots_y(y1+6)] & (1<<transform_screen_to_dots_x(x1+8)))) {
               hitx = x1+8;
               hity = y1+6;
               hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+8)] & (1<<transform_screen_to_dots_x(x1+6))) {
                hitx = x1+6;
                hity = y1+8;
                hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+10)] & (1<<transform_screen_to_dots_x(x1+8))) {
                hitx = x1+8;
                hity = y1+10;
                hitdot = 1;
            }
            else if(dots[transform_screen_to_dots_y(y1+8)] & (1<<transform_screen_to_dots_x(x1+10))) {
                hitx = x1+10;
                hity = y1+8;
                hitdot = 1;
            }
            else hitdot = 0;
//                printf("x1: %d, y1: %d, dots: %d,%d , maze_x: %d, maze_y: %d\n", x1, y1, transform_screen_to_dots_x(x1), transform_screen_to_dots_y(y1), transform_screen_to_board_x(x1), transform_screen_to_board_y(y1));
            if (hitdot == 1) {
                if (transform_screen_to_dots_y(y2) == 1 && transform_screen_to_dots_x(x2) == 26)
                    {p2_threshold = FAST_SPEED; p2_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(y2) == 24 && transform_screen_to_dots_x(x2) == 26)
                    {p2_threshold = FAST_SPEED; p2_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(y2) == 1 && transform_screen_to_dots_x(x2) == 1)
                    {p2_threshold = FAST_SPEED; p2_powerup_counter = INIT_POWERUP_COUNTER;}
                if (transform_screen_to_dots_y(y2) == 24 && transform_screen_to_dots_x(x2) == 1)
                    {p2_threshold = FAST_SPEED; p2_powerup_counter = INIT_POWERUP_COUNTER;}

                dots[transform_screen_to_dots_y(y2)] = dots[transform_screen_to_dots_y(y2)] & ~(1<<transform_screen_to_dots_x(x2));
                IOWR_DOTS(transform_screen_to_dots_y(y2), dots[transform_screen_to_dots_y(y2)]);
                p2_score++;
            }
            temp = read_direction(1);
            if (temp != NONE) next_direction_2 = temp;
            move_result = move(1, x2, y2, next_direction_2);
            if (move_result != 0) last_direction_2 = next_direction_2;
            else move_result = move(1, x2, y2, last_direction_2);
            if (move_result != 0) {
                switch (last_direction_2) {
                 case LEFT:
                   x2--;
                   break;
                 case UP:
                   y2--;
                   break;
                 case RIGHT:
                   x2++;
                   break;
                 case DOWN:
                   y2++;
                   break;
                }
            }
        }
    }
    return 0;
}