/*********************************************************************
//  created:    2012/03/01 - 14:06
//  filename:   PS3remote.cpp
//
//  author:     Pierre Hudelaine
//              Copyright Heudiasyc UMR UTC/CNRS 7253
// 
//  version:    $Id: $
//
//  purpose:    Read the dualshock 3 button using bluetooth
//
*********************************************************************/

#include "ps3remote.h"

using namespace pacpus;


PS3remote::PS3remote()
{
    quit = 0;
    b = NULL;
    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
}


PS3remote::PS3remote(PacpusSocket * socket)
{
    quit = 0;
    b = NULL;
    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
    socket_ = socket;
}


PS3remote::~PS3remote()
{

}


void PS3remote::sendCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
{
    float right, left;
    static char lastLeft = 1;
    static char lastRight = 1;
    char buf[5] = {0};
    buf[1] = leftF;
    buf[3] = rightF;

    left = ((float)Ls / 127) * 100;
    right = ((float)Rs / 127) * 100;
    if(left > 100.0)
        left = 100.0;
    if(right > 100.0)
        right = 100.0;
    buf[0] = (char)left + 1;
    buf[2] = (char)right + 1;
    if(buf[0] != lastLeft || buf[2] != lastRight) {
	fprintf(stderr, "left = %c%d    right = %c%d\n", buf[1] == 1 ? '+' : '-', buf[0], buf[3] == 1 ? '+' : '-', buf[2]);
	socket_->sendDatagrams(QByteArray(buf));
    }
    

    lastLeft = buf[0];
    lastRight = buf[2];
}


void PS3remote::mode2()
{
    uint16_t leftPWM = b->buttons.stick.leftStick_y;
    uint16_t rightPWM = b->buttons.stick.rightStick_y;
    int leftF = 1, rightF = 1;

    if(leftPWM < 135 && leftPWM > 120)
        leftPWM = 0;
    else {
        if(leftPWM > 128) {
            leftPWM -= 128;
            leftF = -1;
        }
        else
            leftPWM = 128 - leftPWM;
    }
    if(rightPWM < 135 && rightPWM > 120)
        rightPWM = 0;
    else {
        if(rightPWM > 128) {
            rightF = -1;
            rightPWM -= 128;
        }
        else
            rightPWM = 128 - rightPWM;
    }
    if(leftPWM > 127)
        leftPWM = 126;
    if(rightPWM > 127)
        rightPWM = 126;
    sendCmd(leftPWM, rightPWM, leftF, rightF);
}


void PS3remote::mode1()
{
    uint16_t speed = b->buttons.stick.leftStick_y;
    float x = (float)b->buttons.stick.rightStick_x;
    int leftF = 1, rightF = 1;
    if(speed < 140 && speed > 115) {
        if(x > 140) {
            x = x - 128;
            rightF = -(leftF = 1);
        }
        else if (x < 115) {
            x = 128 - x;
            leftF = -(rightF = 1);
        }
        else
            x = 0;
        sendCmd(x, x, leftF, rightF);
    }
    else {
        if(speed > 128) {
            rightF = leftF = -1;
            speed -= 128;
        }
        else
            speed = 128 - speed;
        float right = 1.0 - (x > 140 ? 0.0 : ((128.0 - x) / 128.0));
        float left = 1.0 - (x < 115 ? 0.0 : ((x - 128.0) / 128.0));
        sendCmd(right * speed, left * speed, leftF, rightF);
    }
}


void PS3remote::checkMode()
{
    static int lastState = 0;
    static int mode = 1;

    if(b->buttons.digital.select) {
        if(!lastState) {
            mode = mode == 1 ? 2 : 1;
            // TODO later
	    //blueshock_setLeds(0, mode);
            lastState = 1;
        }
    }
    else
        lastState = 0;
    
    if (mode == 1)
	mode1();
    else
	mode2();
}


void PS3remote::run()
{
    while(!quit) {
	shmem->wait();
        b = (struct dualshockButtons_s *)shmem->read();
	
 	checkMode();        
	
	if(!b->available)
	    std::cerr << "Waiting for remote...\n";
    }
}


void PS3remote::stop()
{
    quit = 1;
}