uint8_t Gimbal::SBGC_cmd_angles_unpack(angulos &p, SerialCommand &cmd) {

	for (uint8_t i = 0; i < 3; i++) {

		switch (i)
		{
		case 0:
			p.roll = cmd.readWord();
			p.roll = p.roll*ESCALA;
			break;
		case 1:
			p.pitch = cmd.readWord();
			p.pitch = p.pitch*ESCALA;
			break;
		case 2:
			p.yaw = cmd.readWord();
			p.yaw = p.yaw*ESCALA;
			break;
		}

		//Leemos dos veces, para consumir los datos rc_angle y rc_speed que no son necesarios.
		cmd.readWord();
		cmd.readWord();
	}

	if (cmd.checkLimit()) return 0;
	else return PARSER_ERROR_WRONG_DATA_SIZE;
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_trigger_pack(SBGC_cmd_trigger_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_TRIGGER_PIN);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeByte(p.pin);
		cmd.writeByte(p.state);
	#endif
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_api_virt_ch_control_pack(SBGC_cmd_api_virt_ch_control_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_API_VIRT_CH_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		for(uint8_t i=0; i<SBGC_API_VIRT_NUM_CHANNELS; i++) {
			cmd.writeByte(p.data[i]);
		}
	#endif
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_servo_out_pack(SBGC_cmd_servo_out_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_SERVO_OUT);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		for(uint8_t i=0; i<8; i++) {
			cmd.writeWord(p.servo[i]);
		}
	#endif
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_set_adj_vars_pack(SBGC_cmd_set_adj_vars_var_t vars[], uint8_t vars_num, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_SET_ADJ_VARS_VAL);
	cmd.writeByte(vars_num); // number of variables
	
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		cmd.writeBuf(vars, sizeof(SBGC_cmd_set_adj_vars_var_t)*vars_num);
	#else
		for(uint8_t i=0; i<vars_num; i++) {
			cmd.writeByte(vars[i].id);
			cmd.writeLong(vars[i].val);
		}
	#endif
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_ext_pack(SBGC_cmd_control_ext_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeBuf(p.mode, 3);
		for(uint8_t i=0; i<3; i++) {
			cmd.writeWord(p.data[i].speed);
			cmd.writeWord(p.data[i].angle);
		}
	#endif
}

/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_pack(SBGC_cmd_control_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeByte(p.mode);
		cmd.writeWord(p.speedROLL);
		cmd.writeWord(p.angleROLL);
		cmd.writeWord(p.speedPITCH);
		cmd.writeWord(p.anglePITCH);
		cmd.writeWord(p.speedYAW);
		cmd.writeWord(p.angleYAW);
	#endif
}

void executeSerialCommands()
{
	while (Serial.available()) {
		char c = Serial.read();
		Serial.print(c); // echo
		commandProcessor.update(c);
	}
}

/*
* Unpacks SerialCommand object to vars_buf[var_num].
* 'var_num' specifies the buffer capacity.
* On return, 'var_num' will be set to actual number of received variables.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_set_adj_vars_unpack(SBGC_cmd_set_adj_vars_var_t vars_buf[], uint8_t &vars_num, SerialCommand &cmd) {
	uint8_t num = cmd.readByte(); // actual number of variables
	if(num <= vars_num) {
		vars_num = num;
		#ifdef SBGC_CMD_STRUCT_ALIGNED
				cmd.readBuf(vars_buf, sizeof(SBGC_cmd_set_adj_vars_var_t)*vars_num);
		#else
			for(uint8_t i=0; i<num; i++) {
				vars_buf[i].id = cmd.readByte();
				vars_buf[i].val = cmd.readLong();
			}
		#endif
		
		if(cmd.checkLimit()) return 0;
		else return PARSER_ERROR_WRONG_DATA_SIZE;
	} else {
		return PARSER_ERROR_BUFFER_IS_FULL;
	}
}

/*
* Unpacks SerialCommand object to command structure.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_realtime_data_unpack(SBGC_cmd_realtime_data_t &p, SerialCommand &cmd) {
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		if(cmd.len <= sizeof(p)) {
			memcpy(&p, cmd.data, cmd.len);
			return 0;
		} else {
			return PARSER_ERROR_WRONG_DATA_SIZE;
		}
	#else
		for(uint8_t i=0; i<3; i++) {
			p.sensor_data[i].acc_data = cmd.readWord();
			p.sensor_data[i].gyro_data = cmd.readWord();
		}
		p.serial_error_cnt = cmd.readWord();
		p.system_error = cmd.readWord();
		cmd.skipBytes(4); // reserved
		cmd.readWordArr(p.rc_raw_data, SBGC_RC_NUM_CHANNELS);
		cmd.readWordArr(p.imu_angle, 3);
		cmd.readWordArr(p.frame_imu_angle, 3);
		cmd.readWordArr(p.target_angle, 3);
		p.cycle_time_us = cmd.readWord();
		p.i2c_error_count = cmd.readWord();
		cmd.readByte(); // reserved
		p.battery_voltage = cmd.readWord();
		p.state_flags1 = cmd.readByte();
		p.cur_imu = cmd.readByte();
		p.cur_profile = cmd.readByte();
		cmd.readBuf(p.motor_power, 3);
		
		if(cmd.id == SBGC_CMD_REALTIME_DATA_4) {
			cmd.readWordArr(p.rotor_angle, 3);
			cmd.readByte(); // reserved
			cmd.readWordArr(p.balance_error, 3);
			p.current = cmd.readWord();
			cmd.readWordArr(p.magnetometer_data, 3);
			p.imu_temp_celcius = cmd.readByte();
			p.frame_imu_temp_celcius = cmd.readByte();
			cmd.skipBytes(38);
		}
		
		
		if(cmd.checkLimit()) return 0;
		else return PARSER_ERROR_WRONG_DATA_SIZE;
	#endif
}

int main(void)
{
	bool cmd_ok = false;

	wdt_enable(WDTO_8S);

	// ADC enable; prescaler: 128 (156 KHz @ 20MHz)
	ADCSRA |= (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0));
	DDRC |= _BV(DDC0);  // LED:OUTPUT

	// NLOCK: locked
	DDRC |= _BV(DDC2);  // OUTPUT
	PORTC &= ~_BV(PORTC2);  // LOW

#ifdef DEBUG
	int m = DEBUG_CMD_DELAY;  // ms
	fdevopen(&serial_putc, 0);  // open the stdout and stderr streams
#endif

	sys_serial.begin(57600);
	IF_DEBUG(printf_P(PSTR("BOOT\r\n")));

	// initiate a sync with the daemon after boot
	// there is a 1:65536 chance that sync will not be lost
	// the daemon will initialize the device by the command
	cmd.set(CMD_COMMON, COMMON_RESET, 0, 0);
	common_device.confirm_boot(&cmd);
	//cmd.reset();  // look to the SerialCommand::read

	sys_log.begin();
	SPI.begin();
	if (radio.begin()) {
		 network.begin(RF24_CHANEL, rx_node);
		 nrf_device.setup(&network);
	}

	while(true)
	{
		wdt_reset();

		// CMD_NRF_FORWARD
		if (nrf_device.read(&cmd)) {
			if (!nrf_device.run(&cmd)) cmd.send_header(-1);
			//cmd.reset();
		}

		// CMD_RNG_SEND
		for (int i = 0; i < WDT_BITS_PER_CYCLE; i++) {
			if (rng_device.read(&cmd)) {
				if (!rng_device.run(&cmd)) cmd.send_header(-1);
				//cmd.reset();
			}
		}

		// watchdog trigger
		wdt_device.update();

#ifdef DEBUG
		if (--m <= 0) {
			m = DEBUG_CMD_DELAY;
			//cmd.set(CMD_COMMON, 1, 1460792071, 0); // look to the SerialCommand::read
			//sys_log.write(sys_time.now(), LOG_BOOT);
		}
		//_delay_ms(1);
#endif

		// serial commands
		if (cmd.read()) {
#ifdef DEBUG
			blink_once();
#endif
			cmd_ok = false;
			switch (cmd.get_type()) {
				case CMD_COMMON:
					cmd_ok = common_device.run(&cmd);
					break;
				case CMD_WDT:
					cmd_ok = wdt_device.run(&cmd);
					break;
				case CMD_RNG:
					cmd_ok = rng_device.run(&cmd);
					break;
				case CMD_NRF:
					cmd_ok = nrf_device.run(&cmd);
					break;
				default:
					break;
			}
			if (!cmd_ok) cmd.send_header(-1);
			//cmd.reset();
		}
	} // while(true)

	sys_serial.end();
	return 0;
} // main

void CommandSet::setup()
{
    /* Setup callbacks for SerialCommand commands */
    sCmd.addCommand("L",       this->led);          // Toggles LED
    sCmd.addCommand("ping",    this->ping);         // Check serial link
    sCmd.addCommand("help",    this->help);

    /* Movement commands */
    sCmd.addCommand("M",       this->move);         // Runs wheel motors
    sCmd.addCommand("S",       this->stop);         // Force stops all motors
    sCmd.addCommand("G",       this->go);           // Runs wheel motors with correction

    /* Read from rotary encoders */
    sCmd.addCommand("speeds",  this->speeds);

    /* Misc commands */
    sCmd.addCommand("pixels",  this->pixels);       // Set LED colour

    sCmd.addCommand("grab",    this->grab);         // Grab 0 or 1
    sCmd.addCommand("kick",    this->kick);         // Kick
    sCmd.addCommand("shuntkick", this->shuntkick);  // Shuntkick

    sCmd.addCommand("rotate",  this->rotate);       // Rotate

    /* Debug and inspection commands */

    sCmd.addCommand("ps",      this->proc_dump);    // Show process status
    sCmd.addCommand("ptog",    this->proc_toggle);  // Enable or disable by pid on the fly
    
    sCmd.setDefaultHandler(this->unrecognized);   // Handler for command that isn't matched
}

void CommandSet::go()
{
    /* holonomics and shit */
    float x_vel = atof(sCmd.next());
    float y_vel = atof(sCmd.next());
    float r_vel = atof(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.println(F("Going"));
    #endif

    _go(x_vel, y_vel, r_vel);
}

void CommandSet::rotate()
{
    const int motor_power   = atoi(sCmd.next());
    const long delta        = atoi(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("rotating at "));
    Serial.print(motor_power);
    Serial.print(F(" to "));
    Serial.print(delta);
    Serial.println(F(" stops"));
    #endif

    for (size_t i=0; i < motor_count; i++){
        state.motors[i]->power = motor_power;
        state.initial_displacement[i] = state.motors[i]->disp;
    }

    state.rotation_delta = delta;

    processes.enable((state.rotation_process)->id);

    write_powers();
}

void CommandSet::grab()
{
    const int direction = atoi(sCmd.next());

    if (state.grabber_state != Open && state.grabber_state != Closed) {
        Serial.println(F("N - grab"));
        return;
    }

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("grabbing "));
    Serial.println(direction);
    #endif

    const pid_t pid         = state.grab_handler->id;
    const int motor_power   = 200;

    /* If we're open, doing that again will bugger the vision plate */
    if (direction) {
        state.grabber_state = Closing;
        motorForward(grabber_port, motor_power);
        processes.change(pid, 300L);
    } else if (state.grabber_state != Open) {
        state.grabber_state = Opening;
        motorBackward(grabber_port, motor_power);
        processes.change(pid, 280L);
    } else {
        return;
    }

    processes.enable(pid);
    processes.forward(pid);
}

void CommandSet::pixels()
{
    byte red   = (byte) atoi(sCmd.next());
    byte green = (byte) atoi(sCmd.next());
    byte blue  = (byte) atoi(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("Set pixel colour to "));
    Serial.print(red,HEX);
    Serial.print(green,HEX);
    Serial.println(blue,HEX);
    #endif

    for (uint16_t i = 0; i < 10; i++) {
        state.strip.setPixelColor(i, red, green, blue);
    }
    state.strip.show();
}

void setup()
{
    // initialize the serial communication:
    serial.init(TTYACM0, 115200, openComport);
    serial.setPacketHandler(handlePacket);
    sCmd.addCommand(1, respondData);
    gettimeofday(&start, NULL);
    gettimeofday(&startLoop, NULL);
    uiRxPacketCtr=0;
    uiTxPacketCtr=0;
}

int readArgument(int defaultValue = 255){
  char *arg;

  arg = sCmd.next();
  if (arg != NULL) {
    return atoi(arg);
  }
  else{
    return defaultValue;
  }
}

void CommandSet::proc_toggle()
{
    pid_t pid = (pid_t) atoi(sCmd.next());

    if (pid == NULL) {
        Serial.println("N - ptog");
        return;
    }
    
    process* proc = processes.get_by_id(pid);

    if (proc == NULL) {
        Serial.print(F("Unknown pid "));
        Serial.println(pid);
        return;
    }
    
    proc->enabled ? processes.disable(pid) : processes.enable(pid);

    Serial.print(F("toggled pid "));
    Serial.println(pid);
}

void CommandSet::move()
{
    int new_powers[motor_count];

    for (int i=0; i < motor_count; i++){
        new_powers[i] = atoi(sCmd.next());
        /* Apply direction correction here */
        new_powers[i] *= state.motors[i]->direction;
    }

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.println(F("Moving"));
    #endif
        
    /* update speeds of all drive motors */
    for(int i=0; i < motor_count; i++){
        state.motors[i]->power = state.motors[i]->desired_power = new_powers[i];
        state.motors[i]->disp_delta = 0;
    }
    write_powers();
}