U0 GridInit()
{//Init mouse grid struct. See ::/Demo/Graphics/Grid.CC.
        mouse_grid.x            = mouse_grid.y                  = mouse_grid.z                  = 8;
        mouse_grid.x_offset     = mouse_grid.y_offset   = mouse_grid.z_offset   = 0;
        mouse_grid.x_speed      = mouse_grid.y_speed    = mouse_grid.z_speed    = 1;
        mouse_grid.show         = mouse_grid.snap               = mouse_grid.coord              = FALSE;
}

U0 MouseUpdate(I64 x, I64 y, I64 z, Bool l, Bool r)
{
        mouse.presnap.x = ToI64(mouse.scale.x * x) + mouse.offset.x;
        mouse.presnap.y = ToI64(mouse.scale.y * y) + mouse.offset.y;
        mouse.presnap.z = ToI64(mouse.scale.z * z) + mouse.offset.z;
        if (mouse_grid.snap)
        {
                mouse.pos.x = Trunc(mouse.presnap.x / mouse_grid.x) * mouse_grid.x + mouse_grid.x_offset;
                mouse.pos.y = Trunc(mouse.presnap.y / mouse_grid.y) * mouse_grid.y + mouse_grid.y_offset;
                mouse.pos.z = Trunc(mouse.presnap.z / mouse_grid.z) * mouse_grid.z + mouse_grid.z_offset;
        }
        else
        {
                mouse.pos.x = mouse.presnap.x;
                mouse.pos.y = mouse.presnap.y;
                mouse.pos.z = mouse.presnap.z;
        }

        mouse.pos.x = ClampI64(mouse.pos.x, 0, sys_vbe_mode.width  - 1);
        mouse.pos.y = ClampI64(mouse.pos.y, 0, sys_vbe_mode.height - 1);
        mouse.pos_text.x = mouse.pos.x / FONT_WIDTH;
        if (mouse.pos_text.x >= text.cols)
        {
                mouse.pos_text.x = text.cols - 1;
                mouse.pos.x              = text.cols * FONT_WIDTH - 1;
        }
        mouse.pos_text.y = mouse.pos.y / FONT_HEIGHT;
        if (mouse.pos_text.y >= text.rows)
        {
                mouse.pos_text.y = text.rows - 1;
                mouse.pos.y              = text.rows * FONT_HEIGHT - 1;
        }
        mouse.lb = l;
        mouse.rb = r;
        LBEqual(&kbd.scan_code, SCf_MS_L_DOWN, mouse.lb);
        LBEqual(&kbd.scan_code, SCf_MS_R_DOWN, mouse.rb);
}

U0 MouseSet(I64 x=I64_MAX, I64 y=I64_MAX, I64 z=I64_MAX, I64 l=I64_MAX, I64 r=I64_MAX)
{//Note: Generates a message. See MouseSet().
        if (!(0 <= x < sys_vbe_mode.width))
                x = mouse.pos.x;
        if (!(0 <= y < sys_vbe_mode.height))
                y = mouse.pos.y;
        if (z == I64_MAX)
                z = mouse.pos.z;

        if (!(FALSE <= l <= TRUE))
                l = mouse.lb;
        if (!(FALSE <= r <= TRUE))
                r = mouse.rb;

        x = (x - mouse.offset.x) / mouse.scale.x;
        y = (y - mouse.offset.y) / mouse.scale.y;
        z = (z - mouse.offset.z) / mouse.scale.z;
        MouseUpdate(x, y, z, l, r);
        MouseHardSet(x, y, z, l, r);
}

U0 MouseInit()
{
        MemSet(&mouse, 0, sizeof(CMouseStateGlobals));
        MemSet(&mouse_last, 0, sizeof(CMouseStateGlobals));
        mouse.offset.x   = mouse.offset.y       = mouse.offset.z        = 0;
        mouse.scale.x    = mouse.scale.y        = mouse.scale.z         = 1.0;
        mouse.pos_text.x = mouse.pos_text.y = mouse.pos_text.z  = 0;
        mouse.has_wheel  = FALSE;
        mouse.show               = TRUE;
        mouse.speed              = 0;
        mouse.timestamp  = TSCGet;
        mouse.dbl_time   = 0.175;
        GridInit;
}

U0 MouseHardPacketRead()
{
        U8 j;

        if (TSCGet > mouse_hard.timestamp + counts.time_stamp_freq >> 3)
                FifoU8Flush(mouse_hard.fifo);
        mouse_hard.timestamp = TSCGet;
        FifoU8Insert(mouse_hard.fifo, InU8(KBD_PORT));
        if (FifoU8Count(mouse_hard.fifo) == mouse_hard.pkt_size)
                while (FifoU8Remove(mouse_hard.fifo, &j))
                        FifoU8Insert(mouse_hard.fifo2, j);
}

interrupt U0 IRQMouseHard()
{
        CLD
        OutU8(PIC_2, PIC_EOI);
        OutU8(PIC_1, PIC_EOI);
        mouse_hard.irqs_working = TRUE;
        if (mouse_hard.install_in_progress || !mouse_hard.installed)
        {
                kbd.reset = TRUE;
                return;
        }
        MouseHardPacketRead;
}

U0 MouseHardGetType()
{
        I64 b;

        KbdMouseCmdAck(0xF2);
        b = KbdCmdRead;
        if (b == 3)
                mouse_hard.has_wheel = TRUE;
        else if (b == 4)
                mouse_hard.has_ext_bttns = TRUE;
}

Bool MouseHardReset()
{
        U8   b, *_b;
        F64  timeout;
        Bool res = FALSE;

        mouse_hard.has_wheel     = FALSE;
        mouse_hard.has_ext_bttns = FALSE;

        if (*0x40E(U16 *) == 0x9FC0)
        {
                _b =  0x9FC00+0x30;
                *_b = 1; //This enables Terry's mouse.  It might be for one machine.
//USB DMA packets, set-up by BIOS to make legacy PS/2?
        }

        try
        {
                KbdCmdFlush;
                KbdCmdSend(KBD_CTRL, 0xAD); //Disable Kbd
                KbdCmdSend(KBD_CTRL, 0xA8); //Enable Mouse

                KbdMouseCmdAck(0xFF); //Reset

                timeout = tS + 10.0;
                do
                        try
                        {
                                KbdCmdRead;
                                timeout = 0; //force exit
                        }
                        catch
                                Fs->catch_except = TRUE;
                while (tS < timeout);

                try
                        KbdCmdRead;
                catch
                        Fs->catch_except = TRUE;

                KbdMouseCmdAck(0xF3, 200, 0xF3, 100, 0xF3, 80);
                MouseHardGetType;
                KbdMouseCmdAck(0xF3, 10);
                MouseHardGetType;
                KbdMouseCmdAck(0xE8, 0x03, 0xE6, 0xF3, 100, 0xF4);
                res = TRUE;

                //Enable IRQ 12
                KbdCmdSend(KBD_CTRL, 0x20);
                b = KbdCmdRead;
                KbdCmdSend(KBD_CTRL, 0x60);
                KbdCmdSend(KBD_PORT, (b | 2) & ~0x20);

        }
        catch
                Fs->catch_except = TRUE;

        //This is been added to override failure
        //because the mouse sometimes still works.
        res = TRUE;

        try
                KbdCmdSend(KBD_CTRL, 0xAE); //Enable Keyboard
        catch
                Fs->catch_except = TRUE;
        if (mouse_hard.has_wheel || mouse_hard.has_ext_bttns)
                mouse_hard.pkt_size = 4;
        else
                mouse_hard.pkt_size = 3;
        if (!res)
                try
                        KbdCmdSend(KBD_CTRL, 0xA7); //Disable Mouse
                catch
                        Fs->catch_except = TRUE;

        return res;
}

U0 MouseHardSpeedSet()
{
        I64 dd, tmp;

        if ((dd = SqrI64(mouse_hard_last.pos.x - mouse_hard.pos.x) + SqrI64(mouse_hard_last.pos.y - mouse_hard.pos.y)) &&
                        (tmp = mouse_hard.timestamp - mouse_hard_last.timestamp))
                mouse_hard.speed = Sqrt(dd) * counts.time_stamp_freq / tmp;
        mouse_hard_last.timestamp = mouse_hard.timestamp;
}

U0 MouseHardSetPre()
{
        I64 old_timestamp = mouse_hard_last.timestamp;

        MemCopy(&mouse_hard_last, &mouse_hard, sizeof(CMouseHardStateGlobals));
        mouse_hard_last.timestamp = old_timestamp;
}

U0 MouseHardSetPost()
{
        I64 i;

        mouse_hard.pos.x = mouse_hard.prescale.x * mouse_hard.scale.x * mouse_grid.x_speed;
        mouse_hard.pos.y = mouse_hard.prescale.y * mouse_hard.scale.y * mouse_grid.y_speed;
        mouse_hard.pos.z = mouse_hard.prescale.z * mouse_hard.scale.z * mouse_grid.z_speed;

        i = Trunc(mouse.scale.x * mouse_hard.pos.x / mouse_grid.x) * mouse_grid.x + mouse.offset.x;
//TODO mouse_grid.x_offset?
        if (i < 0)
                mouse.offset.x -= i;
        else if (i >= sys_vbe_mode.width)
                mouse.offset.x += sys_vbe_mode.width - 1 - i;

        i = Trunc(mouse.scale.y * mouse_hard.pos.y / mouse_grid.y) * mouse_grid.y + mouse.offset.y;
        if (i < 0)
                mouse.offset.y -= i;
        else if (i >= sys_vbe_mode.height)
                mouse.offset.y += sys_vbe_mode.height - 1 - i;

        if (mouse_hard.pos.x != mouse_hard_last.pos.x || mouse_hard.pos.y != mouse_hard_last.pos.y ||
                mouse_hard.pos.z != mouse_hard_last.pos.z)
        {
                mouse_hard.event = TRUE;
                MouseHardSpeedSet;
        }
        else
                for (i = 0; i < 5; i++)
                        if (mouse_hard.bttns[i] != mouse_hard_last.bttns[i])
                        {
                                mouse_hard.event = TRUE;
                                break;
                        }
}

U0 MouseHardHandler()
{
        I64 i, dx, dy, dz;
        U8  mouse_buf[4];

        MouseHardSetPre;
        for (i = 0; i < 4; i++)
                mouse_buf[i] = 0;
        for (i = 0; i < mouse_hard.pkt_size; i++)
                if (!FifoU8Remove(mouse_hard.fifo2, &mouse_buf[i]))
                        mouse_buf[i] = 0;

        mouse_hard.bttns[0] =  mouse_buf[0] & 1;
        mouse_hard.bttns[1] = (mouse_buf[0] & 2)    >> 1;
        mouse_hard.bttns[2] = (mouse_buf[0] & 4)    >> 2;
        mouse_hard.bttns[3] = (mouse_buf[3] & 0x10) >> 4;
        mouse_hard.bttns[4] = (mouse_buf[3] & 0x20) >> 5;
        if (mouse_buf[0] & 0x10)
                dx = mouse_buf[1]-256;
        else
                dx = mouse_buf[1];
        if (mouse_buf[0] & 0x20)
                dy = 256 - mouse_buf[2];
        else
                dy = -mouse_buf[2];
        if (mouse_buf[3] & 0x08)
                dz = mouse_buf[3] & 7 - 8;
        else
                dz = mouse_buf[3] & 7;

        mouse_hard.prescale.x += dx;
        mouse_hard.prescale.y += dy;
        mouse_hard.prescale.z += dz;

        MouseHardSetPost;
}

U0 MouseHardSet(I64 x, I64 y, I64 z, I64 l, I64 r)
{
        mouse_hard.timestamp = TSCGet;
        MouseHardSetPre;
        mouse_hard.prescale.x   = x / mouse_hard.scale.x / mouse_grid.x_speed;
        mouse_hard.prescale.y   = y / mouse_hard.scale.y / mouse_grid.y_speed;
        mouse_hard.prescale.z   = z / mouse_hard.scale.z / mouse_grid.z_speed;
        mouse_hard.bttns[0]             = l;
        mouse_hard.bttns[1]             = r;
        MouseHardSetPost;
}

U0 KbdMouseReset()
{
        KbdCmdFlush;
        FifoU8Flush(kbd.fifo2);
        FifoU8Flush(mouse_hard.fifo2);
        FifoI64Flush(kbd.scan_code_fifo);
        kbd.scan_code   = 0;
        kbd.reset               = FALSE;
}

Bool MouseHardEnable(Bool val=TRUE)
{
        Bool old_val = mouse_hard.enabled;

        if (val)
        {
                KbdCmdSend(KBD_CTRL, KBDC_ENABLE_MS);
                mouse_hard.enabled = TRUE;
        }
        else
        {
                KbdCmdSend(KBD_CTRL,  KBDC_DISABLE_MS);
                mouse_hard.enabled = FALSE;
        }
        return old_val;
}

Bool MouseHardDriverInstall(I64 dummy=0) //can be spawned
{
        no_warn dummy;
        I64 i;

        mouse_hard.install_in_progress = TRUE;
        OutU8(PIC_2_DATA, InU8(PIC_2_DATA) | 0x10);
        mouse_hard.installed = mouse_hard.irqs_working = FALSE;
        IntEntrySet(0x2C, &IRQMouseHard);
        for(i = 0; i < 5; i++)
                mouse_hard.bttns[i] = 0;
        if (i = MouseHardReset)
                OutU8(PIC_2_DATA, InU8(PIC_2_DATA) & ~0x10);
        KbdMouseReset;
        mouse_hard.install_attempts++;
        mouse_hard.installed                    = mouse_hard.event=i;
        mouse_hard.install_in_progress  = FALSE;
        return mouse_hard.installed;
}

U0 KbdMouseHandler(Bool poll_kbd, Bool poll_mouse)
{
        if (mouse_hard.install_in_progress)
        {
                Yield;
                return;
        }
        if (kbd.reset)
                KbdMouseReset;
        else
        {
                if (poll_mouse && mouse_hard.installed && !mouse_hard.irqs_working)
                {
                        PUSHFD
                        CLI
                        while (InU8(KBD_CTRL) & 1)
                                MouseHardPacketRead;
                        POPFD
                }

                if (poll_kbd)
                        while (InU8(KBD_CTRL) & 1)
                                KbdPacketRead;

                if (kbd.reset)
                        KbdMouseReset;
                else
                {
                        while (FifoU8Count(kbd.fifo2))
                                KbdHandler;
                        while (FifoU8Count(mouse_hard.fifo2))
                                if (mouse_hard.installed)
                                        MouseHardHandler;
                                else
                                        KbdMouseReset;
                }
        }
}

U0 KbdMouseInit()
{
        MemSet(&kbd, 0, sizeof(CKbdStateGlobals));
        kbd.fifo                        = FifoU8New(8);
        kbd.fifo2                       = FifoU8New(0x1000);
        kbd.scan_code_fifo      = FifoI64New(0x1000);
        kbd.irqs_working        = FALSE;
        MemSet(&mouse_hard, 0, sizeof(CMouseHardStateGlobals));
        mouse_hard.enabled              = TRUE;
        mouse_hard.fifo                 = FifoU8New(8);
        mouse_hard.fifo2                = FifoU8New(0x1000);
        mouse_hard.scale.x              = 0.5;
        mouse_hard.scale.y              = 0.5;
        mouse_hard.scale.z              = 1.0;
        mouse_hard.prescale.x   = sys_vbe_mode.width  / mouse_hard.scale.x / 2.0;
        mouse_hard.prescale.y   = sys_vbe_mode.height / mouse_hard.scale.y / 2.0;
        mouse_hard.prescale.z   = 0 / mouse_hard.scale.z;
        mouse_hard.pos.x                = sys_vbe_mode.width >> 1;
        mouse_hard.pos.y                = sys_vbe_mode.height >> 1;
        MemCopy(&mouse_hard_last, &mouse_hard, sizeof(CMouseHardStateGlobals));
}