/*
	Copyright 2016 - 2021 Benjamin Vedder	benjamin@vedder.se

	This file is part of the VESC firmware.

	The VESC firmware 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.

    The VESC firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "commands.h"
#include "ch.h"
#include "hal.h"
#include "mc_interface.h"
#include "stm32f4xx_conf.h"
#include "servo_simple.h"
#include "buffer.h"
#include "terminal.h"
#include "hw.h"
#include "mcpwm.h"
#include "mcpwm_foc.h"
#include "mc_interface.h"
#include "app.h"
#include "timeout.h"
#include "servo_dec.h"
#include "comm_can.h"
#include "flash_helper.h"
#include "utils.h"
#include "packet.h"
#include "encoder.h"
#include "nrf_driver.h"
#include "gpdrive.h"
#include "confgenerator.h"
#include "imu.h"
#include "shutdown.h"
#if HAS_BLACKMAGIC
#include "bm_if.h"
#endif
#include "minilzo.h"
#include "mempools.h"
#include "bms.h"
#include "qmlui.h"
#include "crc.h"

#include <math.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>

// Threads
static THD_FUNCTION(blocking_thread, arg);
static THD_WORKING_AREA(blocking_thread_wa, 2048);
static thread_t *blocking_tp;

// Private variables
static uint8_t send_buffer_global[PACKET_MAX_PL_LEN];
static uint8_t blocking_thread_cmd_buffer[PACKET_MAX_PL_LEN];
static volatile unsigned int blocking_thread_cmd_len = 0;
static volatile bool is_blocking = false;
static volatile int blocking_thread_motor = 1;
static void(* volatile send_func)(unsigned char *data, unsigned int len) = 0;
static void(* volatile send_func_blocking)(unsigned char *data, unsigned int len) = 0;
static void(* volatile send_func_nrf)(unsigned char *data, unsigned int len) = 0;
static void(* volatile send_func_can_fwd)(unsigned char *data, unsigned int len) = 0;
static void(* volatile appdata_func)(unsigned char *data, unsigned int len) = 0;
static void(* volatile hwdata_func)(unsigned char *data, unsigned int len) = 0;
static disp_pos_mode display_position_mode;
static mutex_t print_mutex;
static mutex_t send_buffer_mutex;
static mutex_t terminal_mutex;
static volatile int fw_version_sent_cnt = 0;
static bool isInitialized = false;

void commands_init(void) {
	chMtxObjectInit(&print_mutex);
	chMtxObjectInit(&send_buffer_mutex);
	chMtxObjectInit(&terminal_mutex);
	chThdCreateStatic(blocking_thread_wa, sizeof(blocking_thread_wa), NORMALPRIO, blocking_thread, NULL);
	isInitialized = true;
}

bool commands_is_initialized(void) {
	return isInitialized;
}

/**
 * Send a packet using the set send function.
 *
 * @param data
 * The packet data.
 *
 * @param len
 * The data length.
 */
void commands_send_packet(unsigned char *data, unsigned int len) {
	if (send_func) {
		send_func(data, len);
	}
}

/**
 * Send a packet using the last can fwd function.
 *
 * @param data
 * The packet data.
 *
 * @param len
 * The data length.
 */
void commands_send_packet_can_last(unsigned char *data, unsigned int len) {
	if (send_func_can_fwd) {
		send_func_can_fwd(data, len);
	}
}

/**
 * Send a packet using the set NRF51 send function. The NRF51 send function
 * is set when the COMM_EXT_NRF_PRESENT and COMM_EXT_NRF_ESB_RX_DATA commands
 * are received, at which point the previous send function is restored. The
 * intention behind that is to make the NRF51-related communication only with
 * the interface that has an NRF51, and prevent the NRF51 communication from
 * interfering with other communication.
 *
 * @param data
 * The packet data.
 *
 * @param len
 * The data length.
 */
void commands_send_packet_nrf(unsigned char *data, unsigned int len) {
	if (send_func_nrf) {
		send_func_nrf(data, len);
	}
}

/**
 * Send data using the function last used by the blocking thread.
 *
 * @param data
 * The packet data.
 *
 * @param len
 * The data length.
 */
void commands_send_packet_last_blocking(unsigned char *data, unsigned int len) {
	if (send_func_blocking) {
		send_func_blocking(data, len);
	}
}

/**
 * Process a received buffer with commands and data.
 *
 * @param data
 * The buffer to process.
 *
 * @param len
 * The length of the buffer.
 */
void commands_process_packet(unsigned char *data, unsigned int len,
		void(*reply_func)(unsigned char *data, unsigned int len)) {

	if (!len) {
		return;
	}

	COMM_PACKET_ID packet_id;

	packet_id = data[0];
	data++;
	len--;

	// The NRF51 ESB implementation is treated like it has its own
	// independent communication interface.
	if (packet_id == COMM_EXT_NRF_PRESENT ||
			packet_id == COMM_EXT_NRF_ESB_RX_DATA) {
		send_func_nrf = reply_func;
	} else {
		send_func = reply_func;
	}

	// Avoid calling invalid function pointer if it is null.
	// commands_send_packet will make the check.
	if (!reply_func) {
		reply_func = commands_send_packet;
	}

	if (!send_func_can_fwd) {
		send_func_can_fwd = reply_func;
	}

	switch (packet_id) {
	case COMM_FW_VERSION: {
		int32_t ind = 0;
		uint8_t send_buffer[60];
		send_buffer[ind++] = COMM_FW_VERSION;
		send_buffer[ind++] = FW_VERSION_MAJOR;
		send_buffer[ind++] = FW_VERSION_MINOR;

		strcpy((char*)(send_buffer + ind), HW_NAME);
		ind += strlen(HW_NAME) + 1;

		memcpy(send_buffer + ind, STM32_UUID_8, 12);
		ind += 12;

		// Add 1 to the UUID for the second motor, so that configuration backup and
		// restore works.
		if (mc_interface_get_motor_thread() == 2) {
			send_buffer[ind - 1]++;
		}

		send_buffer[ind++] = app_get_configuration()->pairing_done;
		send_buffer[ind++] = FW_TEST_VERSION_NUMBER;

		send_buffer[ind++] = HW_TYPE_VESC;

		send_buffer[ind++] = 0; // No custom config

#ifdef HW_HAS_PHASE_FILTERS
		send_buffer[ind++] = 1;
#else
		send_buffer[ind++] = 0;
#endif

#ifdef QMLUI_SOURCE_HW
#ifdef QMLUI_HW_FULLSCREEN
		send_buffer[ind++] = 2;
#else
		send_buffer[ind++] = 1;
#endif
#else
		send_buffer[ind++] = 0;
#endif

#ifdef QMLUI_SOURCE_APP
#ifdef QMLUI_APP_FULLSCREEN
		send_buffer[ind++] = 2;
#else
		send_buffer[ind++] = 1;
#endif
#else
		if (flash_helper_qmlui_data()) {
			send_buffer[ind++] = flash_helper_qmlui_flags();
		} else {
			send_buffer[ind++] = 0;
		}
#endif

		fw_version_sent_cnt++;

		reply_func(send_buffer, ind);
	} break;

	case COMM_JUMP_TO_BOOTLOADER_ALL_CAN:
		data[-1] = COMM_JUMP_TO_BOOTLOADER;
		comm_can_send_buffer(255, data - 1, len + 1, 2);
		chThdSleepMilliseconds(100);
		/* Falls through. */
		/* no break */
	case COMM_JUMP_TO_BOOTLOADER:
		flash_helper_jump_to_bootloader();
		break;

	case COMM_ERASE_NEW_APP_ALL_CAN:
		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(6000);
		}

		data[-1] = COMM_ERASE_NEW_APP;
		comm_can_send_buffer(255, data - 1, len + 1, 2);
		chThdSleepMilliseconds(1500);
		/* Falls through. */
		/* no break */
	case COMM_ERASE_NEW_APP: {
		int32_t ind = 0;

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(6000);
		}
		uint16_t flash_res = flash_helper_erase_new_app(buffer_get_uint32(data, &ind));

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_ERASE_NEW_APP;
		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
		reply_func(send_buffer, ind);
	} break;

	case COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO:
	case COMM_WRITE_NEW_APP_DATA_ALL_CAN:
		if (packet_id == COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO) {
			chMtxLock(&send_buffer_mutex);
			memcpy(send_buffer_global, data + 6, len - 6);
			int32_t ind = 4;
			lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
			lzo1x_decompress_safe(send_buffer_global, len - 6, data + 4, &decompressed_len, NULL);
			chMtxUnlock(&send_buffer_mutex);
			len = decompressed_len + 4;
		}

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(2000);
		}

		data[-1] = COMM_WRITE_NEW_APP_DATA;

		comm_can_send_buffer(255, data - 1, len + 1, 2);
		/* Falls through. */
		/* no break */
	case COMM_WRITE_NEW_APP_DATA_LZO:
	case COMM_WRITE_NEW_APP_DATA: {
		if (packet_id == COMM_WRITE_NEW_APP_DATA_LZO) {
			chMtxLock(&send_buffer_mutex);
			memcpy(send_buffer_global, data + 6, len - 6);
			int32_t ind = 4;
			lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
			lzo1x_decompress_safe(send_buffer_global, len - 6, data + 4, &decompressed_len, NULL);
			chMtxUnlock(&send_buffer_mutex);
			len = decompressed_len + 4;
		}

		int32_t ind = 0;
		uint32_t new_app_offset = buffer_get_uint32(data, &ind);

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(2000);
		}
		uint16_t flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);

		SHUTDOWN_RESET();

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_WRITE_NEW_APP_DATA;
		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
		buffer_append_uint32(send_buffer, new_app_offset, &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_GET_VALUES:
	case COMM_GET_VALUES_SELECTIVE: {
		int32_t ind = 0;
		chMtxLock(&send_buffer_mutex);
		uint8_t *send_buffer = send_buffer_global;
		send_buffer[ind++] = packet_id;

		uint32_t mask = 0xFFFFFFFF;
		if (packet_id == COMM_GET_VALUES_SELECTIVE) {
			int32_t ind2 = 0;
			mask = buffer_get_uint32(data, &ind2);
			buffer_append_uint32(send_buffer, mask, &ind);
		}

		if (mask & ((uint32_t)1 << 0)) {
			buffer_append_float16(send_buffer, mc_interface_temp_fet_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 1)) {
			buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 2)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_motor_current(), 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 3)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 4)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_id(), 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 5)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_iq(), 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 6)) {
			buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 7)) {
			buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
		}
		if (mask & ((uint32_t)1 << 8)) {
			buffer_append_float16(send_buffer, mc_interface_get_input_voltage_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 9)) {
			buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 10)) {
			buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 11)) {
			buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 12)) {
			buffer_append_float32(send_buffer, mc_interface_get_watt_hours_charged(false), 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 13)) {
			buffer_append_int32(send_buffer, mc_interface_get_tachometer_value(false), &ind);
		}
		if (mask & ((uint32_t)1 << 14)) {
			buffer_append_int32(send_buffer, mc_interface_get_tachometer_abs_value(false), &ind);
		}
		if (mask & ((uint32_t)1 << 15)) {
			send_buffer[ind++] = mc_interface_get_fault();
		}
		if (mask & ((uint32_t)1 << 16)) {
			buffer_append_float32(send_buffer, mc_interface_get_pid_pos_now(), 1e6, &ind);
		}
		if (mask & ((uint32_t)1 << 17)) {
			uint8_t current_controller_id = app_get_configuration()->controller_id;
#ifdef HW_HAS_DUAL_MOTORS
			if (mc_interface_get_motor_thread() == 2) {
				current_controller_id = utils_second_motor_id();
			}
#endif
			send_buffer[ind++] = current_controller_id;
		}
		if (mask & ((uint32_t)1 << 18)) {
			buffer_append_float16(send_buffer, NTC_TEMP_MOS1(), 1e1, &ind);
			buffer_append_float16(send_buffer, NTC_TEMP_MOS2(), 1e1, &ind);
			buffer_append_float16(send_buffer, NTC_TEMP_MOS3(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 19)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vd(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 20)) {
			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vq(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 21)) {
			uint8_t status = 0;
			status |= timeout_has_timeout();
			status |= timeout_kill_sw_active() << 1;
			send_buffer[ind++] = status;
		}

		reply_func(send_buffer, ind);
		chMtxUnlock(&send_buffer_mutex);
	} break;

	case COMM_SET_DUTY: {
		int32_t ind = 0;
		mc_interface_set_duty((float)buffer_get_int32(data, &ind) / 100000.0);
		timeout_reset();
	} break;

	case COMM_SET_CURRENT: {
		int32_t ind = 0;
		mc_interface_set_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
	} break;

	case COMM_SET_CURRENT_BRAKE: {
		int32_t ind = 0;
		mc_interface_set_brake_current((float)buffer_get_int32(data, &ind) / 1000.0);
		timeout_reset();
	} break;

	case COMM_SET_RPM: {
		int32_t ind = 0;
		mc_interface_set_pid_speed((float)buffer_get_int32(data, &ind));
		timeout_reset();
	} break;

	case COMM_SET_POS: {
		int32_t ind = 0;
		mc_interface_set_pid_pos((float)buffer_get_int32(data, &ind) / 1000000.0);
		timeout_reset();
	} break;

	case COMM_SET_HANDBRAKE: {
		int32_t ind = 0;
		mc_interface_set_handbrake(buffer_get_float32(data, 1e3, &ind));
		timeout_reset();
	} break;

	case COMM_SET_DETECT: {
		int32_t ind = 0;
		display_position_mode = data[ind++];

		if (mc_interface_get_configuration()->motor_type == MOTOR_TYPE_BLDC) {
			if (display_position_mode == DISP_POS_MODE_NONE) {
				mc_interface_release_motor();
			} else if (display_position_mode == DISP_POS_MODE_INDUCTANCE) {
				mcpwm_set_detect();
			}
		}

		timeout_reset();
	} break;

	case COMM_SET_SERVO_POS: {
		int32_t ind = 0;
		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
	} break;

	case COMM_SET_MCCONF: {
#ifndef	HW_MCCONF_READ_ONLY
		mc_configuration *mcconf = mempools_alloc_mcconf();
		*mcconf = *mc_interface_get_configuration();

		if (confgenerator_deserialize_mcconf(data, mcconf)) {
			utils_truncate_number(&mcconf->l_current_max_scale , 0.0, 1.0);
			utils_truncate_number(&mcconf->l_current_min_scale , 0.0, 1.0);

#ifdef HW_HAS_DUAL_MOTORS
			mcconf->motor_type = MOTOR_TYPE_FOC;
#endif

			mcconf->lo_current_max = mcconf->l_current_max * mcconf->l_current_max_scale;
			mcconf->lo_current_min = mcconf->l_current_min * mcconf->l_current_min_scale;
			mcconf->lo_in_current_max = mcconf->l_in_current_max;
			mcconf->lo_in_current_min = mcconf->l_in_current_min;
			mcconf->lo_current_motor_max_now = mcconf->lo_current_max;
			mcconf->lo_current_motor_min_now = mcconf->lo_current_min;

			commands_apply_mcconf_hw_limits(mcconf);
			conf_general_store_mc_configuration(mcconf, mc_interface_get_motor_thread() == 2);
			mc_interface_set_configuration(mcconf);
			chThdSleepMilliseconds(200);

			int32_t ind = 0;
			uint8_t send_buffer[50];
			send_buffer[ind++] = packet_id;
			reply_func(send_buffer, ind);
		} else {
			commands_printf("Warning: Could not set mcconf due to wrong signature");
		}

		mempools_free_mcconf(mcconf);
#endif
	} break;

	case COMM_GET_MCCONF:
	case COMM_GET_MCCONF_DEFAULT: {
		mc_configuration *mcconf = mempools_alloc_mcconf();

		if (packet_id == COMM_GET_MCCONF) {
			*mcconf = *mc_interface_get_configuration();
		} else {
			confgenerator_set_defaults_mcconf(mcconf);
			volatile const mc_configuration *mcconf_now = mc_interface_get_configuration();

			// Keep the old offsets
			mcconf->foc_offsets_current[0] = mcconf_now->foc_offsets_current[0];
			mcconf->foc_offsets_current[1] = mcconf_now->foc_offsets_current[1];
			mcconf->foc_offsets_current[2] = mcconf_now->foc_offsets_current[2];
			mcconf->foc_offsets_voltage[0] = mcconf_now->foc_offsets_voltage[0];
			mcconf->foc_offsets_voltage[1] = mcconf_now->foc_offsets_voltage[1];
			mcconf->foc_offsets_voltage[2] = mcconf_now->foc_offsets_voltage[2];
			mcconf->foc_offsets_voltage_undriven[0] = mcconf_now->foc_offsets_voltage_undriven[0];
			mcconf->foc_offsets_voltage_undriven[1] = mcconf_now->foc_offsets_voltage_undriven[1];
			mcconf->foc_offsets_voltage_undriven[2] = mcconf_now->foc_offsets_voltage_undriven[2];
		}

		commands_send_mcconf(packet_id, mcconf);
		mempools_free_mcconf(mcconf);
	} break;

	case COMM_SET_APPCONF: {
#ifndef	HW_APPCONF_READ_ONLY
		app_configuration *appconf = mempools_alloc_appconf();
		*appconf = *app_get_configuration();

		if (confgenerator_deserialize_appconf(data, appconf)) {
#ifdef HW_HAS_DUAL_MOTORS
			// Ignore ID when setting second motor config
			if (mc_interface_get_motor_thread() == 2) {
				appconf->controller_id = app_get_configuration()->controller_id;
			}
#endif

			conf_general_store_app_configuration(appconf);
			app_set_configuration(appconf);
			timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current, appconf->kill_sw_mode);
			chThdSleepMilliseconds(200);

			int32_t ind = 0;
			uint8_t send_buffer[50];
			send_buffer[ind++] = packet_id;
			reply_func(send_buffer, ind);
		} else {
			commands_printf("Warning: Could not set appconf due to wrong signature");
		}

		mempools_free_appconf(appconf);
#endif
	} break;

	case COMM_GET_APPCONF:
	case COMM_GET_APPCONF_DEFAULT: {
		app_configuration *appconf = mempools_alloc_appconf();

		if (packet_id == COMM_GET_APPCONF) {
			*appconf = *app_get_configuration();
		} else {
			confgenerator_set_defaults_appconf(appconf);
		}

#ifdef HW_HAS_DUAL_MOTORS
		if (mc_interface_get_motor_thread() == 2) {
			appconf->controller_id = utils_second_motor_id();
		}
#endif

		commands_send_appconf(packet_id, appconf);

		mempools_free_appconf(appconf);
	} break;

	case COMM_SAMPLE_PRINT: {
		uint16_t sample_len;
		uint8_t decimation;
		debug_sampling_mode mode;

		int32_t ind = 0;
		mode = data[ind++];
		sample_len = buffer_get_uint16(data, &ind);
		decimation = data[ind++];

		bool raw = false;
		if (len > (uint32_t)ind) {
			raw = data[ind++];
		}

		mc_interface_sample_print_data(mode, sample_len, decimation, raw);
	} break;

	case COMM_REBOOT:
		conf_general_store_backup_data();
		// Lock the system and enter an infinite loop. The watchdog will reboot.
		__disable_irq();
		for(;;){};
		break;

	case COMM_ALIVE:
		SHUTDOWN_RESET();
		timeout_reset();
		break;

	case COMM_GET_DECODED_PPM: {
		int32_t ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_GET_DECODED_PPM;
		buffer_append_int32(send_buffer, (int32_t)(app_ppm_get_decoded_level() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(servodec_get_last_pulse_len(0) * 1000000.0), &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_GET_DECODED_ADC: {
		int32_t ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_GET_DECODED_ADC;
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level2() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage2() * 1000000.0), &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_GET_DECODED_CHUK: {
		int32_t ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_GET_DECODED_CHUK;
		buffer_append_int32(send_buffer, (int32_t)(app_nunchuk_get_decoded_chuk() * 1000000.0), &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_GET_DECODED_BALANCE: {
		int32_t ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_GET_DECODED_BALANCE;
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_pid_output() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_pitch_angle() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll_angle() * 1000000.0), &ind);
		buffer_append_uint32(send_buffer, app_balance_get_diff_time(), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_debug1() * 1000000.0), &ind);
		buffer_append_uint16(send_buffer, app_balance_get_state(), &ind);
		buffer_append_uint16(send_buffer, app_balance_get_switch_state(), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc1() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc2() * 1000000.0), &ind);
		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_debug2() * 1000000.0), &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_FORWARD_CAN: {
		send_func_can_fwd = reply_func;

#ifdef HW_HAS_DUAL_MOTORS
		if (data[0] == utils_second_motor_id()) {
			mc_interface_select_motor_thread(2);
			commands_process_packet(data + 1, len - 1, reply_func);
			mc_interface_select_motor_thread(1);
		} else {
			comm_can_send_buffer(data[0], data + 1, len - 1, 0);
		}
#else
		comm_can_send_buffer(data[0], data + 1, len - 1, 0);
#endif
	} break;

	case COMM_SET_CHUCK_DATA: {
		chuck_data chuck_d_tmp;

		int32_t ind = 0;
		chuck_d_tmp.js_x = data[ind++];
		chuck_d_tmp.js_y = data[ind++];
		chuck_d_tmp.bt_c = data[ind++];
		chuck_d_tmp.bt_z = data[ind++];
		chuck_d_tmp.acc_x = buffer_get_int16(data, &ind);
		chuck_d_tmp.acc_y = buffer_get_int16(data, &ind);
		chuck_d_tmp.acc_z = buffer_get_int16(data, &ind);

		if (len >= (unsigned int)ind + 2) {
			chuck_d_tmp.rev_has_state = data[ind++];
			chuck_d_tmp.is_rev = data[ind++];
		} else {
			chuck_d_tmp.rev_has_state = false;
			chuck_d_tmp.is_rev = false;
		}
		app_nunchuk_update_output(&chuck_d_tmp);
	} break;

	case COMM_CUSTOM_APP_DATA:
		if (appdata_func) {
			appdata_func(data, len);
		}
		break;

	case COMM_CUSTOM_HW_DATA:
		if (hwdata_func) {
			hwdata_func(data, len);
		}
		break;

	case COMM_NRF_START_PAIRING: {
		int32_t ind = 0;
		nrf_driver_start_pairing(buffer_get_int32(data, &ind));

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = packet_id;
		send_buffer[ind++] = NRF_PAIR_STARTED;
		reply_func(send_buffer, ind);
	} break;

	case COMM_GPD_SET_FSW: {
		timeout_reset();
		int32_t ind = 0;
		gpdrive_set_switching_frequency((float)buffer_get_int32(data, &ind));
	} break;

	case COMM_GPD_BUFFER_SIZE_LEFT: {
		int32_t ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_GPD_BUFFER_SIZE_LEFT;
		buffer_append_int32(send_buffer, gpdrive_buffer_size_left(), &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_GPD_FILL_BUFFER: {
		timeout_reset();
		int32_t ind = 0;
		while (ind < (int)len) {
			gpdrive_add_buffer_sample(buffer_get_float32_auto(data, &ind));
		}
	} break;

	case COMM_GPD_OUTPUT_SAMPLE: {
		timeout_reset();
		int32_t ind = 0;
		gpdrive_output_sample(buffer_get_float32_auto(data, &ind));
	} break;

	case COMM_GPD_SET_MODE: {
		timeout_reset();
		int32_t ind = 0;
		gpdrive_set_mode(data[ind++]);
	} break;

	case COMM_GPD_FILL_BUFFER_INT8: {
		timeout_reset();
		int32_t ind = 0;
		while (ind < (int)len) {
			gpdrive_add_buffer_sample_int((int8_t)data[ind++]);
		}
	} break;

	case COMM_GPD_FILL_BUFFER_INT16: {
		timeout_reset();
		int32_t ind = 0;
		while (ind < (int)len) {
			gpdrive_add_buffer_sample_int(buffer_get_int16(data, &ind));
		}
	} break;

	case COMM_GPD_SET_BUFFER_INT_SCALE: {
		int32_t ind = 0;
		gpdrive_set_buffer_int_scale(buffer_get_float32_auto(data, &ind));
	} break;

	case COMM_GET_VALUES_SETUP:
	case COMM_GET_VALUES_SETUP_SELECTIVE: {
		setup_values val = mc_interface_get_setup_values();

		float wh_batt_left = 0.0;
		float battery_level = mc_interface_get_battery_level(&wh_batt_left);

		int32_t ind = 0;
		chMtxLock(&send_buffer_mutex);
		uint8_t *send_buffer = send_buffer_global;
		send_buffer[ind++] = packet_id;

		uint32_t mask = 0xFFFFFFFF;
		if (packet_id == COMM_GET_VALUES_SETUP_SELECTIVE) {
			int32_t ind2 = 0;
			mask = buffer_get_uint32(data, &ind2);
			buffer_append_uint32(send_buffer, mask, &ind);
		}

		if (mask & ((uint32_t)1 << 0)) {
			buffer_append_float16(send_buffer, mc_interface_temp_fet_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 1)) {
			buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 2)) {
			buffer_append_float32(send_buffer, val.current_tot, 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 3)) {
			buffer_append_float32(send_buffer, val.current_in_tot, 1e2, &ind);
		}
		if (mask & ((uint32_t)1 << 4)) {
			buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 5)) {
			buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
		}
		if (mask & ((uint32_t)1 << 6)) {
			buffer_append_float32(send_buffer, mc_interface_get_speed(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 7)) {
			buffer_append_float16(send_buffer, mc_interface_get_input_voltage_filtered(), 1e1, &ind);
		}
		if (mask & ((uint32_t)1 << 8)) {
			buffer_append_float16(send_buffer, battery_level, 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 9)) {
			buffer_append_float32(send_buffer, val.ah_tot, 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 10)) {
			buffer_append_float32(send_buffer, val.ah_charge_tot, 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 11)) {
			buffer_append_float32(send_buffer, val.wh_tot, 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 12)) {
			buffer_append_float32(send_buffer, val.wh_charge_tot, 1e4, &ind);
		}
		if (mask & ((uint32_t)1 << 13)) {
			buffer_append_float32(send_buffer, mc_interface_get_distance(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 14)) {
			buffer_append_float32(send_buffer, mc_interface_get_distance_abs(), 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 15)) {
			buffer_append_float32(send_buffer, mc_interface_get_pid_pos_now(), 1e6, &ind);
		}
		if (mask & ((uint32_t)1 << 16)) {
			send_buffer[ind++] = mc_interface_get_fault();
		}
		if (mask & ((uint32_t)1 << 17)) {
			uint8_t current_controller_id = app_get_configuration()->controller_id;
#ifdef HW_HAS_DUAL_MOTORS
			if (mc_interface_get_motor_thread() == 2) {
				current_controller_id = utils_second_motor_id();
			}
#endif
			send_buffer[ind++] = current_controller_id;
		}
		if (mask & ((uint32_t)1 << 18)) {
			send_buffer[ind++] = val.num_vescs;
		}
		if (mask & ((uint32_t)1 << 19)) {
			buffer_append_float32(send_buffer, wh_batt_left, 1e3, &ind);
		}
		if (mask & ((uint32_t)1 << 20)) {
			buffer_append_uint32(send_buffer, mc_interface_get_odometer(), &ind);
		}
		if (mask & ((uint32_t)1 << 21)) {
			buffer_append_uint32(send_buffer, chVTGetSystemTimeX() / (CH_CFG_ST_FREQUENCY / 1000), &ind);
		}

		reply_func(send_buffer, ind);
		chMtxUnlock(&send_buffer_mutex);
	    } break;

	case COMM_SET_ODOMETER: {
		int32_t ind = 0;
		mc_interface_set_odometer(buffer_get_uint32(data, &ind));
		timeout_reset();
	} break;

	case COMM_SET_MCCONF_TEMP:
	case COMM_SET_MCCONF_TEMP_SETUP: {
		mc_configuration *mcconf = mempools_alloc_mcconf();
		*mcconf = *mc_interface_get_configuration();

		int32_t ind = 0;
		bool store = data[ind++];
		bool forward_can = data[ind++];
		bool ack = data[ind++];
		bool divide_by_controllers = data[ind++];

		float controller_num = 1.0;

		if (divide_by_controllers) {
			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
				can_status_msg *msg = comm_can_get_status_msg_index(i);
				if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
					controller_num += 1.0;
				}
			}
		}

		mcconf->l_current_min_scale = buffer_get_float32_auto(data, &ind);
		mcconf->l_current_max_scale = buffer_get_float32_auto(data, &ind);

		if (packet_id == COMM_SET_MCCONF_TEMP_SETUP) {
			const float fact = ((mcconf->si_motor_poles / 2.0) * 60.0 *
					mcconf->si_gear_ratio) / (mcconf->si_wheel_diameter * M_PI);

			mcconf->l_min_erpm = buffer_get_float32_auto(data, &ind) * fact;
			mcconf->l_max_erpm = buffer_get_float32_auto(data, &ind) * fact;

			// Write computed RPM back and change forwarded packet id to
			// COMM_SET_MCCONF_TEMP. This way only the master has to be
			// aware of the setup information.
			ind -= 8;
			buffer_append_float32_auto(data, mcconf->l_min_erpm, &ind);
			buffer_append_float32_auto(data, mcconf->l_max_erpm, &ind);
		} else {
			mcconf->l_min_erpm = buffer_get_float32_auto(data, &ind);
			mcconf->l_max_erpm = buffer_get_float32_auto(data, &ind);
		}

		mcconf->l_min_duty = buffer_get_float32_auto(data, &ind);
		mcconf->l_max_duty = buffer_get_float32_auto(data, &ind);
		mcconf->l_watt_min = buffer_get_float32_auto(data, &ind) / controller_num;
		mcconf->l_watt_max = buffer_get_float32_auto(data, &ind) / controller_num;

		// Write divided data back to the buffer, as the other controllers have no way to tell
		// how many controllers are on the bus and thus need pre-divided data.
		// We set divide by controllers to false before forwarding.
		ind -= 8;
		buffer_append_float32_auto(data, mcconf->l_watt_min, &ind);
		buffer_append_float32_auto(data, mcconf->l_watt_max, &ind);

		// Battery limits can be set optionally in a backwards-compatible way.
		if ((int32_t)len >= (ind + 8)) {
			mcconf->l_in_current_min = buffer_get_float32_auto(data, &ind);
			mcconf->l_in_current_max = buffer_get_float32_auto(data, &ind);
		}

		mcconf->lo_current_min = mcconf->l_current_min * mcconf->l_current_min_scale;
		mcconf->lo_current_max = mcconf->l_current_max * mcconf->l_current_max_scale;
		mcconf->lo_current_motor_min_now = mcconf->lo_current_min;
		mcconf->lo_current_motor_max_now = mcconf->lo_current_max;
		mcconf->lo_in_current_min = mcconf->l_in_current_min;
		mcconf->lo_in_current_max = mcconf->l_in_current_max;

		commands_apply_mcconf_hw_limits(mcconf);

		if (store) {
			conf_general_store_mc_configuration(mcconf, mc_interface_get_motor_thread() == 2);
		}

		mc_interface_set_configuration(mcconf);

		if (forward_can) {
			data[-1] = COMM_SET_MCCONF_TEMP;
			data[1] = 0; // No more forward
			data[2] = 0; // No ack
			data[3] = 0; // No dividing, see comment above

			// TODO: Maybe broadcast on CAN-bus?
			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
				can_status_msg *msg = comm_can_get_status_msg_index(i);
				if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
					comm_can_send_buffer(msg->id, data - 1, len + 1, 0);
				}
			}
		}

		if (ack) {
			ind = 0;
			uint8_t send_buffer[50];
			send_buffer[ind++] = packet_id;
			reply_func(send_buffer, ind);
		}

		mempools_free_mcconf(mcconf);
	} break;

	case COMM_GET_MCCONF_TEMP: {
		mc_configuration *mcconf = mempools_alloc_mcconf();
		*mcconf = *mc_interface_get_configuration();
		int32_t ind = 0;
		uint8_t send_buffer[60];

		send_buffer[ind++] = packet_id;
		buffer_append_float32_auto(send_buffer, mcconf->l_current_min_scale, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_current_max_scale, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_min_erpm, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_max_erpm, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_min_duty, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_max_duty, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_watt_min, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_watt_max, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_in_current_min, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->l_in_current_max, &ind);
		// Setup config needed for speed calculation
		send_buffer[ind++] = (uint8_t)mcconf->si_motor_poles;
		buffer_append_float32_auto(send_buffer, mcconf->si_gear_ratio, &ind);
		buffer_append_float32_auto(send_buffer, mcconf->si_wheel_diameter, &ind);

		mempools_free_mcconf(mcconf);
		reply_func(send_buffer, ind);
	} break;

	case COMM_EXT_NRF_PRESENT: {
		if (!conf_general_permanent_nrf_found) {
			nrf_driver_init_ext_nrf();
			if (!nrf_driver_is_pairing()) {
				const app_configuration *appconf = app_get_configuration();
				uint8_t send_buffer[50];
				send_buffer[0] = COMM_EXT_NRF_ESB_SET_CH_ADDR;
				send_buffer[1] = appconf->app_nrf_conf.channel;
				send_buffer[2] = appconf->app_nrf_conf.address[0];
				send_buffer[3] = appconf->app_nrf_conf.address[1];
				send_buffer[4] = appconf->app_nrf_conf.address[2];
				commands_send_packet_nrf(send_buffer, 5);
			}
		}
	} break;

	case COMM_EXT_NRF_ESB_RX_DATA: {
		if (len > 2) {
			unsigned short crc = crc16((unsigned char*)data, len - 2);

			if (crc	== ((unsigned short) data[len - 2] << 8 |
					(unsigned short) data[len - 1])) {
				nrf_driver_process_packet(data, len);
			}
		}
	} break;

	case COMM_APP_DISABLE_OUTPUT: {
		int32_t ind = 0;
		bool fwd_can = data[ind++];
		int time = buffer_get_int32(data, &ind);
		app_disable_output(time);

		if (fwd_can) {
			data[0] = 0; // Don't continue forwarding
			comm_can_send_buffer(255, data - 1, len + 1, 0);
		}
	} break;

	case COMM_TERMINAL_CMD_SYNC:
		data[len] = '\0';
		chMtxLock(&terminal_mutex);
		terminal_process_string((char*)data);
		chMtxUnlock(&terminal_mutex);
		break;

	case COMM_GET_IMU_DATA: {
		int32_t ind = 0;
		uint8_t send_buffer[70];
		send_buffer[ind++] = packet_id;

		int32_t ind2 = 0;
		uint32_t mask = buffer_get_uint16(data, &ind2);

		float rpy[3], acc[3], gyro[3], mag[3], q[4];
		imu_get_rpy(rpy);
		imu_get_accel(acc);
		imu_get_gyro(gyro);
		imu_get_mag(mag);
		imu_get_quaternions(q);

		buffer_append_uint16(send_buffer, mask, &ind);

		if (mask & ((uint32_t)1 << 0)) {
			buffer_append_float32_auto(send_buffer, rpy[0], &ind);
		}
		if (mask & ((uint32_t)1 << 1)) {
			buffer_append_float32_auto(send_buffer, rpy[1], &ind);
		}
		if (mask & ((uint32_t)1 << 2)) {
			buffer_append_float32_auto(send_buffer, rpy[2], &ind);
		}

		if (mask & ((uint32_t)1 << 3)) {
			buffer_append_float32_auto(send_buffer, acc[0], &ind);
		}
		if (mask & ((uint32_t)1 << 4)) {
			buffer_append_float32_auto(send_buffer, acc[1], &ind);
		}
		if (mask & ((uint32_t)1 << 5)) {
			buffer_append_float32_auto(send_buffer, acc[2], &ind);
		}

		if (mask & ((uint32_t)1 << 6)) {
			buffer_append_float32_auto(send_buffer, gyro[0], &ind);
		}
		if (mask & ((uint32_t)1 << 7)) {
			buffer_append_float32_auto(send_buffer, gyro[1], &ind);
		}
		if (mask & ((uint32_t)1 << 8)) {
			buffer_append_float32_auto(send_buffer, gyro[2], &ind);
		}

		if (mask & ((uint32_t)1 << 9)) {
			buffer_append_float32_auto(send_buffer, mag[0], &ind);
		}
		if (mask & ((uint32_t)1 << 10)) {
			buffer_append_float32_auto(send_buffer, mag[1], &ind);
		}
		if (mask & ((uint32_t)1 << 11)) {
			buffer_append_float32_auto(send_buffer, mag[2], &ind);
		}

		if (mask & ((uint32_t)1 << 12)) {
			buffer_append_float32_auto(send_buffer, q[0], &ind);
		}
		if (mask & ((uint32_t)1 << 13)) {
			buffer_append_float32_auto(send_buffer, q[1], &ind);
		}
		if (mask & ((uint32_t)1 << 14)) {
			buffer_append_float32_auto(send_buffer, q[2], &ind);
		}
		if (mask & ((uint32_t)1 << 15)) {
			buffer_append_float32_auto(send_buffer, q[3], &ind);
		}

		if (mask & ((uint32_t)1 << 16)) {
			uint8_t current_controller_id = app_get_configuration()->controller_id;
#ifdef HW_HAS_DUAL_MOTORS
			if (mc_interface_get_motor_thread() == 2) {
				current_controller_id = utils_second_motor_id();
			}
#endif
			send_buffer[ind++] = current_controller_id;
		}

		reply_func(send_buffer, ind);
	} break;

	case COMM_ERASE_BOOTLOADER_ALL_CAN:
		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(6000);
		}

		data[-1] = COMM_ERASE_BOOTLOADER;
		comm_can_send_buffer(255, data - 1, len + 1, 2);
		chThdSleepMilliseconds(1500);
		/* Falls through. */
		/* no break */
	case COMM_ERASE_BOOTLOADER: {
		int32_t ind = 0;

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(6000);
		}
		uint16_t flash_res = flash_helper_erase_bootloader();

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_ERASE_BOOTLOADER;
		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
		reply_func(send_buffer, ind);
	} break;

	case COMM_SET_CURRENT_REL: {
		int32_t ind = 0;
		mc_interface_set_current_rel(buffer_get_float32(data, 1e5, &ind));
		timeout_reset();
	} break;

	case COMM_CAN_FWD_FRAME: {
		int32_t ind = 0;
		uint32_t id = buffer_get_uint32(data, &ind);
		bool is_ext = data[ind++];

		if (is_ext) {
			comm_can_transmit_eid(id, data + ind, len - ind);
		} else {
			comm_can_transmit_sid(id, data + ind, len - ind);
		}
	} break;

	case COMM_SET_BATTERY_CUT: {
		int32_t ind = 0;
		float start = buffer_get_float32(data, 1e3, &ind);
		float end = buffer_get_float32(data, 1e3, &ind);
		bool store = data[ind++];
		bool fwd_can = data[ind++];

		if (fwd_can) {
			comm_can_conf_battery_cut(255, store, start, end);
		}

		mc_configuration *mcconf = mempools_alloc_mcconf();
		*mcconf = *mc_interface_get_configuration();

		if (mcconf->l_battery_cut_start != start || mcconf->l_battery_cut_end != end) {
			mcconf->l_battery_cut_start = start;
			mcconf->l_battery_cut_end = end;

			if (store) {
				conf_general_store_mc_configuration(mcconf,
						mc_interface_get_motor_thread() == 2);
			}

			mc_interface_set_configuration(mcconf);
		}

		mempools_free_mcconf(mcconf);

		// Send ack
		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = packet_id;
		reply_func(send_buffer, ind);
	} break;

	case COMM_GET_BATTERY_CUT: {
		int32_t ind = 0;
		uint8_t send_buffer[60];
		volatile const mc_configuration *mcconf = mc_interface_get_configuration();

		send_buffer[ind++] = packet_id;
		buffer_append_float32(send_buffer, mcconf->l_battery_cut_start, 1e3, &ind);
		buffer_append_float32(send_buffer, mcconf->l_battery_cut_end, 1e3, &ind);

		reply_func(send_buffer, ind);
	} break;

	case COMM_SET_CAN_MODE: {
		int32_t ind = 0;
		bool store = data[ind++];
		bool ack = data[ind++];
		int mode = data[ind++];

		app_configuration *appconf = mempools_alloc_appconf();
		*appconf = *app_get_configuration();
		appconf->can_mode = mode;

		if (store) {
			conf_general_store_app_configuration(appconf);
		}

		app_set_configuration(appconf);

		mempools_free_appconf(appconf);

		if (ack) {
			ind = 0;
			uint8_t send_buffer[50];
			send_buffer[ind++] = packet_id;
			reply_func(send_buffer, ind);
		}
	} break;

	case COMM_BMS_GET_VALUES:
	case COMM_BMS_SET_CHARGE_ALLOWED:
	case COMM_BMS_SET_BALANCE_OVERRIDE:
	case COMM_BMS_RESET_COUNTERS:
	case COMM_BMS_FORCE_BALANCE:
	case COMM_BMS_ZERO_CURRENT_OFFSET: {
		bms_process_cmd(data - 1, len + 1, reply_func);
		break;
	}

	// Power switch
	case COMM_PSW_GET_STATUS: {
		int32_t ind = 0;
		bool by_id = data[ind++];
		int id_ind = buffer_get_int16(data, &ind);

		int psws_num = 0;
		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
			psw_status *stat = comm_can_get_psw_status_index(i);
			if (stat->id >= 0) {
				psws_num++;
			} else {
				break;
			}
		}

		psw_status *stat = 0;
		if (by_id) {
			stat = comm_can_get_psw_status_id(id_ind);
		} else if (id_ind < psws_num) {
			stat = comm_can_get_psw_status_index(id_ind);
		}

		if (stat) {
			ind = 0;
			uint8_t send_buffer[70];

			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, stat->id, &ind);
			buffer_append_int16(send_buffer, psws_num, &ind);
			buffer_append_float32_auto(send_buffer, UTILS_AGE_S(stat->rx_time), &ind);
			buffer_append_float32_auto(send_buffer, stat->v_in, &ind);
			buffer_append_float32_auto(send_buffer, stat->v_out, &ind);
			buffer_append_float32_auto(send_buffer, stat->temp, &ind);
			send_buffer[ind++] = stat->is_out_on;
			send_buffer[ind++] = stat->is_pch_on;
			send_buffer[ind++] = stat->is_dsc_on;

			reply_func(send_buffer, ind);
		}
	} break;

	case COMM_PSW_SWITCH: {
		int32_t ind = 0;
		int id = buffer_get_int16(data, &ind);
		bool is_on = data[ind++];
		bool plot = data[ind++];
		comm_can_psw_switch(id, is_on, plot);
	} break;

	case COMM_GET_QML_UI_HW: {
#ifdef QMLUI_SOURCE_HW
		int32_t ind = 0;

		int32_t len_qml = buffer_get_int32(data, &ind);
		int32_t ofs_qml = buffer_get_int32(data, &ind);

		if ((len_qml + ofs_qml) > DATA_QML_HW_SIZE || len_qml > (PACKET_MAX_PL_LEN - 10)) {
			break;
		}

		chMtxLock(&send_buffer_mutex);
		ind = 0;
		send_buffer_global[ind++] = packet_id;
		buffer_append_int32(send_buffer_global, DATA_QML_HW_SIZE, &ind);
		buffer_append_int32(send_buffer_global, ofs_qml, &ind);
		memcpy(send_buffer_global + ind, data_qml_hw + ofs_qml, len_qml);
		ind += len_qml;
		reply_func(send_buffer_global, ind);

		chMtxUnlock(&send_buffer_mutex);
#endif
	} break;

	case COMM_GET_QML_UI_APP: {
		int32_t ind = 0;

		int32_t len_qml = buffer_get_int32(data, &ind);
		int32_t ofs_qml = buffer_get_int32(data, &ind);

		uint8_t *qmlui_data = flash_helper_qmlui_data();
		int32_t qmlui_len = flash_helper_qmlui_size();

#ifdef QMLUI_SOURCE_APP
		qmlui_data = data_qml_app;
		qmlui_len = DATA_QML_APP_SIZE;
#endif

		if (!qmlui_data) {
			break;
		}

		if ((len_qml + ofs_qml) > qmlui_len || len_qml > (PACKET_MAX_PL_LEN - 10)) {
			break;
		}

		chMtxLock(&send_buffer_mutex);
		ind = 0;
		send_buffer_global[ind++] = packet_id;
		buffer_append_int32(send_buffer_global, qmlui_len, &ind);
		buffer_append_int32(send_buffer_global, ofs_qml, &ind);
		memcpy(send_buffer_global + ind, qmlui_data + ofs_qml, len_qml);
		ind += len_qml;
		reply_func(send_buffer_global, ind);

		chMtxUnlock(&send_buffer_mutex);
	} break;

	case COMM_QMLUI_ERASE: {
		int32_t ind = 0;

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(6000);
		}
		uint16_t flash_res = flash_helper_erase_qmlui();

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_QMLUI_ERASE;
		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
		reply_func(send_buffer, ind);
	} break;

	case COMM_QMLUI_WRITE: {
		int32_t ind = 0;
		uint32_t qmlui_offset = buffer_get_uint32(data, &ind);

		if (nrf_driver_ext_nrf_running()) {
			nrf_driver_pause(2000);
		}
		uint16_t flash_res = flash_helper_write_qmlui(qmlui_offset, data + ind, len - ind);

		SHUTDOWN_RESET();

		ind = 0;
		uint8_t send_buffer[50];
		send_buffer[ind++] = COMM_QMLUI_WRITE;
		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
		buffer_append_uint32(send_buffer, qmlui_offset, &ind);
		reply_func(send_buffer, ind);
	} break;

	case COMM_IO_BOARD_GET_ALL: {
		int32_t ind = 0;
		int id = buffer_get_int16(data, &ind);

		io_board_adc_values *adc_1_4 = comm_can_get_io_board_adc_1_4_id(id);
		io_board_adc_values *adc_5_8 = comm_can_get_io_board_adc_5_8_id(id);
		io_board_digial_inputs *digital_in = comm_can_get_io_board_digital_in_id(id);

		if (!adc_1_4 && !adc_5_8 && !digital_in) {
			break;
		}

		uint8_t send_buffer[70];
		ind = 0;
		send_buffer[ind++] = packet_id;
		buffer_append_int16(send_buffer, id, &ind);

		if (adc_1_4) {
			send_buffer[ind++] = 1;
			buffer_append_float32_auto(send_buffer, UTILS_AGE_S(adc_1_4->rx_time), &ind);
			buffer_append_float16(send_buffer, adc_1_4->adc_voltages[0], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_1_4->adc_voltages[1], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_1_4->adc_voltages[2], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_1_4->adc_voltages[3], 1e2, &ind);
		}

		if (adc_5_8) {
			send_buffer[ind++] = 2;
			buffer_append_float32_auto(send_buffer, UTILS_AGE_S(adc_1_4->rx_time), &ind);
			buffer_append_float16(send_buffer, adc_5_8->adc_voltages[0], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_5_8->adc_voltages[1], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_5_8->adc_voltages[2], 1e2, &ind);
			buffer_append_float16(send_buffer, adc_5_8->adc_voltages[3], 1e2, &ind);
		}

		if (digital_in) {
			send_buffer[ind++] = 3;
			buffer_append_float32_auto(send_buffer, UTILS_AGE_S(adc_1_4->rx_time), &ind);
			buffer_append_uint32(send_buffer, (digital_in->inputs >> 32) & 0xFFFFFFFF, &ind);
			buffer_append_uint32(send_buffer, (digital_in->inputs >> 0) & 0xFFFFFFFF, &ind);
		}

		reply_func(send_buffer, ind);
	} break;

	case COMM_IO_BOARD_SET_PWM: {
		int32_t ind = 0;
		int id = buffer_get_int16(data, &ind);
		int channel = buffer_get_int16(data, &ind);
		float duty = buffer_get_float32_auto(data, &ind);
		comm_can_io_board_set_output_pwm(id, channel, duty);
	} break;

	case COMM_IO_BOARD_SET_DIGITAL: {
		int32_t ind = 0;
		int id = buffer_get_int16(data, &ind);
		int channel = buffer_get_int16(data, &ind);
		bool on = data[ind++];
		comm_can_io_board_set_output_digital(id, channel, on);
	} break;

	case COMM_GET_STATS: {
		int32_t ind = 0;
		uint32_t mask = buffer_get_uint16(data, &ind);

		ind = 0;
		uint8_t send_buffer[60];
		send_buffer[ind++] = packet_id;
		buffer_append_uint32(send_buffer, mask, &ind);

		if (mask & ((uint32_t)1 << 0)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_speed_avg(), &ind); }
		if (mask & ((uint32_t)1 << 1)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_speed_max(), &ind); }
		if (mask & ((uint32_t)1 << 2)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_power_avg(), &ind); }
		if (mask & ((uint32_t)1 << 3)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_power_max(), &ind); }
		if (mask & ((uint32_t)1 << 4)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_current_avg(), &ind); }
		if (mask & ((uint32_t)1 << 5)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_current_max(), &ind); }
		if (mask & ((uint32_t)1 << 6)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_mosfet_avg(), &ind); }
		if (mask & ((uint32_t)1 << 7)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_mosfet_max(), &ind); }
		if (mask & ((uint32_t)1 << 8)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_motor_avg(), &ind); }
		if (mask & ((uint32_t)1 << 9)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_temp_motor_max(), &ind); }
		if (mask & ((uint32_t)1 << 10)) { buffer_append_float32_auto(send_buffer, mc_interface_stat_count_time(), &ind); }

		reply_func(send_buffer, ind);
	} break;

	case COMM_RESET_STATS: {
		bool ack = false;

		if (len > 0) {
			ack = data[0];
		}

		if (ack) {
			int32_t ind = 0;
			uint8_t send_buffer[50];
			send_buffer[ind++] = packet_id;
			reply_func(send_buffer, ind);
		}
	} break;

	// Blocking commands. Only one of them runs at any given time, in their
	// own thread. If other blocking commands come before the previous one has
	// finished, they are discarded.
	case COMM_TERMINAL_CMD:
	case COMM_DETECT_MOTOR_PARAM:
	case COMM_DETECT_MOTOR_R_L:
	case COMM_DETECT_MOTOR_FLUX_LINKAGE:
	case COMM_DETECT_ENCODER:
	case COMM_DETECT_HALL_FOC:
	case COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP:
	case COMM_DETECT_APPLY_ALL_FOC:
	case COMM_PING_CAN:
	case COMM_BM_CONNECT:
	case COMM_BM_ERASE_FLASH_ALL:
	case COMM_BM_WRITE_FLASH_LZO:
	case COMM_BM_WRITE_FLASH:
	case COMM_BM_REBOOT:
	case COMM_BM_DISCONNECT:
	case COMM_BM_MAP_PINS_DEFAULT:
	case COMM_BM_MAP_PINS_NRF5X:
	case COMM_BM_MEM_READ:
	case COMM_GET_IMU_CALIBRATION:
	case COMM_BM_MEM_WRITE:
		if (!is_blocking) {
			memcpy(blocking_thread_cmd_buffer, data - 1, len + 1);
			blocking_thread_cmd_len = len + 1;
			is_blocking = true;
			blocking_thread_motor = mc_interface_get_motor_thread();
			send_func_blocking = reply_func;
			chEvtSignal(blocking_tp, (eventmask_t)1);
		}
		break;

	default:
		break;
	}
}

void commands_printf(const char* format, ...) {
	chMtxLock(&print_mutex);

	va_list arg;
	va_start (arg, format);
	int len;
	static char print_buffer[255];

	print_buffer[0] = COMM_PRINT;
	len = vsnprintf(print_buffer + 1, 254, format, arg);
	va_end (arg);

	if(len > 0) {
		commands_send_packet_last_blocking((unsigned char*)print_buffer,
				(len < 254) ? len + 1 : 255);
	}

	chMtxUnlock(&print_mutex);
}

void commands_send_rotor_pos(float rotor_pos) {
	uint8_t buffer[5];
	int32_t index = 0;
	buffer[index++] = COMM_ROTOR_POSITION;
	buffer_append_int32(buffer, (int32_t)(rotor_pos * 100000.0), &index);
	commands_send_packet(buffer, index);
}

void commands_send_experiment_samples(float *samples, int len) {
	if ((len * 4 + 1) > 256) {
		return;
	}

	uint8_t buffer[len * 4 + 1];
	int32_t index = 0;

	buffer[index++] = COMM_EXPERIMENT_SAMPLE;

	for (int i = 0;i < len;i++) {
		buffer_append_int32(buffer, (int32_t)(samples[i] * 10000.0), &index);
	}

	commands_send_packet(buffer, index);
}

void commands_fwd_can_frame(int len, unsigned char *data, uint32_t id, bool is_extended) {
	if (len > 8) {
		len = 8;
	}

	uint8_t buffer[len + 6];
	int32_t index = 0;
	buffer[index++] = COMM_CAN_FWD_FRAME;
	buffer_append_uint32(buffer, id, &index);
	buffer[index++] = is_extended;
	memcpy(buffer + index, data, len);
	index += len;
	commands_send_packet(buffer, index);
}

disp_pos_mode commands_get_disp_pos_mode(void) {
	return display_position_mode;
}

void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len)) {
	appdata_func = func;
}

void commands_set_hw_data_handler(void(*func)(unsigned char *data, unsigned int len)) {
	hwdata_func = func;
}

void commands_send_app_data(unsigned char *data, unsigned int len) {
	int32_t index = 0;
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[index++] = COMM_CUSTOM_APP_DATA;
	memcpy(send_buffer_global + index, data, len);
	index += len;
	commands_send_packet(send_buffer_global, index);
	chMtxUnlock(&send_buffer_mutex);
}

void commands_send_hw_data(unsigned char *data, unsigned int len) {
	int32_t index = 0;
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[index++] = COMM_CUSTOM_HW_DATA;
	memcpy(send_buffer_global + index, data, len);
	index += len;
	commands_send_packet(send_buffer_global, index);
	chMtxUnlock(&send_buffer_mutex);
}

void commands_send_gpd_buffer_notify(void) {
	int32_t index = 0;
	uint8_t buffer[1];
	buffer[index++] = COMM_GPD_BUFFER_NOTIFY;
	commands_send_packet(buffer, index);
}

void commands_send_mcconf(COMM_PACKET_ID packet_id, mc_configuration *mcconf) {
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[0] = packet_id;
	int32_t len = confgenerator_serialize_mcconf(send_buffer_global + 1, mcconf);
	commands_send_packet(send_buffer_global, len + 1);
	chMtxUnlock(&send_buffer_mutex);
}

void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf) {
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[0] = packet_id;
	int32_t len = confgenerator_serialize_appconf(send_buffer_global + 1, appconf);
	commands_send_packet(send_buffer_global, len + 1);
	chMtxUnlock(&send_buffer_mutex);
}

inline static float hw_lim_upper(float l, float h) {(void)l; return h;}

void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
	utils_truncate_number(&mcconf->l_current_max_scale, 0.0, 1.0);
	utils_truncate_number(&mcconf->l_current_min_scale, 0.0, 1.0);

	float ctrl_loop_freq = 0.0;

	// This limit should always be active, as starving the threads never
	// makes sense.
#ifdef HW_LIM_FOC_CTRL_LOOP_FREQ
    if (mcconf->foc_sample_v0_v7 == true) {
    	//control loop executes twice per pwm cycle when sampling in v0 and v7
		utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ);
		ctrl_loop_freq = mcconf->foc_f_sw;
    } else {
#ifdef HW_HAS_DUAL_MOTORS
    	utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ);
    	ctrl_loop_freq = mcconf->foc_f_sw;
#else
		utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ * 2.0);
		ctrl_loop_freq = mcconf->foc_f_sw / 2.0;
#endif
    }
#endif

    if (ctrl_loop_freq >= (hw_lim_upper(HW_LIM_FOC_CTRL_LOOP_FREQ) * 0.9)) {
    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 2);
    } else if (ctrl_loop_freq >= (hw_lim_upper(HW_LIM_FOC_CTRL_LOOP_FREQ) * 0.7)) {
    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 4);
    } else {
    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 10);
    }

#ifndef DISABLE_HW_LIMITS

    // TODO: Maybe truncate values that get close to numerical instabilities when set
    // close to each other, such as
    //
    // conf->l_temp_motor_start, conf->l_temp_motor_end
    // and
    // conf->l_temp_fet_start, conf->l_temp_fet_end
    //
    // A division by 0 is avoided in the code, but getting close can still make things
    // oscillate. At the moment we leave the responsibility of setting sane values
    // to the user.

#ifdef HW_LIM_CURRENT
	utils_truncate_number(&mcconf->l_current_max, HW_LIM_CURRENT);
	utils_truncate_number(&mcconf->l_current_min, HW_LIM_CURRENT);
#endif
#ifdef HW_LIM_CURRENT_IN
	utils_truncate_number(&mcconf->l_in_current_max, HW_LIM_CURRENT_IN);
	utils_truncate_number(&mcconf->l_in_current_min, HW_LIM_CURRENT);
#endif
#ifdef HW_LIM_CURRENT_ABS
	utils_truncate_number(&mcconf->l_abs_current_max, HW_LIM_CURRENT_ABS);
#endif
#ifdef HW_LIM_VIN
	utils_truncate_number(&mcconf->l_max_vin, HW_LIM_VIN);
	utils_truncate_number(&mcconf->l_min_vin, HW_LIM_VIN);
#endif
#ifdef HW_LIM_ERPM
	utils_truncate_number(&mcconf->l_max_erpm, HW_LIM_ERPM);
	utils_truncate_number(&mcconf->l_min_erpm, HW_LIM_ERPM);
#endif
#ifdef HW_LIM_DUTY_MIN
	utils_truncate_number(&mcconf->l_min_duty, HW_LIM_DUTY_MIN);
#endif
#ifdef HW_LIM_DUTY_MAX
	utils_truncate_number(&mcconf->l_max_duty, HW_LIM_DUTY_MAX);
#endif
#ifdef HW_LIM_TEMP_FET
	utils_truncate_number(&mcconf->l_temp_fet_start, HW_LIM_TEMP_FET);
	utils_truncate_number(&mcconf->l_temp_fet_end, HW_LIM_TEMP_FET);
#endif
#ifdef HW_FOC_CURRENT_FILTER_LIM
	utils_truncate_number(&mcconf->foc_current_filter_const, HW_FOC_CURRENT_FILTER_LIM);
#endif
#endif
}

void commands_init_plot(char *namex, char *namey) {
	int ind = 0;
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[ind++] = COMM_PLOT_INIT;
	memcpy(send_buffer_global + ind, namex, strlen(namex));
	ind += strlen(namex);
	send_buffer_global[ind++] = '\0';
	memcpy(send_buffer_global + ind, namey, strlen(namey));
	ind += strlen(namey);
	send_buffer_global[ind++] = '\0';
	commands_send_packet(send_buffer_global, ind);
	chMtxUnlock(&send_buffer_mutex);
}

void commands_plot_add_graph(char *name) {
	int ind = 0;
	chMtxLock(&send_buffer_mutex);
	send_buffer_global[ind++] = COMM_PLOT_ADD_GRAPH;
	memcpy(send_buffer_global + ind, name, strlen(name));
	ind += strlen(name);
	send_buffer_global[ind++] = '\0';
	commands_send_packet(send_buffer_global, ind);
	chMtxUnlock(&send_buffer_mutex);
}

void commands_plot_set_graph(int graph) {
	int ind = 0;
	uint8_t buffer[2];
	buffer[ind++] = COMM_PLOT_SET_GRAPH;
	buffer[ind++] = graph;
	commands_send_packet(buffer, ind);
}

void commands_send_plot_points(float x, float y) {
	int32_t ind = 0;
	uint8_t buffer[10];
	buffer[ind++] = COMM_PLOT_DATA;
	buffer_append_float32_auto(buffer, x, &ind);
	buffer_append_float32_auto(buffer, y, &ind);
	commands_send_packet(buffer, ind);
}

int commands_get_fw_version_sent_cnt(void) {
	return fw_version_sent_cnt;
}

// TODO: The commands_set_ble_name and commands_set_ble_pin are not
// tested. Test them, and remove this comment when done!

void commands_set_ble_name(char* name) {
	int ind = 0;
	int name_len = strlen(name);
	if (name_len > 27) {
		name_len = 27;
	}

	uint8_t buffer[name_len + 2];
	buffer[ind++] = COMM_SET_BLE_NAME;
	memcpy(buffer + ind, name, name_len);
	ind += name_len;
	buffer[ind++] = '\0';

#ifdef HW_UART_P_DEV
	app_uartcomm_send_packet(buffer, ind, UART_PORT_BUILTIN);
#else
	app_uartcomm_send_packet(buffer, ind, UART_PORT_COMM_HEADER);
#endif
}

void commands_set_ble_pin(char* pin) {
	int ind = 0;
	int pin_len = strlen(pin);
	if (pin_len > 27) {
		pin_len = 27;
	}

	uint8_t buffer[pin_len + 2];
	buffer[ind++] = COMM_SET_BLE_NAME;
	memcpy(buffer + ind, pin, pin_len);
	ind += pin_len;
	buffer[ind++] = '\0';
#ifdef HW_UART_P_DEV
	app_uartcomm_send_packet(buffer, ind, UART_PORT_BUILTIN);
#else
	app_uartcomm_send_packet(buffer, ind, UART_PORT_COMM_HEADER);
#endif
}

static THD_FUNCTION(blocking_thread, arg) {
	(void)arg;

	chRegSetThreadName("comm_block");

	blocking_tp = chThdGetSelfX();

	for(;;) {
		is_blocking = false;

		chEvtWaitAny((eventmask_t) 1);

		mc_interface_select_motor_thread(blocking_thread_motor);

		uint8_t *data = blocking_thread_cmd_buffer;
		unsigned int len = blocking_thread_cmd_len;

		COMM_PACKET_ID packet_id;
		static uint8_t send_buffer[512];

		packet_id = data[0];
		data++;
		len--;

		switch (packet_id) {
		case COMM_DETECT_MOTOR_PARAM: {
			int32_t ind = 0;
			float detect_current = buffer_get_float32(data, 1e3, &ind);
			float detect_min_rpm = buffer_get_float32(data, 1e3, &ind);
			float detect_low_duty = buffer_get_float32(data, 1e3, &ind);
			float detect_cycle_int_limit;
			float detect_coupling_k;
			int8_t detect_hall_table[8];
			int detect_hall_res;

			if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
					detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
					detect_hall_table, &detect_hall_res)) {
				detect_cycle_int_limit = 0.0;
				detect_coupling_k = 0.0;
			}

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
			buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
			buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
			memcpy(send_buffer + ind, detect_hall_table, 8);
			ind += 8;
			send_buffer[ind++] = detect_hall_res;

			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_DETECT_MOTOR_R_L: {
			mc_configuration *mcconf = mempools_alloc_mcconf();
			*mcconf = *mc_interface_get_configuration();
			mc_configuration *mcconf_old = mempools_alloc_mcconf();
			*mcconf_old = *mcconf;

			mcconf->motor_type = MOTOR_TYPE_FOC;
			mc_interface_set_configuration(mcconf);

			float r = 0.0;
			float l = 0.0;
			float ld_lq_diff = 0.0;
			bool res = mcpwm_foc_measure_res_ind(&r, &l, &ld_lq_diff);
			mc_interface_set_configuration(mcconf_old);

			if (!res) {
				r = 0.0;
				l = 0.0;
			}

			int32_t ind = 0;
			send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
			buffer_append_float32(send_buffer, r, 1e6, &ind);
			buffer_append_float32(send_buffer, l, 1e3, &ind);
			buffer_append_float32(send_buffer, ld_lq_diff, 1e3, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}

			mempools_free_mcconf(mcconf);
			mempools_free_mcconf(mcconf_old);
		} break;

		case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
			int32_t ind = 0;
			float current = buffer_get_float32(data, 1e3, &ind);
			float min_rpm = buffer_get_float32(data, 1e3, &ind);
			float duty = buffer_get_float32(data, 1e3, &ind);
			float resistance = buffer_get_float32(data, 1e6, &ind);

			float linkage;
			bool res = conf_general_measure_flux_linkage(current, duty, min_rpm, resistance, &linkage);

			if (!res) {
				linkage = 0.0;
			}

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE;
			buffer_append_float32(send_buffer, linkage, 1e7, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_DETECT_ENCODER: {
			if (encoder_is_configured()) {
				mc_configuration *mcconf = mempools_alloc_mcconf();
				*mcconf = *mc_interface_get_configuration();
				mc_configuration *mcconf_old = mempools_alloc_mcconf();
				*mcconf_old = *mcconf;

				int32_t ind = 0;
				float current = buffer_get_float32(data, 1e3, &ind);

				mcconf->motor_type = MOTOR_TYPE_FOC;
				mcconf->foc_f_sw = 10000.0;
				mcconf->foc_current_kp = 0.01;
				mcconf->foc_current_ki = 10.0;
				mc_interface_set_configuration(mcconf);

				float offset = 0.0;
				float ratio = 0.0;
				bool inverted = false;
				mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
				mc_interface_set_configuration(mcconf_old);

				ind = 0;
				send_buffer[ind++] = COMM_DETECT_ENCODER;
				buffer_append_float32(send_buffer, offset, 1e6, &ind);
				buffer_append_float32(send_buffer, ratio, 1e6, &ind);
				send_buffer[ind++] = inverted;

				if (send_func_blocking) {
					send_func_blocking(send_buffer, ind);
				}

				mempools_free_mcconf(mcconf);
				mempools_free_mcconf(mcconf_old);
			} else {
				int32_t ind = 0;
				send_buffer[ind++] = COMM_DETECT_ENCODER;
				buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
				buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
				send_buffer[ind++] = false;

				if (send_func_blocking) {
					send_func_blocking(send_buffer, ind);
				}
			}
		} break;

		case COMM_DETECT_HALL_FOC: {
			mc_configuration *mcconf = mempools_alloc_mcconf();
			*mcconf = *mc_interface_get_configuration();

			if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
				mc_configuration *mcconf_old = mempools_alloc_mcconf();
				*mcconf_old = *mcconf;

				int32_t ind = 0;
				float current = buffer_get_float32(data, 1e3, &ind);

				mcconf->motor_type = MOTOR_TYPE_FOC;
				mcconf->foc_f_sw = 10000.0;
				mcconf->foc_current_kp = 0.01;
				mcconf->foc_current_ki = 10.0;
				mc_interface_set_configuration(mcconf);

				uint8_t hall_tab[8];
				bool res = mcpwm_foc_hall_detect(current, hall_tab);
				mc_interface_set_configuration(mcconf_old);

				ind = 0;
				send_buffer[ind++] = COMM_DETECT_HALL_FOC;
				memcpy(send_buffer + ind, hall_tab, 8);
				ind += 8;
				send_buffer[ind++] = res ? 0 : 1;

				if (send_func_blocking) {
					send_func_blocking(send_buffer, ind);
				}

				mempools_free_mcconf(mcconf_old);
			} else {
				int32_t ind = 0;
				send_buffer[ind++] = COMM_DETECT_HALL_FOC;
				memset(send_buffer, 255, 8);
				ind += 8;
				send_buffer[ind++] = 0;
				if (send_func_blocking) {
					send_func_blocking(send_buffer, ind);
				}
			}

			mempools_free_mcconf(mcconf);
		} break;

		case COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP: {
			int32_t ind = 0;
			float current = buffer_get_float32(data, 1e3, &ind);
			float erpm_per_sec = buffer_get_float32(data, 1e3, &ind);
			float duty = buffer_get_float32(data, 1e3, &ind);
			float resistance = buffer_get_float32(data, 1e6, &ind);
			float inductance = 0.0;

			if (len >= (uint32_t)ind + 4) {
				inductance = buffer_get_float32(data, 1e8, &ind);
			}

			float linkage, linkage_undriven, undriven_samples;
			bool res = conf_general_measure_flux_linkage_openloop(current, duty,
					erpm_per_sec, resistance, inductance,
					&linkage, &linkage_undriven, &undriven_samples);

			if (undriven_samples > 60) {
				linkage = linkage_undriven;
			}

			if (!res) {
				linkage = 0.0;
			}

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP;
			buffer_append_float32(send_buffer, linkage, 1e7, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_DETECT_APPLY_ALL_FOC: {
			int32_t ind = 0;
			bool detect_can = data[ind++];
			float max_power_loss = buffer_get_float32(data, 1e3, &ind);
			float min_current_in = buffer_get_float32(data, 1e3, &ind);
			float max_current_in = buffer_get_float32(data, 1e3, &ind);
			float openloop_rpm = buffer_get_float32(data, 1e3, &ind);
			float sl_erpm = buffer_get_float32(data, 1e3, &ind);

			int res = conf_general_detect_apply_all_foc_can(detect_can, max_power_loss,
					min_current_in, max_current_in, openloop_rpm, sl_erpm);

			ind = 0;
			send_buffer[ind++] = COMM_DETECT_APPLY_ALL_FOC;
			buffer_append_int16(send_buffer, res, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_TERMINAL_CMD:
			data[len] = '\0';
			chMtxLock(&terminal_mutex);
			terminal_process_string((char*)data);
			chMtxUnlock(&terminal_mutex);
			break;

		case COMM_PING_CAN: {
			int32_t ind = 0;
			send_buffer[ind++] = COMM_PING_CAN;

			for (uint8_t i = 0;i < 255;i++) {
				HW_TYPE hw_type;
				if (comm_can_ping(i, &hw_type)) {
					send_buffer[ind++] = i;
				}
			}

			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

#if HAS_BLACKMAGIC
		case COMM_BM_CONNECT: {
			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, bm_connect(), &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_ERASE_FLASH_ALL: {
			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, bm_erase_flash_all(), &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_WRITE_FLASH_LZO:
		case COMM_BM_WRITE_FLASH: {
			if (packet_id == COMM_BM_WRITE_FLASH_LZO) {
				memcpy(send_buffer, data + 6, len - 6);
				int32_t ind = 4;
				lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
				lzo1x_decompress_safe(send_buffer, len - 6, data + 4, &decompressed_len, NULL);
				len = decompressed_len + 4;
			}

			int32_t ind = 0;
			uint32_t addr = buffer_get_uint32(data, &ind);

			int res = bm_write_flash(addr, data + ind, len - ind);

			ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, res, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_REBOOT: {
			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, bm_reboot(), &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_HALT_REQ: {
			bm_halt_req();

			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_DISCONNECT: {
			bm_disconnect();
			bm_leave_nrf_debug_mode();

			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_MAP_PINS_DEFAULT: {
			bm_default_swd_pins();
			int32_t ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, 1, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_MAP_PINS_NRF5X: {
			int32_t ind = 0;
			send_buffer[ind++] = packet_id;

#ifdef NRF5x_SWDIO_GPIO
			buffer_append_int16(send_buffer, 1, &ind);
			bm_change_swd_pins(NRF5x_SWDIO_GPIO, NRF5x_SWDIO_PIN,
					NRF5x_SWCLK_GPIO, NRF5x_SWCLK_PIN);
#else
			buffer_append_int16(send_buffer, 0, &ind);
#endif
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		case COMM_BM_MEM_READ: {
			int32_t ind = 0;
			uint32_t addr = buffer_get_uint32(data, &ind);
			uint16_t read_len = buffer_get_uint16(data, &ind);

			if (read_len > sizeof(send_buffer) - 3) {
				read_len = sizeof(send_buffer) - 3;
			}

			int res = bm_mem_read(addr, send_buffer + 3, read_len);

			ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, res, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind + read_len);
			}
		} break;

		case COMM_BM_MEM_WRITE: {
			int32_t ind = 0;
			uint32_t addr = buffer_get_uint32(data, &ind);

			int res = bm_mem_write(addr, data + ind, len - ind);

			ind = 0;
			send_buffer[ind++] = packet_id;
			buffer_append_int16(send_buffer, res, &ind);
			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;
#endif
		case COMM_GET_IMU_CALIBRATION: {
			int32_t ind = 0;
			float yaw = buffer_get_float32(data, 1e3, &ind);
			float imu_cal[9];
			imu_get_calibration(yaw, imu_cal);

			ind = 0;
			send_buffer[ind++] = COMM_GET_IMU_CALIBRATION;
			buffer_append_float32(send_buffer, imu_cal[0], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[1], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[2], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[3], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[4], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[5], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[6], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[7], 1e6, &ind);
			buffer_append_float32(send_buffer, imu_cal[8], 1e6, &ind);

			if (send_func_blocking) {
				send_func_blocking(send_buffer, ind);
			}
		} break;

		default:
			break;
		}
	}
}