U0 GridInit()
{//Init mouse grid struct. See ::/Demo/Graphics/Grid.ZC.
    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_framebuffer_width  - 1);
    mouse.pos.y = ClampI64(mouse.pos.y, 0, sys_framebuffer_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);
}

public U0 MouseRawReset()
{
    mouse_hard.raw_data.x = 0;
    mouse_hard.raw_data.y = 0;
    mouse_hard.raw_data.z = 0;
    mouse_hard.raw_bttns[0] = FALSE;
    mouse_hard.raw_bttns[1] = FALSE;
    mouse_hard.raw_bttns[2] = FALSE;
    mouse_hard.raw_bttns[3] = FALSE;
    mouse_hard.raw_bttns[4] = FALSE;
}

CMouseRawQueue *MouseRawQueueFind(CTask *task)
{
    CMouseRawQueue *entry = mouse_hard.raw_queue->next;

    while (entry != mouse_hard.raw_queue)
    {
        if (entry->task == task)
            return entry;
        entry = entry->next;
    }

    return NULL;
}

U0 MouseRawWatcherInner(CTask *task, CMouseRawQueue *entry, Bool *_found)
{
    CTask *task2;

    if (task == entry->task)
        *_found = TRUE;

    task2 = task->next_child_task;
    while (!*_found && task2 != (&task->next_child_task)(U8 *) - offset(CTask.next_sibling_task))
    {
        MouseRawWatcherInner(task2, entry, _found);
        task2 = task2->next_sibling_task;
    }
}

U0 MouseRawWatcher()
{
    CMouseRawQueue *entry, *next_entry;
    I64 i;
    CCPU *c;
    Bool found;

    while (TRUE)
    {
        entry = mouse_hard.raw_queue->next;
        while (entry != mouse_hard.raw_queue)
        {
            found = FALSE;
            next_entry = entry->next;
            PUSHFD
            CLI
            for (i = 0; i < mp_count; i++)
            {
                c = &cpu_structs[i];
                MouseRawWatcherInner(c->executive_task, entry, &found);
            }
            POPFD

            if (!found)
            {
                QueueRemove(entry);
                Free(entry);
            }

            entry = next_entry;
        }
        Sleep(1000);
    }
}

public Bool MouseRaw(Bool val, CTask *task=NULL)
{ // Places mouse in "raw" mode, button presses will not go to windows manager when true
    CMouseRawQueue *entry;
    Bool old_val;

    if (!task)
        task = Fs;

    if (val)
    {
        old_val = TRUE;
        if (!MouseRawQueueFind(task))
        {
            entry = CAlloc(sizeof(CMouseRawQueue));
            entry->task = task;
            QueueInsertRev(entry, mouse_hard.raw_queue);
            old_val = FALSE;
        }
    }
    else
    {
        old_val = FALSE;
        if (entry = MouseRawQueueFind(task))
        {
            QueueRemove(entry);
            Free(entry);
            old_val = TRUE;
        }
    }

    return old_val;
}

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_framebuffer_width))
        x = mouse.pos.x;
    if (!(0 <= y < sys_framebuffer_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;
    MouseRawReset;
}

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;

    MouseRawReset;
    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_framebuffer_width)
        mouse.offset.x += sys_framebuffer_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_framebuffer_height)
        mouse.offset.y += sys_framebuffer_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];
    Bool is_raw_mode;

    if (mouse_hard.raw_queue->next == mouse_hard.raw_queue)
        is_raw_mode = FALSE; // queue check branch skips func call stack push/pop, small speedup
    else
        is_raw_mode = MouseRawQueueFind(sys_focus_task);

    if (!is_raw_mode)
        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;

    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;

    if (is_raw_mode)
    {
        // buttons / position data need to by consumed by app
        // buttons stay down, positions keep accumulating until
        // consumed by app and reset with MouseRawReset
        mouse_hard.raw_bttns[0] |=  mouse_buf[0] & 1;
        mouse_hard.raw_bttns[1] |= (mouse_buf[0] & 2)    >> 1;
        mouse_hard.raw_bttns[2] |= (mouse_buf[0] & 4)    >> 2;
        mouse_hard.raw_bttns[3] |= (mouse_buf[3] & 0x10) >> 4;
        mouse_hard.raw_bttns[4] |= (mouse_buf[3] & 0x20) >> 5;
        mouse_hard.raw_data.x += dx;
        mouse_hard.raw_data.y += dy;
        mouse_hard.raw_data.z += dz;
        mouse.show = FALSE;
        mouse.lb = FALSE;
        mouse.rb = FALSE;
    }
    else
    {
        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;
        mouse_hard.prescale.x += dx;
        mouse_hard.prescale.y += dy;
        mouse_hard.prescale.z += dz;
        mouse.show = TRUE;
        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_framebuffer_width  / mouse_hard.scale.x / 2.0;
    mouse_hard.prescale.y   = sys_framebuffer_height / mouse_hard.scale.y / 2.0;
    mouse_hard.prescale.z   = 0 / mouse_hard.scale.z;
    mouse_hard.pos.x        = sys_framebuffer_width >> 1;
    mouse_hard.pos.y        = sys_framebuffer_height >> 1;
    mouse_hard.raw_queue    = CAlloc(sizeof(CQueue)); // head of CMouseRawQueue queue
    QueueInit(mouse_hard.raw_queue);
    MemCopy(&mouse_hard_last, &mouse_hard, sizeof(CMouseHardStateGlobals));
}