/***************************************************************************
 *                           CommunicationAvatarVOR.cpp                    *
 *                           -------------------                           *
 * copyright            : (C) 2015 by Niceto Luque and Francisco Naveros   *
 * email                : nluque@ugr.es and fnaveros@ugr.es                *
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 3 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/


#include "../../include/vor_model/CommunicationAvatarVOR.h"
#include "../../include/communication/CdSocket.h"

#include "../../include/interface/C_interface_for_robot_control.h"

#include <iostream>
#include <string>
#include <sstream> 
using namespace std;

CommunicationAvatarVOR::CommunicationAvatarVOR(struct log * var_log) :endConnection(false),var_log(var_log){
}

CommunicationAvatarVOR::~CommunicationAvatarVOR(){
}

void CommunicationAvatarVOR::Send(int N_values, float * input_signal, float * cerebelar_signal){
outputBuffer[0] = input_signal[0];
outputBuffer[1] = cerebelar_signal[0];


socket->sendBuffer(((void*)outputBuffer), sizeof(float) * 2 * N_values);


}

void CommunicationAvatarVOR::Receive(){
	socket->receiveBuffer(inputBuffer, 4);
}

void CommunicationAvatarVOR::Communication(string Avatar_IP){
	socket = new CdSocket(CLIENT, Avatar_IP, 11000);
	while (!endConnection){
		Receive();
		int index = var_log->nregs-1;
		Send(NUM_JOINTS, var_log->regs[index].cereb_input_vars, var_log->regs[index].robot_state_vars);
	}
	delete socket;
}

void CommunicationAvatarVOR::StopConnection(){
	endConnection = true;
}




