/*********************************************************************
//  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));
    
    char buf[4] = {1, 1, 1, 1};
    
    timer_ = new QTimer();
    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
}


PS3remote::PS3remote(PacpusSocket * socket)
{
    quit = 0;
    b = NULL;
    
    shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
    
    socket_ = socket;
    
    char buf[4] = {1, 1, 1, 1};
    
    timer_ = new QTimer();
    connect(timer_, SIGNAL(timeout()), this, SLOT(wifibotRemanentSending()));
}


PS3remote::~PS3remote()
{
}


void PS3remote::setCmd(uint16_t Ls, uint16_t Rs, int leftF, int rightF)
{
    float right, left;
    static char lastLeft = 1;
    static char lastRight = 1;
    
    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]);
	sendCmd();
    }    

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


void PS3remote::sendCmd()
{
    if (b->timeout || !b->available)
      memset(buf, 1, sizeof(buf)); // Mise a 1 du buffer pour arreter le wifibot
    
    socket_->sendDatagrams(QByteArray(buf, 4));
    timer_->start(500);
}


void PS3remote::modeChar()
{
    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;
    
    setCmd(leftPWM, rightPWM, leftF, rightF);
}


void PS3remote::modePS3()
{
    int16_t speed = b->buttons.analog.r2 - b->buttons.analog.l2;
    float x = (float)b->buttons.stick.leftStick_x;
    int leftF = 1, rightF = 1;
        
   if(speed < 10 && speed > -10) {
        if(x > 140) {
            x = x - 128;
            rightF = -(leftF = 1);
        } else if (x < 115) {
            x = 128 - x;
            leftF = -(rightF = 1);
        } else {
            x = 0;
	}
	
        setCmd(x, x, leftF, rightF);	
    } else {
      
        if(speed > 0) {
            rightF = leftF = 1;
        } else {
	    rightF = leftF = -1;
	}
	
	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));

	speed /= 2.5;
	speed = abs(speed);
	
	setCmd(right * speed, left * speed, leftF, rightF);
    }    
}


void PS3remote::modeJoystick()
{
    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;
	}
	
        setCmd(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));
        
	setCmd(right * speed, left * speed, leftF, rightF);
    }
}


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

    if(b->buttons.digital.select) {
        if(!lastState) {
            //mode = mode == 1 ? 2 : 1;
            // TODO later
	    //blueshock_setLeds(0, mode);
	    mode = (mode + 1) % 3;
	    lastState = 1;
        }
    } else {
        lastState = 0;
    }
    
    switch (mode){
      case 0:
	modePS3();
	break;
      
      case 1:
	modeJoystick();
	break;
	
      default:
      case 2:
	modeChar();
	break;
    }
}


void PS3remote::wifibotRemanentSending()
{
    sendCmd();
    fprintf(stderr, "Remanence\n");
    timer_->start(500);
}

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


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