#include "mbed.h"
#include "USBMouseKeyboard.h"
#include "MMA8451Q.h"

DigitalOut led(LED_RED);

MMA8451Q acc(PTE25, PTE24, (0x1d<<1));

USBMouseKeyboard keyboard(REL_MOUSE);

int main()
{
    float level_XYZ[3]{};
    acc.getAccAllAxis(&level_XYZ[0]);

    float twist = 0.1;

    //still
    float threshold_Z = 0.05;
    //walk
    float threshold_A = 0.07;
    //walk
    float threshold_B = 0.15;
    //run
    float threshold_C = 0.18;
    //sprint

    float x=0;
    float y=0;
    float z=0;
    uint8_t left = 0, right = 0, up = 0, down = 0, walk = 0, sprint = 0, crouch = 0;
    while (true)
    {
        x = acc.getAccX() - level_XYZ[0];
        y = acc.getAccY() - level_XYZ[1];
        z = acc.getAccZ() - level_XYZ[2];

        //keyboard.printf("x: %f, y: %f, z: %f\n", x, y, z);

        /*
        if(fabs(z) > twist)//twist
        {
            keyboard.printf("z: %f\n", z);
            //keyboard.update(z*10, 0, 0, 0);
        }*/

        if(x > threshold_Z)//left
        {
            if(right)right=0;
            if(!left)left=LEFT_ARROW;
        }else if(x < -threshold_Z)//right
        {
            if(!right)right=RIGHT_ARROW;
            if(left)left=0;
        }else
        {
            if(right)right=0;
            if(left)left=0;
        }

        if(y < -threshold_Z)//up
        {
            if(down)down=0;
            if(!up)up=UP_ARROW;
        }else if(y > threshold_Z)//down
        {
            if(!down)down=DOWN_ARROW;
            if(up)up=0;
        }else
        {
            if(up)up=0;
            if(down)down=0;
        }

        //thresholds
        float mag = fabs(x) + fabs(y);

        /*
        if(mag < threshold_A)
        {
            if(!crouch)crouch=KEY_SHIFT;
        } else crouch=0;
        **/

        if(mag < threshold_B)
        {
            if(!walk)walk='c';//+c
            if(sprint)sprint=0;//-e
        }

        if(mag > threshold_B && mag < threshold_C)
        {
            if(walk)walk=0;//-c
            if(sprint)sprint=0;//-e
        }

        if(mag > threshold_C)
        {
            if(walk)walk=0;//-c
            if(!sprint)sprint='e';//+e
        }

        keyboard.keyCode_ATH(left, right, up, down, walk, sprint, crouch);

        led = !led; // toggle led

        wait(0.05f);
    }
}