#include "controller.h"

using namespace pacpus;

DualshockAPI::DualshockAPI()
{
    uid_t uid = getuid();
    if(uid)
        throw "Run as root and you will be happy :)";
    bdaddr_t any = {{0, 0, 0, 0, 0, 0}};
    csk = l2cap_listen(&any, L2CAP_PSM_HIDP_CTRL);
    isk = l2cap_listen(&any, L2CAP_PSM_HIDP_INTR);
    if(csk < 0 || isk < 0)
        throw "Unable to listen on l2cap socket";
    quit = 0;
    controller.paired = 0;
    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
    struct dualshockButtons_s d;
    d.available = 0;
    d.time = road_time();
    shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
}

int DualshockAPI::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm)
{
    int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
    if(sk < 0)
        throw "Unable to open a socket";
    struct sockaddr_l2 addr;
    memset(&addr, 0, sizeof(addr));
    addr.l2_family = AF_BLUETOOTH;
    addr.l2_bdaddr = *bdaddr;//(bdaddr_t) {{0, 0, 0, 0, 0, 0}};
    addr.l2_psm = htobs(psm);
    if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        close(sk);
        throw "Bind error";
    }
    if (listen(sk, 5) < 0)
        throw "Listen error";
    return sk;
}

void DualshockAPI::update_shared_memory()
{
    struct dualshockButtons_s d;
    d.available = controller.paired;
    d.buttons = controller.buttons;
    d.time = road_time();
    shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
    dbtFile.writeRecord(d.time, 0, (const char*)&d,
            sizeof(struct dualshockButtons_s));
}

void DualshockAPI::main_loop()
{
    //std::cerr << "Main loop\n";
    while(!quit) {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(STDIN_FILENO, &fds);
        int fdmax = 0;
        FD_SET(csk, &fds);
        FD_SET(isk, &fds);
        fdmax = csk > isk ? csk : isk;
        if(controller.paired) {
            FD_SET(controller.csk, &fds);
            if (controller.csk > fdmax)
                fdmax = controller.csk;
            FD_SET(controller.isk, &fds);
            if (controller.isk > fdmax)
                fdmax = controller.isk;
        }
        //std::cerr << "    elem\n";
        //std::cerr << "Select\n";
        struct timeval tv = {2, 0}; //timeout = 2s
        if (select(fdmax + 1, &fds, NULL, NULL, &tv) < 0)
            throw "Select";

        if (FD_ISSET(csk, &fds))
            handle_connection();
        if (controller.paired && FD_ISSET(controller.isk, &fds)) {
            unsigned char report[256];
            int nr = recv(controller.isk, report, sizeof(report), 0);
            if (nr <= 0)
                handle_disconnection();
            else
                handle_report(report, nr);
        }
    }
    quit = 2;
}

void DualshockAPI::stop()
{
    quit = 1;
    while(quit != 2) //avoid a race condition
        ;
    dbtFile.close();
}

void DualshockAPI::handle_connection()
{
    std::cerr << "Connection !\n";
    static int currentIndex = 0;
    int cs, is;
    bdaddr_t baddr;

    if((cs = accept(csk, NULL, NULL)) < 0)
        throw "Accept";
    if((is = accept(isk, NULL, NULL)) < 0)
        throw "Accept";

    struct sockaddr_l2 addr;
    socklen_t addrlen = sizeof(addr);
    if (getpeername(is, (struct sockaddr *)&addr, &addrlen) < 0)
        throw "Getpeername";
    baddr = addr.l2_bdaddr;
    unsigned char resp[64];
    char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
        '\xf2', sizeof(resp), sizeof(resp)>>8 };
    (void)send(cs, get03f2, sizeof(get03f2), 0);
    (void)recv(cs, resp, sizeof(resp), 0);

    if(controller.paired == 1) {
        close(controller.csk);
        close(controller.isk);
    }
    controller.index = currentIndex++;
    controller.addr = baddr;
    controller.paired = 1;
    controller.csk = cs;
    controller.isk = is;
    setup_device(controller.csk);
}

void DualshockAPI::setup_device(int sk)
{
    int index = 1; //FIXME
    char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
        '\xf4', '\x42', '\x03', '\00', '\00' };
    send(sk, set03f4, sizeof(set03f4), 0);
    unsigned char ack;
    int nr = recv(sk, &ack, sizeof(ack), 0);
    if (nr != 1 || ack != 0)
        throw "Device is not responding";
    static const char ledmask[10] = { 1, 2, 4, 8, 6, 7, 11, 13, 14, 15 };
#define LED_PERMANENT '\xff', '\x27', '\00', '\00', '\x32'
    char set0201[] = {
        HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01,
        '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00',
        '\00', static_cast<char>(ledmask[index % 10] << 1),
        LED_PERMANENT,
        LED_PERMANENT,
        LED_PERMANENT,
        LED_PERMANENT,
        '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00'
    };
    set0201[3] = set0201[5] = 4;
    set0201[5] = 0;
    set0201[6] = 0x70;
    send(sk, set0201, sizeof(set0201), 0);
    nr = recv(sk, &ack, sizeof(ack), 0);
    if (nr != 1 || ack != 0)
        throw "Device is not responding";
}

void DualshockAPI::handle_disconnection()
{
    std::cerr << "Disconnection\n";
    controller.paired = 0;
}

void DualshockAPI::handle_report(unsigned char buf[], int len)
{
    //std::cerr << "Data\n";
    (void)len;
    if (buf[0] != 0xa1)
        return;

    /* Digital */
    controller.buttons.digital.select   = (buf[3] & 0x01) >> 0;
    controller.buttons.digital.l3       = (buf[3] & 0x02) >> 1;
    controller.buttons.digital.r3       = (buf[3] & 0x04) >> 2;
    controller.buttons.digital.start    = (buf[3] & 0x08) >> 3;
    controller.buttons.digital.up       = (buf[3] & 0x10) >> 4;
    controller.buttons.digital.right    = (buf[3] & 0x20) >> 5;
    controller.buttons.digital.down     = (buf[3] & 0x40) >> 6;
    controller.buttons.digital.left     = (buf[3] & 0x80) >> 7;
    controller.buttons.digital.l2       = (buf[4] & 0x01) >> 1;
    controller.buttons.digital.r2       = (buf[4] & 0x02) >> 2;
    controller.buttons.digital.l1       = (buf[4] & 0x04) >> 3;
    controller.buttons.digital.r1       = (buf[4] & 0x08) >> 4;
    controller.buttons.digital.triangle = (buf[4] & 0x10) >> 5;
    controller.buttons.digital.circle   = (buf[4] & 0x20) >> 6;
    controller.buttons.digital.cross    = (buf[4] & 0x40) >> 7;
    controller.buttons.digital.square   = (buf[4] & 0x80) >> 8;

    /* Sticks */
    controller.buttons.stick.leftStick_x     = buf[7];
    controller.buttons.stick.leftStick_y     = buf[8];
    controller.buttons.stick.rightStick_x    = buf[9];
    controller.buttons.stick.rightStick_y    = buf[10];

    /* Analog */
    controller.buttons.analog.l2        = buf[19];
    controller.buttons.analog.r2        = buf[20];
    controller.buttons.analog.l1        = buf[21];
    controller.buttons.analog.r1        = buf[22];
    controller.buttons.analog.triangle  = buf[23];
    controller.buttons.analog.circle    = buf[24];
    controller.buttons.analog.cross     = buf[25];
    controller.buttons.analog.square    = buf[26];

    /* Axis */
    controller.buttons.axis.x                = buf[43];
    controller.buttons.axis.y                = buf[45];
    controller.buttons.axis.z                = buf[47];
    controller.buttons.axis.gZ               = buf[47];

    update_shared_memory();
}

void DualshockAPI::run()
{
    dbtFile.open("dualshock.dbt", WriteMode, FILE_DBT_UNKNOWN,
            sizeof(struct dualshockButtons_s));
    quit = 0;
    main_loop();
}

DualshockAPI::~DualshockAPI()
{
    delete(shmem);
}