#include "ps2_mouse.h"
#include "alt_up_ps2_port.h"
#include "basic_io.h"

#define X_OFFSET 0
#define Y_OFFSET 2
#define RADIUS 50


//void clearscreen() //clear the screen
//{
//    int i,j;
//    for (i=0;i<640;i++)
//      for (j=0;j<480;j++)
//        Vga_Clr_Pixel(VGA_0_BASE,i,j);
//    
//}

int trace()
{
unsigned int curX=400, curY=273, movX=0, movY=0;
int status;
unsigned char byte0, byte1, byte2, signX, signY;
//unsigned int buffer;
//clearscreen();
////Initialize VGA display
//VGA_Ctrl_Reg vga_ctrl_set;
//  
//vga_ctrl_set.VGA_Ctrl_Flags.RED_ON    = 1;
//vga_ctrl_set.VGA_Ctrl_Flags.GREEN_ON  = 1;
//vga_ctrl_set.VGA_Ctrl_Flags.BLUE_ON   = 1;
//vga_ctrl_set.VGA_Ctrl_Flags.CURSOR_ON = 0;
//  
//Vga_Write_Ctrl(VGA_0_BASE, vga_ctrl_set.Value);
//Set_Pixel_On_Color(0,0,0);
//Set_Pixel_Off_Color(400,596,948);
//Set_Cursor_Color(1000,640,0);
  
//Vga_Set_Pixel(VGA_0_BASE, curX,curY); //set initial cursor position


//initialize PS/2 mouse
//printf("Initialize the mouse\n");  

clear_FIFO();
switch (get_mode()) {
  case PS2_KEYBOARD:
    //printf("Keyboard detected on PS/2 port\n");
    break;
  case PS2_MOUSE:
    //printf("Yes! Mouse detected on PS/2 port\n");
    break;
  default:
    //printf("Error: Unrecognized or no device on PS/2 port\n");
    return;
 
  }
  
 //Enable Stream Mode
 //if ( reset_mouse() == PS2_SUCCESS ) printf("MOUSE RESETTED!");
 
 status = write_data_byte_with_ack(0xF0, DEFAULT_PS2_TIMEOUT_VAL); 
// if (status == PS2_SUCCESS) printf("Remote Mode Set!\n");
// else printf("Fail to set Remote Mode!\n");

int counter=0;

//usleep(50);

while(1)
   {
//read data from mouse
status = write_data_byte_with_ack(0xEB, DEFAULT_PS2_TIMEOUT_VAL); 

while(0xFA == (0xFF & IORD_ALT_UP_PS2_PORT_DATA(ALT_UP_PS2_BASE)));
usleep(2000);
byte0=IORD_ALT_UP_PS2_PORT_DATA(ALT_UP_PS2_BASE);
usleep(2000);
byte1=IORD_ALT_UP_PS2_PORT_DATA(ALT_UP_PS2_BASE);
usleep(2000);
byte2=IORD_ALT_UP_PS2_PORT_DATA(ALT_UP_PS2_BASE);
//msleep(1);

//buffer = read_data_reg();
//
//printf("buffer is %x\n", buffer);
//
//   // if (status == PS2_SUCCESS) {
//    byte0 = (alt_u8) ((buffer >>16) & 0xff);
//    byte1 = (alt_u8) ((buffer >>8) & 0xff);
//    byte2 = (alt_u8) (buffer & 0xff);

   
    //printf("byte0 = %x", byte0);
    //printf(" byte1 = %x", byte1);
    //printf(" byte2 = %x\n", byte2);
    
    counter = counter + 1;
    if (counter == 10000) { //printf("Counter 10000 Ends! \n"); break; }
    
    signX= (byte0 >>4) & 0x01 ;
    signY= (byte0 >>5) & 0x01 ; 
    
    //movX = byte1 >> 2; // X: complement to positive
    //movY = byte2 >> 2; // Y: complement to positive
    //if ( movX > 20 ) movX = 5;
    //if ( movY > 20 ) movY = 5;
    
    if (signX == 1) {
        movX = (255 - byte1) ; // X: go left
        if ( movX > 40 ) movX = 40;
        curX = curX - movX;
        
    }
    else {
        movX = byte1 ; // X: go right
        if ( movX > 40 ) movX = 40;
        curX = curX + movX;
    }
     
    if (signY == 1) {
        movY = (255 - byte2) ; // Y: go down
        if ( movY > 40 ) movY = 40;
        curY = curY + movY ;
    }
    else {
        movY = byte2 ; // Y: go up
        if ( movY > 40 ) movY = 40;
        curY = curY - movY;
        //if ( curY > 480) curY = 480;
    }
    
        
    //if (curX > 640) curX = 640;
    //if (curY > 480) curY = 480;
    
//    printf("signX = %d ", signX);
//    printf("signY = %d ", signY);
//    printf("movX = %d ", movX);
//    printf("movY = %d ", movY);
//    printf("curX = %d ", curX); 
//    printf("curY = %d\n", curY);
    
    //clearscreen();
    //Set_Cursor_XY(curX, curY);
    //Vga_Set_Pixel(VGA_0_BASE, curX, curY);
    //put_vga_char("b", 20, 20);
    //usleep(100);
    
        // Move circle
    IOWR_16DIRECT(VGA_BASE, X_OFFSET, curX);
    IOWR_16DIRECT(VGA_BASE, Y_OFFSET, curY);
        
    }   
}
}