<commit_before>#include <Poco/Exception.h>
#include <Poco/Logger.h>
#include <Poco/Nullable.h>
#include <Poco/Timestamp.h>

#include "di/Injectable.h"
#include "model/Device.h"
#include "model/Gateway.h"
#include "service/DeviceServiceImpl.h"
#include "work/DeviceUnpairWork.h"
#include "work/WorkFacade.h"

BEEEON_OBJECT_BEGIN(BeeeOn, DeviceServiceImpl)
BEEEON_OBJECT_CASTABLE(DeviceService)
BEEEON_OBJECT_REF("deviceDao", &DeviceServiceImpl::setDeviceDao)
BEEEON_OBJECT_REF("controlDao", &DeviceServiceImpl::setControlDao)
BEEEON_OBJECT_REF("sensorHistoryDao", &DeviceServiceImpl::setSensorHistoryDao)
BEEEON_OBJECT_REF("devicePropertyDao", &DeviceServiceImpl::setDevicePropertyDao)
BEEEON_OBJECT_REF("gatewayRPC", &DeviceServiceImpl::setGatewayRPC)
BEEEON_OBJECT_REF("accessPolicy", &DeviceServiceImpl::setAccessPolicy)
BEEEON_OBJECT_REF("workFacade", &DeviceServiceImpl::setWorkFacade)
BEEEON_OBJECT_REF("transactionManager", &DeviceServiceImpl::setTransactionManager)
BEEEON_OBJECT_END(BeeeOn, DeviceServiceImpl)

using namespace std;
using namespace Poco;
using namespace BeeeOn;

DeviceServiceImpl::DeviceServiceImpl()
{
}

void DeviceServiceImpl::setDeviceDao(DeviceDao::Ptr dao)
{
	m_dao = dao;
}

void DeviceServiceImpl::setControlDao(ControlDao::Ptr dao)
{
	m_controlDao = dao;
}

void DeviceServiceImpl::setSensorHistoryDao(SensorHistoryDao::Ptr dao)
{
	m_historyDao = dao;
}

void DeviceServiceImpl::setDevicePropertyDao(DevicePropertyDao::Ptr dao)
{
	m_propertyDao = dao;
}

void DeviceServiceImpl::setGatewayRPC(GatewayRPC::Ptr rpc)
{
	m_gatewayRPC = rpc;
}

void DeviceServiceImpl::setWorkFacade(WorkFacade::Ptr facade)
{
	m_workFacade = facade;
}

void DeviceServiceImpl::setAccessPolicy(DeviceAccessPolicy::Ptr policy)
{
	m_policy = policy;
}

bool DeviceServiceImpl::doFetch(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.target(), input.base());
	return m_dao->fetch(input.target(), input.base());
}

void DeviceServiceImpl::valuesFor(DeviceWithData &device)
{
	// fetch last value of all device modules
	vector<ModuleInfo> sensors;
	list<Control> controls;
	size_t count = 0;

	for (const auto &info : *device.type()) {
		if (info.isControllable())
			controls.emplace_back(Control{info.id()});
		else
			sensors.emplace_back(info);

		count += 1;
	}

	// values of all modules
	vector<ValueAt> values(count);

	// a null result means that there is no data for that sensor yet
	vector<Nullable<ValueAt>> nullableValues;
	m_historyDao->fetchMany(device, sensors, nullableValues);

	size_t i = 0;
	for (const auto &sensor : sensors) {
		const auto &current = nullableValues.at(i++);
		const unsigned int index = sensor.id();

		if (!current.isNull())
			values[index] = current;

		// else leave the value as invalid
	}

	m_controlDao->fetchBy(controls, device);

	for (const auto &control : controls) {
		const unsigned int index = control.id();

		const auto &confirmed = control.lastConfirmed();
		if (!confirmed.isNull()) {
			const Control::State &state = confirmed;
			values[index] = {state.at().value(), state.value()};
		}
		// else leave the value as invalid
	}

	device.setValues(values);
}

void DeviceServiceImpl::doFetchMany(Single<list<Device>> &input)
{
	m_policy->assureMany(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.target());

	list<Device> &devices = input.target();
	m_dao->fetchMany(devices);
}

void DeviceServiceImpl::doFetchMany(Single<list<DeviceWithData>> &input)
{
	list<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_dao->fetchMany(devices);

	// convert to list of DeviceWithData
	list<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, device.gateway());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchMany(Relation<list<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET, input, input.base());

	list<Device> &devices = input.target();

	m_dao->fetchMany(devices);

	list<Device>::iterator it = devices.begin();

	while (it != devices.end()) {
		Device &device = *it;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			// drop inaccessible devices
			it = devices.erase(it);
			continue;
		}

		++it; // no erase occured, continue
	}
}

/**
 * The method performs 1 + 2 * N Dao requests where N is the number of devices.
 * The first query obtains list of Device instances. Because we need a list
 * of DeviceWithData instances, the loop would convert it. The conversion
 * fetches module data for every single device.
 *
 * The method should be optimized (moved to Dao layer) if needed.
 */
void DeviceServiceImpl::doFetchMany(Relation<list<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	list<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assureMany(DeviceAccessPolicy::ACTION_USER_GET, input, devices);

	m_dao->fetchMany(devices);

	// convert to list of DeviceWithData
	list<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchActiveBy(Relation<vector<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchActiveBy(input.target(), input.base());
}

void DeviceServiceImpl::doFetchActiveBy(Relation<vector<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	vector<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchActiveBy(devices, input.base());

	// convert to list of DeviceWithData
	vector<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchInactiveBy(Relation<vector<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchInactiveBy(input.target(), input.base());
}

void DeviceServiceImpl::doFetchInactiveBy(Relation<vector<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	vector<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchInactiveBy(devices, input.base());

	// convert to list of DeviceWithData
	vector<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;

}

Work DeviceServiceImpl::doUnregister(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UNREGISTER,
			input, input.target(), input.base());

	Device &device = input.target();

	if (!m_dao->fetch(device, input.base()))
		throw NotFoundException("no such device " + device);

	Work work(WorkID::random());
	DeviceUnpairWork content;
	content.setGatewayID(device.gateway().id());
	content.setDeviceID(device.id());
	work.setContent(content);

	m_workFacade->schedule(work, input);
	return work;
}

bool DeviceServiceImpl::doActivate(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_ACTIVATE,
			input, input.target(), input.base());

	Device &device = input.target();

	if (!m_dao->fetch(device, input.base()))
		return false;

	return tryActivateAndUpdate(device, input.base());
}

bool DeviceServiceImpl::tryActivateAndUpdate(Device &device,
		const Gateway &gateway, bool forceUpdate)
{
	const DeviceStatus &status = device.status();

	if (!status.active()) {
		Device copy(device);

		m_gatewayRPC->pairDevice([copy, this](GatewayRPCResult::Ptr r) {
			switch (r->status()) {
			case GatewayRPCResult::Status::PENDING:
				Loggable::forClass(typeid(DeviceServiceImpl)).information(
					"device " + device + " pairing is pending...",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::ACCEPTED:
				Loggable::forClass(typeid(DeviceServiceImpl)).debug(
					"device " + device + " would be paired",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::SUCCESS:
				Loggable::forClass(typeid(DeviceServiceImpl)).information(
					"device " + device + " successfully paired",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::FAILED:
			case GatewayRPCResult::Status::TIMEOUT:
			case GatewayRPCResult::Status::NOT_CONNECTED:
				Loggable::forClass(typeid(DeviceServiceImpl)).warning(
					"device " + device + " failed to pair",
					__FILE__, __LINE__);
				break;
			}
		}, gateway, device);

		device.status().setState(DeviceStatus::STATE_ACTIVE);
		device.status().setLastChanged(Timestamp());
		return m_dao->update(device, gateway);
	}

	return forceUpdate? m_dao->update(device, gateway) : false;
}

bool DeviceServiceImpl::prepareUpdate(RelationWithData<Device, Gateway> &input)
{
	if (!m_dao->fetch(input.target(), input.base()))
		return false;

	input.data().partial(input.target());
	return true;
}

bool DeviceServiceImpl::doUpdate(RelationWithData<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.target(), input.base());

	if (!prepareUpdate(input))
		return false;

	return m_dao->update(input.target(), input.base());
}

bool DeviceServiceImpl::doUpdateAndActivate(
		RelationWithData<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE_AND_ACTIVATE,
			input, input.target(), input.base());

	if (!prepareUpdate(input))
		return false;

	return tryActivateAndUpdate(input.target(), input.base(), true);
}

bool DeviceServiceImpl::doCreateProperty(RelationWithData<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	input.data().full(input.target());

	return m_propertyDao->insert(input.target(), input.base());
}

bool DeviceServiceImpl::doUpdateProperty(RelationWithData<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	if (!m_propertyDao->fetch(input.target(), input.base()))
		return false;

	input.data().partial(input.target());

	return m_propertyDao->update(input.target(), input.base());
}

bool DeviceServiceImpl::doRemoveProperty(Relation<const DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	return m_propertyDao->remove(input.target(), input.base());
}

bool DeviceServiceImpl::doFindProperty(Relation<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base(), gateway);

	return m_propertyDao->fetch(input.target(), input.base());
}

void DeviceServiceImpl::doListProperties(Relation<list<DeviceProperty>, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base(), gateway);

	return m_propertyDao->fetchByDevice(input.target(), input.base());
}
<commit_msg>DeviceServiceImpl: differentiate failure and timeout during pair<commit_after>#include <Poco/Exception.h>
#include <Poco/Logger.h>
#include <Poco/Nullable.h>
#include <Poco/Timestamp.h>

#include "di/Injectable.h"
#include "model/Device.h"
#include "model/Gateway.h"
#include "service/DeviceServiceImpl.h"
#include "work/DeviceUnpairWork.h"
#include "work/WorkFacade.h"

BEEEON_OBJECT_BEGIN(BeeeOn, DeviceServiceImpl)
BEEEON_OBJECT_CASTABLE(DeviceService)
BEEEON_OBJECT_REF("deviceDao", &DeviceServiceImpl::setDeviceDao)
BEEEON_OBJECT_REF("controlDao", &DeviceServiceImpl::setControlDao)
BEEEON_OBJECT_REF("sensorHistoryDao", &DeviceServiceImpl::setSensorHistoryDao)
BEEEON_OBJECT_REF("devicePropertyDao", &DeviceServiceImpl::setDevicePropertyDao)
BEEEON_OBJECT_REF("gatewayRPC", &DeviceServiceImpl::setGatewayRPC)
BEEEON_OBJECT_REF("accessPolicy", &DeviceServiceImpl::setAccessPolicy)
BEEEON_OBJECT_REF("workFacade", &DeviceServiceImpl::setWorkFacade)
BEEEON_OBJECT_REF("transactionManager", &DeviceServiceImpl::setTransactionManager)
BEEEON_OBJECT_END(BeeeOn, DeviceServiceImpl)

using namespace std;
using namespace Poco;
using namespace BeeeOn;

DeviceServiceImpl::DeviceServiceImpl()
{
}

void DeviceServiceImpl::setDeviceDao(DeviceDao::Ptr dao)
{
	m_dao = dao;
}

void DeviceServiceImpl::setControlDao(ControlDao::Ptr dao)
{
	m_controlDao = dao;
}

void DeviceServiceImpl::setSensorHistoryDao(SensorHistoryDao::Ptr dao)
{
	m_historyDao = dao;
}

void DeviceServiceImpl::setDevicePropertyDao(DevicePropertyDao::Ptr dao)
{
	m_propertyDao = dao;
}

void DeviceServiceImpl::setGatewayRPC(GatewayRPC::Ptr rpc)
{
	m_gatewayRPC = rpc;
}

void DeviceServiceImpl::setWorkFacade(WorkFacade::Ptr facade)
{
	m_workFacade = facade;
}

void DeviceServiceImpl::setAccessPolicy(DeviceAccessPolicy::Ptr policy)
{
	m_policy = policy;
}

bool DeviceServiceImpl::doFetch(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.target(), input.base());
	return m_dao->fetch(input.target(), input.base());
}

void DeviceServiceImpl::valuesFor(DeviceWithData &device)
{
	// fetch last value of all device modules
	vector<ModuleInfo> sensors;
	list<Control> controls;
	size_t count = 0;

	for (const auto &info : *device.type()) {
		if (info.isControllable())
			controls.emplace_back(Control{info.id()});
		else
			sensors.emplace_back(info);

		count += 1;
	}

	// values of all modules
	vector<ValueAt> values(count);

	// a null result means that there is no data for that sensor yet
	vector<Nullable<ValueAt>> nullableValues;
	m_historyDao->fetchMany(device, sensors, nullableValues);

	size_t i = 0;
	for (const auto &sensor : sensors) {
		const auto &current = nullableValues.at(i++);
		const unsigned int index = sensor.id();

		if (!current.isNull())
			values[index] = current;

		// else leave the value as invalid
	}

	m_controlDao->fetchBy(controls, device);

	for (const auto &control : controls) {
		const unsigned int index = control.id();

		const auto &confirmed = control.lastConfirmed();
		if (!confirmed.isNull()) {
			const Control::State &state = confirmed;
			values[index] = {state.at().value(), state.value()};
		}
		// else leave the value as invalid
	}

	device.setValues(values);
}

void DeviceServiceImpl::doFetchMany(Single<list<Device>> &input)
{
	m_policy->assureMany(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.target());

	list<Device> &devices = input.target();
	m_dao->fetchMany(devices);
}

void DeviceServiceImpl::doFetchMany(Single<list<DeviceWithData>> &input)
{
	list<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_dao->fetchMany(devices);

	// convert to list of DeviceWithData
	list<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, device.gateway());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchMany(Relation<list<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET, input, input.base());

	list<Device> &devices = input.target();

	m_dao->fetchMany(devices);

	list<Device>::iterator it = devices.begin();

	while (it != devices.end()) {
		Device &device = *it;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			// drop inaccessible devices
			it = devices.erase(it);
			continue;
		}

		++it; // no erase occured, continue
	}
}

/**
 * The method performs 1 + 2 * N Dao requests where N is the number of devices.
 * The first query obtains list of Device instances. Because we need a list
 * of DeviceWithData instances, the loop would convert it. The conversion
 * fetches module data for every single device.
 *
 * The method should be optimized (moved to Dao layer) if needed.
 */
void DeviceServiceImpl::doFetchMany(Relation<list<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	list<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assureMany(DeviceAccessPolicy::ACTION_USER_GET, input, devices);

	m_dao->fetchMany(devices);

	// convert to list of DeviceWithData
	list<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchActiveBy(Relation<vector<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchActiveBy(input.target(), input.base());
}

void DeviceServiceImpl::doFetchActiveBy(Relation<vector<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	vector<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchActiveBy(devices, input.base());

	// convert to list of DeviceWithData
	vector<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;
}

void DeviceServiceImpl::doFetchInactiveBy(Relation<vector<Device>, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchInactiveBy(input.target(), input.base());
}

void DeviceServiceImpl::doFetchInactiveBy(Relation<vector<DeviceWithData>, Gateway> &input)
{
	// fetch list of Devices
	vector<Device> devices;
	for (const auto &dev : input.target())
		devices.emplace_back(dev);

	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base());
	m_dao->fetchInactiveBy(devices, input.base());

	// convert to list of DeviceWithData
	vector<DeviceWithData> result;
	for (const auto &dev : devices) {
		DeviceWithData device = dev;

		try {
			m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
					input, device, input.base());
		} catch (const InvalidAccessException &e) {
			continue;
		}

		valuesFor(device);
		result.emplace_back(device);
	}

	input.target() = result;

}

Work DeviceServiceImpl::doUnregister(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UNREGISTER,
			input, input.target(), input.base());

	Device &device = input.target();

	if (!m_dao->fetch(device, input.base()))
		throw NotFoundException("no such device " + device);

	Work work(WorkID::random());
	DeviceUnpairWork content;
	content.setGatewayID(device.gateway().id());
	content.setDeviceID(device.id());
	work.setContent(content);

	m_workFacade->schedule(work, input);
	return work;
}

bool DeviceServiceImpl::doActivate(Relation<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_ACTIVATE,
			input, input.target(), input.base());

	Device &device = input.target();

	if (!m_dao->fetch(device, input.base()))
		return false;

	return tryActivateAndUpdate(device, input.base());
}

bool DeviceServiceImpl::tryActivateAndUpdate(Device &device,
		const Gateway &gateway, bool forceUpdate)
{
	const DeviceStatus &status = device.status();

	if (!status.active()) {
		Device copy(device);

		m_gatewayRPC->pairDevice([copy, this](GatewayRPCResult::Ptr r) {
			switch (r->status()) {
			case GatewayRPCResult::Status::PENDING:
				Loggable::forClass(typeid(DeviceServiceImpl)).information(
					"device " + device + " pairing is pending...",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::ACCEPTED:
				Loggable::forClass(typeid(DeviceServiceImpl)).debug(
					"device " + device + " would be paired",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::SUCCESS:
				Loggable::forClass(typeid(DeviceServiceImpl)).information(
					"device " + device + " successfully paired",
					__FILE__, __LINE__);
				break;

			case GatewayRPCResult::Status::FAILED:
				Loggable::forClass(typeid(DeviceServiceImpl)).warning(
					"device " + device + " failed to pair",
					__FILE__, __LINE__);
				break;
			case GatewayRPCResult::Status::TIMEOUT:
			case GatewayRPCResult::Status::NOT_CONNECTED:
				Loggable::forClass(typeid(DeviceServiceImpl)).warning(
					"device " + device + " failed to pair on time",
					__FILE__, __LINE__);
				break;
			}
		}, gateway, device);

		device.status().setState(DeviceStatus::STATE_ACTIVE);
		device.status().setLastChanged(Timestamp());
		return m_dao->update(device, gateway);
	}

	return forceUpdate? m_dao->update(device, gateway) : false;
}

bool DeviceServiceImpl::prepareUpdate(RelationWithData<Device, Gateway> &input)
{
	if (!m_dao->fetch(input.target(), input.base()))
		return false;

	input.data().partial(input.target());
	return true;
}

bool DeviceServiceImpl::doUpdate(RelationWithData<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.target(), input.base());

	if (!prepareUpdate(input))
		return false;

	return m_dao->update(input.target(), input.base());
}

bool DeviceServiceImpl::doUpdateAndActivate(
		RelationWithData<Device, Gateway> &input)
{
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE_AND_ACTIVATE,
			input, input.target(), input.base());

	if (!prepareUpdate(input))
		return false;

	return tryActivateAndUpdate(input.target(), input.base(), true);
}

bool DeviceServiceImpl::doCreateProperty(RelationWithData<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	input.data().full(input.target());

	return m_propertyDao->insert(input.target(), input.base());
}

bool DeviceServiceImpl::doUpdateProperty(RelationWithData<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	if (!m_propertyDao->fetch(input.target(), input.base()))
		return false;

	input.data().partial(input.target());

	return m_propertyDao->update(input.target(), input.base());
}

bool DeviceServiceImpl::doRemoveProperty(Relation<const DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_UPDATE,
			input, input.base(), gateway);

	return m_propertyDao->remove(input.target(), input.base());
}

bool DeviceServiceImpl::doFindProperty(Relation<DeviceProperty, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base(), gateway);

	return m_propertyDao->fetch(input.target(), input.base());
}

void DeviceServiceImpl::doListProperties(Relation<list<DeviceProperty>, Device> &input)
{
	const Gateway &gateway = input.base().gateway();
	m_policy->assure(DeviceAccessPolicy::ACTION_USER_GET,
			input, input.base(), gateway);

	return m_propertyDao->fetchByDevice(input.target(), input.base());
}
<|endoftext|>


<commit_before>#include <Sound.h>
#include <Display.h>
#include <SDL.h>
#include <SDL_mixer.h>

#include <hx/Thread.h>

namespace nme
{

bool gSDLIsInit = false;

class SDLSoundChannel;

bool sChannelsInit = false;
enum { sMaxChannels = 8 };

bool sUsedChannel[sMaxChannels];
bool sDoneChannel[sMaxChannels];
void *sUsedMusic = 0;
bool sDoneMusic = false;
enum { STEREO_SAMPLES = 2 };

unsigned int  sSoundPos = 0;

void onChannelDone(int inChannel)
{
   if (sUsedChannel[inChannel])
      sDoneChannel[inChannel] = true;
}

void onMusicDone()
{
   if (sUsedMusic)
      sDoneMusic = true;
}

void  onPostMix(void *udata, Uint8 *stream, int len)
{
   sSoundPos += len / sizeof(short) / STEREO_SAMPLES ;
}

static bool Init()
{
   if (!gSDLIsInit)
   {
      ELOG("Please init Stage before creating sound.");
      return false;
   }

   if (!sChannelsInit)
   {
      sChannelsInit = true;
      for(int i=0;i<sMaxChannels;i++)
      {
         sUsedChannel[i] = false;
         sDoneChannel[i] = false;
      }
      Mix_ChannelFinished(onChannelDone);
      Mix_HookMusicFinished(onMusicDone);
      Mix_SetPostMix(onPostMix,0);
   }

   return sChannelsInit;
}

// ---  Using "Mix_Chunk" API ----------------------------------------------------

class SDLSoundChannel : public SoundChannel
{
  enum { BUF_SIZE = (1<<17) };

public:
   SDLSoundChannel(Object *inSound, Mix_Chunk *inChunk, double inStartTime, int inLoops,
                  const SoundTransform &inTransform)
   {
      mChunk = inChunk;
      mDynamicBuffer = 0;
      mSound = inSound;
      mSound->IncRef();

      mChannel = -1;

      // Allocate myself a channel
      if (mChunk)
      {
         for(int i=0;i<sMaxChannels;i++)
            if (!sUsedChannel[i])
            {
               IncRef();
               sDoneChannel[i] = false;
               sUsedChannel[i] = true;
               mChannel = i;
               break;
            }
      }

      if (mChannel>=0)
      {
         Mix_PlayChannel( mChannel , mChunk, inLoops<0 ? -1 : inLoops==0 ? 0 : inLoops-1 );
         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
         // Mix_SetPanning
      }
   }

   SDLSoundChannel(const ByteArray &inBytes, const SoundTransform &inTransform)
   {
      Mix_QuerySpec(&mFrequency, &mFormat, &mChannels);
      if (mFrequency!=44100)
         ELOG("Warning - Frequency mismatch %d",mFrequency);
      if (mFormat!=32784)
         ELOG("Warning - Format mismatch    %d",mFormat);
      if (mChannels!=2)
         ELOG("Warning - channe mismatch    %d",mChannels);

      mChunk = 0;
      mDynamicBuffer = new short[BUF_SIZE * STEREO_SAMPLES];
      memset(mDynamicBuffer,0,BUF_SIZE*sizeof(short));
      mSound = 0;
      mChannel = -1;
      mDynamicChunk.allocated = 0;
      mDynamicChunk.abuf = (Uint8 *)mDynamicBuffer;
      mDynamicChunk.alen = BUF_SIZE * sizeof(short) * STEREO_SAMPLES; // bytes
      mDynamicChunk.volume = MIX_MAX_VOLUME;
      mDynamicFillPos = 0;
      mDynamicStartPos = 0;
      mDynamicDataDue = 0;

      // Allocate myself a channel
      for(int i=0;i<sMaxChannels;i++)
         if (!sUsedChannel[i])
         {
            IncRef();
            sDoneChannel[i] = false;
            sUsedChannel[i] = true;
            mChannel = i;
            break;
         }

      if (mChannel>=0)
      {
         FillBuffer(inBytes);
         // Just once ...
         if (mDynamicFillPos<1024)
         {
            mDynamicDone = true;
            mDynamicChunk.alen = mDynamicFillPos * sizeof(short) * STEREO_SAMPLES;
            Mix_PlayChannel( mChannel , &mDynamicChunk,  0 );
         }
         else
         {
            mDynamicDone = false;
            // TODO: Lock?
            Mix_PlayChannel( mChannel , &mDynamicChunk,  -1 );
            mDynamicStartPos = sSoundPos;
         }

         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
      }
   }

   void FillBuffer(const ByteArray &inBytes)
   {
      int time_samples = inBytes.Size()/sizeof(float)/STEREO_SAMPLES;
      const float *buffer = (const float *)inBytes.Bytes();
      enum { MASK = BUF_SIZE - 1 };

      for(int i=0;i<time_samples;i++)
      {
         int mono_pos =  (i+mDynamicFillPos) & MASK;
         mDynamicBuffer[ mono_pos<<1 ] = *buffer++ * ((1<<15)-1);
         mDynamicBuffer[ (mono_pos<<1) + 1 ] = *buffer++ * ((1<<15)-1);
      }

      if (mDynamicFillPos<(sSoundPos-mDynamicStartPos))
         ELOG("Too slow - FillBuffer %d / %d)", mDynamicFillPos, (sSoundPos-mDynamicStartPos) );
      mDynamicFillPos += time_samples;
      if (time_samples<1024 && !mDynamicDone)
      {
         mDynamicDone = true;
         for(int i=0;i<2048;i++)
         {
            int mono_pos =  (i+mDynamicFillPos) & MASK;
            mDynamicBuffer[ mono_pos<<1 ] = 0;
            mDynamicBuffer[ (mono_pos<<1) + 1 ] = 0;
         }

         int samples_left = (int)mDynamicFillPos - (int)(sSoundPos-mDynamicStartPos);
         int ticks_left = samples_left*1000/44100;
         //printf("Expire in %d (%d)\n", samples_left, ticks_left );
         Mix_ExpireChannel(mChannel, ticks_left>0 ? ticks_left : 1 );
      }
   }

   ~SDLSoundChannel()
   {
      delete [] mDynamicBuffer;

      if (mSound)
         mSound->DecRef();
   }

   void CheckDone()
   {
      if (mChannel>=0 && sDoneChannel[mChannel])
      {
         sDoneChannel[mChannel] = false;
         int c = mChannel;
         mChannel = -1;
         DecRef();
         sUsedChannel[c] = 0;
      }
   }

   bool isComplete()
   {
      CheckDone();
      return mChannel < 0;
   }
   double getLeft() { return 1; }
   double getRight() { return 1; }
   double getPosition() { return 1; }
   void stop()
   {
      if (mChannel>=0)
         Mix_HaltChannel(mChannel);
   }
   void setTransform(const SoundTransform &inTransform)
   {
      if (mChannel>=0)
         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
   }

   double getDataPosition()
   {
      return (sSoundPos-mDynamicStartPos)*1000.0/mFrequency;
   }
   bool needsData()
   {
      if (!mDynamicBuffer || mDynamicDone)
         return false;

      if (mDynamicDataDue<=sSoundPos)
      {
         mDynamicDone = true;
         return true;
      }

      return false;

   }

   void addData(const ByteArray &inBytes)
   {
      mDynamicDone = false;
      mDynamicDataDue = mDynamicFillPos + mDynamicStartPos;
      FillBuffer(inBytes);
   }

   Object    *mSound;
   Mix_Chunk *mChunk;
   int       mChannel;

   Mix_Chunk mDynamicChunk;
   short    *mDynamicBuffer;
   unsigned int  mDynamicFillPos;
   unsigned int  mDynamicStartPos;
   unsigned int  mDynamicDataDue;
   bool      mDynamicDone;
   int       mFrequency;
   Uint16    mFormat;
   int       mChannels;
};

SoundChannel *SoundChannel::Create(const ByteArray &inBytes,const SoundTransform &inTransform)
{
   if (!Init())
      return 0;
   return new SDLSoundChannel(inBytes,inTransform);
}

class SDLSound : public Sound
{
public:
   SDLSound(const std::string &inFilename)
   {
      IncRef();

      #ifdef HX_MACOS
      char name[1024];
      GetBundleFilename(inFilename.c_str(),name,1024);
      #else
      const char *name = inFilename.c_str();
      #endif

      mChunk = Mix_LoadWAV(name);
      if ( mChunk == NULL )
      {
         mError = SDL_GetError();
         // ELOG("Error %s (%s)", mError.c_str(), name );
      }
   }

   SDLSound(unsigned char *inData, int len)
   {
      IncRef();

      mChunk = Mix_LoadWAV_RW(SDL_RWFromMem(inData, len), 1);
      if ( mChunk == NULL )
      {
         mError = SDL_GetError();
         // ELOG("Error %s (%s)", mError.c_str(), name );
      }
   }

   ~SDLSound()
   {
      if (mChunk)
         Mix_FreeChunk( mChunk );
   }
   double getLength()
   {
      if (mChunk==0) return 0;
      #if defined(DYNAMIC_SDL) || defined(WEBOS)
      // ?
      return 0.0;
      #else
	  return 0.0;
      #endif
   }
   // Will return with one ref...
   SoundChannel *openChannel(double startTime, int loops, const SoundTransform &inTransform)
   {
      if (!mChunk)
         return 0;
      return new SDLSoundChannel(this,mChunk,startTime, loops,inTransform);
   }
   int getBytesLoaded() { return mChunk ? mChunk->alen : 0; }
   int getBytesTotal() { return mChunk ? mChunk->alen : 0; }
   bool ok() { return mChunk; }
   std::string getError() { return mError; }

   std::string mError;
   Mix_Chunk *mChunk;
};

// ---  Using "Mix_Music" API ----------------------------------------------------

class SDLMusicChannel : public SoundChannel
{
public:
   SDLMusicChannel(Object *inSound, Mix_Music *inMusic, double inStartTime, int inLoops,
                  const SoundTransform &inTransform)
   {
      mMusic = inMusic;
      mSound = inSound;
      mSound->IncRef();

      mPlaying = false;
      if (mMusic)
      {
         mPlaying = true;
         sUsedMusic = this;
         sDoneMusic = false;
         IncRef();
         Mix_PlayMusic( mMusic, inLoops<0 ? -1 : inLoops==0 ? 0 : inLoops-1 );
         Mix_VolumeMusic( inTransform.volume*MIX_MAX_VOLUME );
         // Mix_SetPanning
      }
   }
   ~SDLMusicChannel()
   {
      mSound->DecRef();
   }

   void CheckDone()
   {
      if (mPlaying && (sDoneMusic || (sUsedMusic!=this)) )
      {
         mPlaying = false;
         if (sUsedMusic == this)
         {
            sUsedMusic = 0;
            sDoneMusic = false;
         }
         DecRef();
      }
   }

   bool isComplete()
   {
      CheckDone();
      return !mPlaying;
   }
   double getLeft() { return 1; }
   double getRight() { return 1; }
   double getPosition() { return 1; }
   void stop()
   {
      if (mMusic)
         Mix_HaltMusic();
   }
   void setTransform(const SoundTransform &inTransform)
   {
      if (mMusic>=0)
         Mix_VolumeMusic( inTransform.volume*MIX_MAX_VOLUME );
   }

   bool      mPlaying;
   Object    *mSound;
   Mix_Music *mMusic;
};

class SDLMusic : public Sound
{
public:
   SDLMusic(const std::string &inFilename)
   {
      IncRef();

      #ifdef HX_MACOS
      char name[1024];
      GetBundleFilename(inFilename.c_str(),name,1024);
      #else
      const char *name = inFilename.c_str();
      #endif

      mMusic = Mix_LoadMUS(name);
      if ( mMusic == NULL )
      {
         mError = SDL_GetError();
         ELOG("Error in music %s (%s)", mError.c_str(), name );
      }
   }

   SDLMusic(unsigned char *inData, int len)
   {
      IncRef();

      mMusic = Mix_LoadMUS_RW(SDL_RWFromMem(inData, len));
      if ( mMusic == NULL )
      {
         mError = SDL_GetError();
         ELOG("Error in music with len (%d)", len );
      }
   }
   ~SDLMusic()
   {
      if (mMusic)
         Mix_FreeMusic( mMusic );
   }
   double getLength()
   {
      if (mMusic==0) return 0;
      // TODO:
      return 60000;
   }
   // Will return with one ref...
   SoundChannel *openChannel(double startTime, int loops, const SoundTransform &inTransform)
   {
      if (!mMusic)
         return 0;
      return new SDLMusicChannel(this,mMusic,startTime, loops,inTransform);
   }
   int getBytesLoaded() { return mMusic ? 100 : 0; }
   int getBytesTotal() { return mMusic ? 100 : 0; }
   bool ok() { return mMusic; }
   std::string getError() { return mError; }

   std::string mError;
   Mix_Music *mMusic;
};

// --- External Interface -----------------------------------------------------------

Sound *Sound::Create(const std::string &inFilename,bool inForceMusic)
{
   if (!Init())
      return 0;
   Sound *sound = inForceMusic ? 0 :  new SDLSound(inFilename);
   if (!sound || !sound->ok())
   {
      if (sound) sound->DecRef();
      sound = new SDLMusic(inFilename);
   }
   return sound;
}

Sound *Sound::Create(unsigned char *inData, int len, bool inForceMusic) {
   if (!Init())
      return 0;
   Sound *sound = inForceMusic ? 0 :  new SDLSound(inData, len);
   if (!sound || !sound->ok())
   {
      if (sound) sound->DecRef();
      sound = new SDLMusic(inData, len);
   }
   return sound;
}

}
<commit_msg>Added proper position to SDL music channels, but cannot set the position because it causes crash errors in SDL, currently<commit_after>#include <Sound.h>
#include <Display.h>
#include <SDL.h>
#include <SDL_mixer.h>

#include <hx/Thread.h>

namespace nme
{

bool gSDLIsInit = false;

class SDLSoundChannel;

bool sChannelsInit = false;
enum { sMaxChannels = 8 };

bool sUsedChannel[sMaxChannels];
bool sDoneChannel[sMaxChannels];
void *sUsedMusic = 0;
bool sDoneMusic = false;
enum { STEREO_SAMPLES = 2 };

unsigned int  sSoundPos = 0;

void onChannelDone(int inChannel)
{
   if (sUsedChannel[inChannel])
      sDoneChannel[inChannel] = true;
}

void onMusicDone()
{
   if (sUsedMusic)
      sDoneMusic = true;
}

void  onPostMix(void *udata, Uint8 *stream, int len)
{
   sSoundPos += len / sizeof(short) / STEREO_SAMPLES ;
}

static bool Init()
{
   if (!gSDLIsInit)
   {
      ELOG("Please init Stage before creating sound.");
      return false;
   }

   if (!sChannelsInit)
   {
      sChannelsInit = true;
      for(int i=0;i<sMaxChannels;i++)
      {
         sUsedChannel[i] = false;
         sDoneChannel[i] = false;
      }
      Mix_ChannelFinished(onChannelDone);
      Mix_HookMusicFinished(onMusicDone);
      Mix_SetPostMix(onPostMix,0);
   }

   return sChannelsInit;
}

// ---  Using "Mix_Chunk" API ----------------------------------------------------

class SDLSoundChannel : public SoundChannel
{
  enum { BUF_SIZE = (1<<17) };

public:
   SDLSoundChannel(Object *inSound, Mix_Chunk *inChunk, double inStartTime, int inLoops,
                  const SoundTransform &inTransform)
   {
      mChunk = inChunk;
      mDynamicBuffer = 0;
      mSound = inSound;
      mSound->IncRef();

      mChannel = -1;

      // Allocate myself a channel
      if (mChunk)
      {
         for(int i=0;i<sMaxChannels;i++)
            if (!sUsedChannel[i])
            {
               IncRef();
               sDoneChannel[i] = false;
               sUsedChannel[i] = true;
               mChannel = i;
               break;
            }
      }

      if (mChannel>=0)
      {
         Mix_PlayChannel( mChannel , mChunk, inLoops<0 ? -1 : inLoops==0 ? 0 : inLoops-1 );
         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
         // Mix_SetPanning
      }
   }

   SDLSoundChannel(const ByteArray &inBytes, const SoundTransform &inTransform)
   {
      Mix_QuerySpec(&mFrequency, &mFormat, &mChannels);
      if (mFrequency!=44100)
         ELOG("Warning - Frequency mismatch %d",mFrequency);
      if (mFormat!=32784)
         ELOG("Warning - Format mismatch    %d",mFormat);
      if (mChannels!=2)
         ELOG("Warning - channe mismatch    %d",mChannels);

      mChunk = 0;
      mDynamicBuffer = new short[BUF_SIZE * STEREO_SAMPLES];
      memset(mDynamicBuffer,0,BUF_SIZE*sizeof(short));
      mSound = 0;
      mChannel = -1;
      mDynamicChunk.allocated = 0;
      mDynamicChunk.abuf = (Uint8 *)mDynamicBuffer;
      mDynamicChunk.alen = BUF_SIZE * sizeof(short) * STEREO_SAMPLES; // bytes
      mDynamicChunk.volume = MIX_MAX_VOLUME;
      mDynamicFillPos = 0;
      mDynamicStartPos = 0;
      mDynamicDataDue = 0;

      // Allocate myself a channel
      for(int i=0;i<sMaxChannels;i++)
         if (!sUsedChannel[i])
         {
            IncRef();
            sDoneChannel[i] = false;
            sUsedChannel[i] = true;
            mChannel = i;
            break;
         }

      if (mChannel>=0)
      {
         FillBuffer(inBytes);
         // Just once ...
         if (mDynamicFillPos<1024)
         {
            mDynamicDone = true;
            mDynamicChunk.alen = mDynamicFillPos * sizeof(short) * STEREO_SAMPLES;
            Mix_PlayChannel( mChannel , &mDynamicChunk,  0 );
         }
         else
         {
            mDynamicDone = false;
            // TODO: Lock?
            Mix_PlayChannel( mChannel , &mDynamicChunk,  -1 );
            mDynamicStartPos = sSoundPos;
         }

         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
      }
   }

   void FillBuffer(const ByteArray &inBytes)
   {
      int time_samples = inBytes.Size()/sizeof(float)/STEREO_SAMPLES;
      const float *buffer = (const float *)inBytes.Bytes();
      enum { MASK = BUF_SIZE - 1 };

      for(int i=0;i<time_samples;i++)
      {
         int mono_pos =  (i+mDynamicFillPos) & MASK;
         mDynamicBuffer[ mono_pos<<1 ] = *buffer++ * ((1<<15)-1);
         mDynamicBuffer[ (mono_pos<<1) + 1 ] = *buffer++ * ((1<<15)-1);
      }

      if (mDynamicFillPos<(sSoundPos-mDynamicStartPos))
         ELOG("Too slow - FillBuffer %d / %d)", mDynamicFillPos, (sSoundPos-mDynamicStartPos) );
      mDynamicFillPos += time_samples;
      if (time_samples<1024 && !mDynamicDone)
      {
         mDynamicDone = true;
         for(int i=0;i<2048;i++)
         {
            int mono_pos =  (i+mDynamicFillPos) & MASK;
            mDynamicBuffer[ mono_pos<<1 ] = 0;
            mDynamicBuffer[ (mono_pos<<1) + 1 ] = 0;
         }

         int samples_left = (int)mDynamicFillPos - (int)(sSoundPos-mDynamicStartPos);
         int ticks_left = samples_left*1000/44100;
         //printf("Expire in %d (%d)\n", samples_left, ticks_left );
         Mix_ExpireChannel(mChannel, ticks_left>0 ? ticks_left : 1 );
      }
   }

   ~SDLSoundChannel()
   {
      delete [] mDynamicBuffer;

      if (mSound)
         mSound->DecRef();
   }

   void CheckDone()
   {
      if (mChannel>=0 && sDoneChannel[mChannel])
      {
         sDoneChannel[mChannel] = false;
         int c = mChannel;
         mChannel = -1;
         DecRef();
         sUsedChannel[c] = 0;
      }
   }

   bool isComplete()
   {
      CheckDone();
      return mChannel < 0;
   }
   double getLeft() { return 1; }
   double getRight() { return 1; }
   double getPosition() { return 1; }
   void stop()
   {
      if (mChannel>=0)
         Mix_HaltChannel(mChannel);
   }
   void setTransform(const SoundTransform &inTransform)
   {
      if (mChannel>=0)
         Mix_Volume( mChannel, inTransform.volume*MIX_MAX_VOLUME );
   }

   double getDataPosition()
   {
      return (sSoundPos-mDynamicStartPos)*1000.0/mFrequency;
   }
   bool needsData()
   {
      if (!mDynamicBuffer || mDynamicDone)
         return false;

      if (mDynamicDataDue<=sSoundPos)
      {
         mDynamicDone = true;
         return true;
      }

      return false;

   }

   void addData(const ByteArray &inBytes)
   {
      mDynamicDone = false;
      mDynamicDataDue = mDynamicFillPos + mDynamicStartPos;
      FillBuffer(inBytes);
   }

   Object    *mSound;
   Mix_Chunk *mChunk;
   int       mChannel;

   Mix_Chunk mDynamicChunk;
   short    *mDynamicBuffer;
   unsigned int  mDynamicFillPos;
   unsigned int  mDynamicStartPos;
   unsigned int  mDynamicDataDue;
   bool      mDynamicDone;
   int       mFrequency;
   Uint16    mFormat;
   int       mChannels;
};

SoundChannel *SoundChannel::Create(const ByteArray &inBytes,const SoundTransform &inTransform)
{
   if (!Init())
      return 0;
   return new SDLSoundChannel(inBytes,inTransform);
}

class SDLSound : public Sound
{
public:
   SDLSound(const std::string &inFilename)
   {
      IncRef();

      #ifdef HX_MACOS
      char name[1024];
      GetBundleFilename(inFilename.c_str(),name,1024);
      #else
      const char *name = inFilename.c_str();
      #endif

      mChunk = Mix_LoadWAV(name);
      if ( mChunk == NULL )
      {
         mError = SDL_GetError();
         // ELOG("Error %s (%s)", mError.c_str(), name );
      }
   }

   SDLSound(unsigned char *inData, int len)
   {
      IncRef();

      mChunk = Mix_LoadWAV_RW(SDL_RWFromMem(inData, len), 1);
      if ( mChunk == NULL )
      {
         mError = SDL_GetError();
         // ELOG("Error %s (%s)", mError.c_str(), name );
      }
   }

   ~SDLSound()
   {
      if (mChunk)
         Mix_FreeChunk( mChunk );
   }
   double getLength()
   {
      if (mChunk==0) return 0;
      #if defined(DYNAMIC_SDL) || defined(WEBOS)
      // ?
      return 0.0;
      #else
	  return 0.0;
      #endif
   }
   // Will return with one ref...
   SoundChannel *openChannel(double startTime, int loops, const SoundTransform &inTransform)
   {
      if (!mChunk)
         return 0;
      return new SDLSoundChannel(this,mChunk,startTime, loops,inTransform);
   }
   int getBytesLoaded() { return mChunk ? mChunk->alen : 0; }
   int getBytesTotal() { return mChunk ? mChunk->alen : 0; }
   bool ok() { return mChunk; }
   std::string getError() { return mError; }

   std::string mError;
   Mix_Chunk *mChunk;
};

// ---  Using "Mix_Music" API ----------------------------------------------------

class SDLMusicChannel : public SoundChannel
{
public:
   SDLMusicChannel(Object *inSound, Mix_Music *inMusic, double inStartTime, int inLoops,
                  const SoundTransform &inTransform)
   {
      mMusic = inMusic;
      mSound = inSound;
      mSound->IncRef();

      mPlaying = false;
      if (mMusic)
      {
         mPlaying = true;
         sUsedMusic = this;
         sDoneMusic = false;
		 mStartTime = SDL_GetTicks ();
		 mLength = 0;
         IncRef();
         Mix_PlayMusic( mMusic, inLoops<0 ? -1 : inLoops==0 ? 0 : inLoops-1 );
         Mix_VolumeMusic( inTransform.volume*MIX_MAX_VOLUME );
         if (inStartTime > 0)
		 {
			 // this is causing crash errors

			 //Mix_RewindMusic();
			 //int seconds = inStartTime / 1000;
			 //Mix_SetMusicPosition(seconds);
			 //mStartTime = SDL_GetTicks () - inStartTime;
		 }
         // Mix_SetPanning not available for music
      }
   }
   ~SDLMusicChannel()
   {
      mSound->DecRef();
   }

   void CheckDone()
   {
      if (mPlaying && (sDoneMusic || (sUsedMusic!=this)) )
      {
		 mLength = SDL_GetTicks () - mStartTime;
         mPlaying = false;
         if (sUsedMusic == this)
         {
            sUsedMusic = 0;
            sDoneMusic = false;
         }
         DecRef();
      }
   }

   bool isComplete()
   {
      CheckDone();
      return !mPlaying;
   }
   double getLeft() { return 1; }
   double getRight() { return 1; }
   double getPosition() { return mPlaying ? SDL_GetTicks() - mStartTime : mLength; }
   void stop()
   {
      if (mMusic)
         Mix_HaltMusic();
   }
   void setTransform(const SoundTransform &inTransform)
   {
      if (mMusic>=0)
         Mix_VolumeMusic( inTransform.volume*MIX_MAX_VOLUME );
   }

   bool      mPlaying;
   Object    *mSound;
   int       mStartTime;
   int       mLength;
   Mix_Music *mMusic;
};

class SDLMusic : public Sound
{
public:
   SDLMusic(const std::string &inFilename)
   {
      IncRef();

      #ifdef HX_MACOS
      char name[1024];
      GetBundleFilename(inFilename.c_str(),name,1024);
      #else
      const char *name = inFilename.c_str();
      #endif

      mMusic = Mix_LoadMUS(name);
      if ( mMusic == NULL )
      {
         mError = SDL_GetError();
         ELOG("Error in music %s (%s)", mError.c_str(), name );
      }
   }

   SDLMusic(unsigned char *inData, int len)
   {
      IncRef();

      mMusic = Mix_LoadMUS_RW(SDL_RWFromMem(inData, len));
      if ( mMusic == NULL )
      {
         mError = SDL_GetError();
         ELOG("Error in music with len (%d)", len );
      }
   }
   ~SDLMusic()
   {
      if (mMusic)
         Mix_FreeMusic( mMusic );
   }
   double getLength()
   {
      if (mMusic==0) return 0;
      // TODO:
      return 60000;
   }
   // Will return with one ref...
   SoundChannel *openChannel(double startTime, int loops, const SoundTransform &inTransform)
   {
      if (!mMusic)
         return 0;
      return new SDLMusicChannel(this,mMusic,startTime, loops,inTransform);
   }
   int getBytesLoaded() { return mMusic ? 100 : 0; }
   int getBytesTotal() { return mMusic ? 100 : 0; }
   bool ok() { return mMusic; }
   std::string getError() { return mError; }

   std::string mError;
   Mix_Music *mMusic;
};

// --- External Interface -----------------------------------------------------------

Sound *Sound::Create(const std::string &inFilename,bool inForceMusic)
{
   if (!Init())
      return 0;
   Sound *sound = inForceMusic ? 0 :  new SDLSound(inFilename);
   if (!sound || !sound->ok())
   {
      if (sound) sound->DecRef();
      sound = new SDLMusic(inFilename);
   }
   return sound;
}

Sound *Sound::Create(unsigned char *inData, int len, bool inForceMusic) {
   if (!Init())
      return 0;
   Sound *sound = inForceMusic ? 0 :  new SDLSound(inData, len);
   if (!sound || !sound->ok())
   {
      if (sound) sound->DecRef();
      sound = new SDLMusic(inData, len);
   }
   return sound;
}

}
<|endoftext|>


<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "packet/frame.h"
#include "packet/serialize.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "angle.h"
#include "blink.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <algorithm>
#include <array>
#include <type_traits>

#include "bicycle/kinematic.h" /* simplified bicycle model */
#include "bicycle/whipple.h" /* whipple bicycle model */
#include "oracle.h" /* oracle observer */
#include "kalman.h" /* kalman filter observer */
#include "haptic.h" /* handlebar feedback */
#include "simbicycle.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using observer_t = observer::Oracle<model_t>;
    using haptic_t = haptic::HandlebarStatic;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
    using haptic_t = haptic::HandlebarDynamic;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t, haptic_t>;

    /* sensors */
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    constexpr float fixed_velocity = 7.0f;

    /* transmission */
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> encode_buffer;
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> packet_buffer;
    SimulationMessage msg;
    constexpr SimulationMessage msg_init = SimulationMessage_init_zero;

    /* kinematics loop */
    constexpr systime_t kinematics_loop_time = US2ST(8333); /* update kinematics at 120 Hz */

    /* dynamics loop */
    constexpr model::real_t dynamics_period = 0.005; /* 5 ms -> 200 Hz */
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;

    dacsample_t set_handlebar_torque(float handlebar_torque) {
        int32_t saturated_value = (handlebar_torque/sa::MAX_KOLLMORGEN_TORQUE * 2048) +
            sa::KOLLMORGEN_DAC_ZERO_OFFSET;
        saturated_value = std::min<int32_t>(std::max<int32_t>(saturated_value, 0), 4096);
        dacsample_t aout = static_cast<dacsample_t>(saturated_value);
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);
        return aout;
    }

    void update_and_transmit_kinematics(bicycle_t& bicycle) {
        bicycle.update_kinematics();
        /*
         * TODO: Pose message should be transmitted asynchronously.
         * After starting transmission, the simulation should start to calculate pose for the next timestep.
         * This is currently not necessary given the observed timings.
         */
        //size_t bytes_encoded = packet::serialize::encode_delimited(bicycle.pose(),
        //        encode_buffer.data(), encode_buffer.size());
        //TODO: clear message
        //message::set_simulation_loop_values(&msg, bicycle);
        //size_t bytes_encoded = packet::serialize::encode_delimited(
        //        msg, encode_buffer.data(), encode_buffer.size());
        //size_t bytes_written = packet::frame::stuff(
        //        encode_buffer.data(), packet_buffer.data(), bytes_encoded);
        //if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        //    usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        //}
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            typename S::observer_t& observer = bicycle.observer();
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            /* Reduce steer measurement noise covariance */
            observer.set_R(parameters::defaultvalue::kalman::R/1000);

            /* prime the Kalman gain matrix */
            bicycle.prime_observer();

            /* We start with steer angle equal to the measurement and all other state elements at zero.  */
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    angle::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            // no-op
            (void)bicycle;
        }
    };

    THD_WORKING_AREA(wa_kinematics_thread, 2048);
    THD_FUNCTION(kinematics_thread, arg) {
        bicycle_t& bicycle = *static_cast<bicycle_t*>(arg);

        chRegSetThreadName("kinematics");
        while (true) {
            systime_t starttime = chVTGetSystemTime();
            update_and_transmit_kinematics(bicycle);
            systime_t sleeptime = kinematics_loop_time + starttime - chVTGetSystemTime();
            if (sleeptime >= kinematics_loop_time) {
                continue;
            } else {
                chThdSleep(sleeptime);
            }
        }
    }

    size_t write_message_to_encode_buffer(const SimulationMessage& m) {
        const size_t bytes_encoded = packet::serialize::encode_delimited(
                m, encode_buffer.data(), encode_buffer.size());
        return packet::frame::stuff(
                encode_buffer.data(), packet_buffer.data(), bytes_encoded);
    }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     * - Kernel initialization, the main() function becomes a thread and the
     *   RTOS is active.
     */
    halInit();
    chSysInit();

    /*
     * Initialize a serial-over-USB CDC driver.
     */
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    /*
     * Activate the USB driver and then the USB bus pull-up on D+.
     * Note, a delay is inserted in order to not have to disconnect the cable
     * after a reset.
     */
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    /* create the blink thread and print state monitor */
    chBlinkThreadCreateStatic();

    /*
     * Start sensors.
     * Encoder:
     *   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
     *   Pins for encoder driver 3 are already set in board.h.
     */
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(1000); /* trigger ADC conversion at 1 kHz */

    /*
     * Set torque measurement enable line low.
     * The output of the Kistler torque sensor is not valid until after a falling edge
     * on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
     * reversed due to NPN switch Q1.
     */
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    /*
     * Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
     * The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
     * and must be changed to use as analog output.
     */
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_torque(0.0f);

    /*
     * Initialize bicycle. The initial velocity is important as we use it to prime
     * the Kalman gain matrix.
     */
    bicycle_t bicycle(fixed_velocity, dynamics_period);

    /*
     * Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
     * TODO: naming here is poor
     */
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::HANDLEBAR_INERTIA);

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle);

    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    /*
     * Transmit initial message containing gitsha1, model, and observer data.
     * This initial transmission is blocking.
     */
    msg = msg_init;
    msg.timestamp = chVTGetSystemTime();
    message::set_simulation_full_model_observer(&msg, bicycle);
    size_t bytes_written = write_message_to_encode_buffer(msg);

    /* block until ready */
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }

    /* transmit data */
    if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        chTMStartMeasurementX(&transmission_time_measurement);
        usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        chTMStopMeasurementX(&transmission_time_measurement);
    }

    /*
     * Normal main() thread activity, in this demo it simulates the bicycle
     * dynamics in real-time (roughly).
     */
    chThdCreateStatic(wa_kinematics_thread, sizeof(wa_kinematics_thread),
            NORMALPRIO - 1, kinematics_thread, static_cast<void*>(&bicycle));
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        /* get sensor measurements */
        const float kistler_torque = static_cast<float>(
                analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        const float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = angle::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = -angle::encoder_count<float>(encoder_rear_wheel);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(angle::encoder_rate(encoder_rear_wheel)));

        /* yaw angle, just use previous state value */
        const float yaw_angle = angle::wrap(bicycle.pose().yaw);

        /* calculate rider applied torque */
        const float inertia_torque = -handlebar_model.feedback_torque(bicycle.observer().state());
        const float steer_torque = kistler_torque - inertia_torque;

        /* simulate bicycle */
        bicycle.set_v(fixed_velocity);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        /* generate handlebar torque output */
        const dacsample_t handlebar_torque_dac = set_handlebar_torque(bicycle.handlebar_feedback_torque());
        chTMStopMeasurementX(&computation_time_measurement);

        /* prepare message for transmission */
        msg = msg_init;
        msg.timestamp = starttime;
        message::set_bicycle_input(&msg.input,
                (model_t::input_t() << roll_torque, steer_torque).finished());
        msg.has_input = true;
        message::set_simulation_state(&msg, bicycle);
        message::set_simulation_auxiliary_state(&msg, bicycle);
        if (std::is_same<observer_t, typename observer::Kalman<model_t>>::value) {
            message::set_kalman_gain(&msg.kalman, bicycle.observer());
            msg.has_kalman = true;
        }
        message::set_simulation_actuators(&msg, handlebar_torque_dac);
        message::set_simulation_sensors(&msg,
                analog.get_adc12(), analog.get_adc13(),
                encoder_steer.count(), encoder_rear_wheel.count());
        message::set_simulation_timing(&msg,
                computation_time_measurement.last, transmission_time_measurement.last);
        msg.feedback_torque = bicycle.handlebar_feedback_torque();
        msg.has_feedback_torque = true;
        size_t bytes_written = write_message_to_encode_buffer(msg);

        /* transmit message */
        if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
            chTMStartMeasurementX(&transmission_time_measurement);
            usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
            chTMStopMeasurementX(&transmission_time_measurement);
        }

        const systime_t looptime = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        const systime_t sleeptime = looptime + starttime - chVTGetSystemTime();
        if (sleeptime >= looptime) {
            //chDbgAssert(false, "deadline missed");
            continue;
        } else {
            chThdSleep(sleeptime);
        }
    }
}
<commit_msg>Refactor comment type in Flimnap<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "packet/frame.h"
#include "packet/serialize.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "angle.h"
#include "blink.h"
#include "usbconfig.h"
#include "saconfig.h"

#include "parameters.h"

#include <algorithm>
#include <array>
#include <type_traits>

#include "bicycle/kinematic.h" // simplified bicycle model
#include "bicycle/whipple.h" // whipple bicycle model
#include "oracle.h" // oracle observer
#include "kalman.h" // kalman filter observer
#include "haptic.h" // handlebar feedback
#include "simbicycle.h"

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using observer_t = observer::Oracle<model_t>;
    using haptic_t = haptic::HandlebarStatic;
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleWhipple;
    using observer_t = observer::Kalman<model_t>;
    using haptic_t = haptic::HandlebarDynamic;
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
    using bicycle_t = sim::Bicycle<model_t, observer_t, haptic_t>;

    // sensors
    Analog analog;
    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1), 3.0f);
    filter::MovingAverage<float, 5> velocity_filter;

    constexpr float fixed_velocity = 7.0f;

    // transmission
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> encode_buffer;
    std::array<uint8_t, SimulationMessage_size + packet::frame::PACKET_OVERHEAD> packet_buffer;
    SimulationMessage msg;
    constexpr SimulationMessage msg_init = SimulationMessage_init_zero;

    // kinematics loop
    constexpr systime_t kinematics_loop_time = US2ST(8333); // update kinematics at 120 Hz

    // dynamics loop
    constexpr model::real_t dynamics_period = 0.005; //5 ms -> 200 Hz
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;

    dacsample_t set_handlebar_torque(float handlebar_torque) {
        int32_t saturated_value = (handlebar_torque/sa::MAX_KOLLMORGEN_TORQUE * 2048) +
            sa::KOLLMORGEN_DAC_ZERO_OFFSET;
        saturated_value = std::min<int32_t>(std::max<int32_t>(saturated_value, 0), 4096);
        dacsample_t aout = static_cast<dacsample_t>(saturated_value);
        dacPutChannelX(sa::KOLLM_DAC, 0, aout);
        return aout;
    }

    void update_and_transmit_kinematics(bicycle_t& bicycle) {
        bicycle.update_kinematics();
        //
        //TODO: Pose message should be transmitted asynchronously.
        //After starting transmission, the simulation should start to calculate pose for the next timestep.
        //This is currently not necessary given the observed timings.
        //
        //size_t bytes_encoded = packet::serialize::encode_delimited(bicycle.pose(),
        //        encode_buffer.data(), encode_buffer.size());
        //TODO: clear message
        //message::set_simulation_loop_values(&msg, bicycle);
        //size_t bytes_encoded = packet::serialize::encode_delimited(
        //        msg, encode_buffer.data(), encode_buffer.size());
        //size_t bytes_written = packet::frame::stuff(
        //        encode_buffer.data(), packet_buffer.data(), bytes_encoded);
        //if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        //    usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        //}
    }

    template <typename T>
    struct observer_initializer{
        template <typename S = T>
        typename std::enable_if<std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            typename S::observer_t& observer = bicycle.observer();
            observer.set_Q(parameters::defaultvalue::kalman::Q(observer.dt()));
            // Reduce steer measurement noise covariance
            observer.set_R(parameters::defaultvalue::kalman::R/1000);

            // prime the Kalman gain matrix
            bicycle.prime_observer();

            // We start with steer angle equal to the measurement and all other state elements at zero.
            model_t::state_t x0 = model_t::state_t::Zero();
            model_t::set_state_element(x0, model_t::state_index_t::steer_angle,
                    angle::encoder_count<float>(encoder_steer));
            observer.set_x(x0);
        }
        template <typename S = T>
        typename std::enable_if<!std::is_same<typename S::observer_t, observer::Kalman<model_t>>::value, void>::type
        initialize(S& bicycle) {
            // no-op
            (void)bicycle;
        }
    };

    THD_WORKING_AREA(wa_kinematics_thread, 2048);
    THD_FUNCTION(kinematics_thread, arg) {
        bicycle_t& bicycle = *static_cast<bicycle_t*>(arg);

        chRegSetThreadName("kinematics");
        while (true) {
            systime_t starttime = chVTGetSystemTime();
            update_and_transmit_kinematics(bicycle);
            systime_t sleeptime = kinematics_loop_time + starttime - chVTGetSystemTime();
            if (sleeptime >= kinematics_loop_time) {
                continue;
            } else {
                chThdSleep(sleeptime);
            }
        }
    }

    size_t write_message_to_encode_buffer(const SimulationMessage& m) {
        const size_t bytes_encoded = packet::serialize::encode_delimited(
                m, encode_buffer.data(), encode_buffer.size());
        return packet::frame::stuff(
                encode_buffer.data(), packet_buffer.data(), bytes_encoded);
    }
} // namespace

/*
 * Application entry point.
 */
int main(void) {

    //System initializations.
    //- HAL initialization, this also initializes the configured device drivers
    //  and performs the board-specific initializations.
    //- Kernel initialization, the main() function becomes a thread and the
    //  RTOS is active.
    halInit();
    chSysInit();

    // Initialize a serial-over-USB CDC driver.
    sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);

    //Activate the USB driver and then the USB bus pull-up on D+.
    //Note, a delay is inserted in order to not have to disconnect the cable
    //after a reset.
    board_usb_lld_disconnect_bus();   //usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(1500);
    usbStart(serusbcfg.usbp, &usbcfg);
    board_usb_lld_connect_bus();      //usbConnectBus(serusbcfg.usbp);

    // create the blink thread and print state monitor
    chBlinkThreadCreateStatic();

    // Start sensors.
    // Encoder:
    //   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
    //   Pins for encoder driver 3 are already set in board.h.
    palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
    encoder_steer.start();
    encoder_rear_wheel.start();
    analog.start(1000); // trigger ADC conversion at 1 kHz

    //Set torque measurement enable line low.
    //The output of the Kistler torque sensor is not valid until after a falling edge
    //on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
    //reversed due to NPN switch Q1.
    palClearLine(LINE_TORQUE_MEAS_EN);
    chThdSleepMilliseconds(1);
    palSetLine(LINE_TORQUE_MEAS_EN);

    // Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
    // The default line configuration is OUTPUT_OPENDRAIN_PULLUP  for SPI1_ENC1_NSS
    // and must be changed to use as analog output.
    palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
    dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
    set_handlebar_torque(0.0f);

    // Initialize bicycle. The initial velocity is important as we use it to prime
    // the Kalman gain matrix.
    bicycle_t bicycle(fixed_velocity, dynamics_period);

    // Initialize HandlebarDynamic object to estimate torque due to handlebar inertia.
    // TODO: naming here is poor
    haptic::HandlebarDynamic handlebar_model(bicycle.model(), sa::HANDLEBAR_INERTIA);

    observer_initializer<observer_t> oi;
    oi.initialize(bicycle);

    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    // Transmit initial message containing gitsha1, model, and observer data.
    // This initial transmission is blocking.
    msg = msg_init;
    msg.timestamp = chVTGetSystemTime();
    message::set_simulation_full_model_observer(&msg, bicycle);
    size_t bytes_written = write_message_to_encode_buffer(msg);

    // block until ready
    while ((SDU1.config->usbp->state != USB_ACTIVE) || (SDU1.state != SDU_READY)) {
        chThdSleepMilliseconds(10);
    }

    // transmit data
    if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
        chTMStartMeasurementX(&transmission_time_measurement);
        usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
        chTMStopMeasurementX(&transmission_time_measurement);
    }

    // Normal main() thread activity, in this project it simulates the bicycle
    // dynamics in real-time (roughly).
    chThdCreateStatic(wa_kinematics_thread, sizeof(wa_kinematics_thread),
            NORMALPRIO - 1, kinematics_thread, static_cast<void*>(&bicycle));
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        // get sensor measurements
        const float kistler_torque = static_cast<float>(
                analog.get_adc12()*2.0f*sa::MAX_KISTLER_TORQUE/4096 -
                sa::MAX_KISTLER_TORQUE);
        const float motor_torque = static_cast<float>(
                analog.get_adc13()*2.0f*sa::MAX_KOLLMORGEN_TORQUE/4096 -
                sa::MAX_KOLLMORGEN_TORQUE);
        const float steer_angle = angle::encoder_count<float>(encoder_steer);
        const float rear_wheel_angle = -angle::encoder_count<float>(encoder_rear_wheel);
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(angle::encoder_rate(encoder_rear_wheel)));

        // yaw angle, just use previous state value
        const float yaw_angle = angle::wrap(bicycle.pose().yaw);

        // calculate rider applied torque
        const float inertia_torque = -handlebar_model.feedback_torque(bicycle.observer().state());
        const float steer_torque = kistler_torque - inertia_torque;

        // simulate bicycle
        bicycle.set_v(fixed_velocity);
        bicycle.update_dynamics(roll_torque, steer_torque, yaw_angle, steer_angle, rear_wheel_angle);

        // generate handlebar torque output
        const dacsample_t handlebar_torque_dac = set_handlebar_torque(bicycle.handlebar_feedback_torque());
        chTMStopMeasurementX(&computation_time_measurement);

        // prepare message for transmission
        msg = msg_init;
        msg.timestamp = starttime;
        message::set_bicycle_input(&msg.input,
                (model_t::input_t() << roll_torque, steer_torque).finished());
        msg.has_input = true;
        message::set_simulation_state(&msg, bicycle);
        message::set_simulation_auxiliary_state(&msg, bicycle);
        if (std::is_same<observer_t, typename observer::Kalman<model_t>>::value) {
            message::set_kalman_gain(&msg.kalman, bicycle.observer());
            msg.has_kalman = true;
        }
        message::set_simulation_actuators(&msg, handlebar_torque_dac);
        message::set_simulation_sensors(&msg,
                analog.get_adc12(), analog.get_adc13(),
                encoder_steer.count(), encoder_rear_wheel.count());
        message::set_simulation_timing(&msg,
                computation_time_measurement.last, transmission_time_measurement.last);
        msg.feedback_torque = bicycle.handlebar_feedback_torque();
        msg.has_feedback_torque = true;
        size_t bytes_written = write_message_to_encode_buffer(msg);

        // transmit message
        if ((SDU1.config->usbp->state == USB_ACTIVE) && (SDU1.state == SDU_READY)) {
            chTMStartMeasurementX(&transmission_time_measurement);
            usbTransmit(SDU1.config->usbp, SDU1.config->bulk_in, packet_buffer.data(), bytes_written);
            chTMStopMeasurementX(&transmission_time_measurement);
        }

        const systime_t looptime = MS2ST(static_cast<systime_t>(1000*bicycle.dt()));
        const systime_t sleeptime = looptime + starttime - chVTGetSystemTime();
        if (sleeptime >= looptime) {
            //chDbgAssert(false, "deadline missed");
            continue;
        } else {
            chThdSleep(sleeptime);
        }
    }
}
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (directui@nokia.com)
**
** This file is part of meegotouch-controlpanelsoundsettingsapplet.
**
** If you have questions regarding the use of this file, please contact
** Nokia at directui@nokia.com.
**
** This library is free software; you can redistribute it and/or
** modify it under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation
** and appearing in the file LICENSE.LGPL included in the packaging
** of this file.
**
****************************************************************************/
#include "drilldownitem.h"

#include <MWidget>
#include <MLabel>
#include <MImageWidget>
#include <QGraphicsGridLayout>

#define DEBUG
#define WARNING
#include "../debug.h"

DrillDownItem::DrillDownItem (
        MBasicListItem::ItemStyle  style,
        QGraphicsItem             *parent) :
    MBasicListItem (style, parent)
{
}

QGraphicsLayout *
DrillDownItem::createLayout()
{
    QGraphicsGridLayout *layout;
    MLabel              *titleLabel;
    MLabel              *subTitleLabel;
    MImageWidget        *iconWidget;
    MImageWidget        *drillIconWidget;

    layout = new QGraphicsGridLayout(this);
    layout->setContentsMargins(0, 0, 0, 0);
    layout->setSpacing(0);

    titleLabel =  titleLabelWidget ();
    subTitleLabel = subtitleLabelWidget ();
    iconWidget = imageWidget();

    drillIconWidget = new MImageWidget ();
    drillIconWidget->setImage ("icon-m-common-drilldown-arrow");

    // FIXME: According to the layout guide we should use CommonDrilldownIcon,
    // but that does not look good. It makes the icon pixelated.
    drillIconWidget->setStyleName ("CommonMainIcon");
    titleLabel->setStyleName ("CommonTitleInverted");
    subTitleLabel->setStyleName ("CommonSubTitleInverted");

    switch (itemStyle()) {
        case TitleWithSubtitle:
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 0);
            layout->setAlignment(titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The sub-title label.
             */
            layout->addItem (subTitleLabel, 1, 0);
            layout->setAlignment (subTitleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 1, 2, 1);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        case IconWithTitleAndSubtitle:
            /*
             * The left side icon.
             */
            layout->addItem (iconWidget, 0, 0, 2, 1);
            layout->setAlignment (iconWidget, Qt::AlignVCenter | Qt::AlignRight);
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 1);
            layout->setAlignment (titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The sub-title label.
             */
            layout->addItem (subTitleLabel, 1, 1, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 2, 2, 1);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        case IconWithTitle:
            /*
             * The left side icon.
             */
            layout->addItem (iconWidget, 0, 0);
            layout->setAlignment (iconWidget, Qt::AlignVCenter | Qt::AlignRight);
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 1);
            layout->setAlignment (titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 2);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        default:
            SYS_WARNING ("itemStyle not supported.");
    }

    setStyleName ("CommonPanelInverted");
    return layout;
}

<commit_msg>Fixes: Use the proper style name for drilldown icon in theme-aplet<commit_after>/****************************************************************************
**
** Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (directui@nokia.com)
**
** This file is part of meegotouch-controlpanelsoundsettingsapplet.
**
** If you have questions regarding the use of this file, please contact
** Nokia at directui@nokia.com.
**
** This library is free software; you can redistribute it and/or
** modify it under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation
** and appearing in the file LICENSE.LGPL included in the packaging
** of this file.
**
****************************************************************************/
#include "drilldownitem.h"

#include <MWidget>
#include <MLabel>
#include <MImageWidget>
#include <QGraphicsGridLayout>

#define DEBUG
#define WARNING
#include "../debug.h"

DrillDownItem::DrillDownItem (
        MBasicListItem::ItemStyle  style,
        QGraphicsItem             *parent) :
    MBasicListItem (style, parent)
{
}

QGraphicsLayout *
DrillDownItem::createLayout()
{
    QGraphicsGridLayout *layout;
    MLabel              *titleLabel;
    MLabel              *subTitleLabel;
    MImageWidget        *iconWidget;
    MImageWidget        *drillIconWidget;

    layout = new QGraphicsGridLayout(this);
    layout->setContentsMargins(0, 0, 0, 0);
    layout->setSpacing(0);

    titleLabel =  titleLabelWidget ();
    subTitleLabel = subtitleLabelWidget ();
    iconWidget = imageWidget();

    drillIconWidget = new MImageWidget ();
    drillIconWidget->setImage ("icon-m-common-drilldown-arrow");

    drillIconWidget->setStyleName ("CommonDrillDownIcon");
    titleLabel->setStyleName ("CommonTitleInverted");
    subTitleLabel->setStyleName ("CommonSubTitleInverted");

    switch (itemStyle()) {
        case TitleWithSubtitle:
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 0);
            layout->setAlignment(titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The sub-title label.
             */
            layout->addItem (subTitleLabel, 1, 0);
            layout->setAlignment (subTitleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 1, 2, 1);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        case IconWithTitleAndSubtitle:
            /*
             * The left side icon.
             */
            layout->addItem (iconWidget, 0, 0, 2, 1);
            layout->setAlignment (iconWidget, Qt::AlignVCenter | Qt::AlignRight);
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 1);
            layout->setAlignment (titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The sub-title label.
             */
            layout->addItem (subTitleLabel, 1, 1, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 2, 2, 1);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        case IconWithTitle:
            /*
             * The left side icon.
             */
            layout->addItem (iconWidget, 0, 0);
            layout->setAlignment (iconWidget, Qt::AlignVCenter | Qt::AlignRight);
            /*
             * The title label.
             */
            layout->addItem (titleLabel, 0, 1);
            layout->setAlignment (titleLabel, Qt::AlignLeft | Qt::AlignVCenter);
            /*
             * The drill down icon.
             */
            layout->addItem(drillIconWidget, 0, 2);
            layout->setAlignment (drillIconWidget, Qt::AlignVCenter | Qt::AlignRight);
            break;

        default:
            SYS_WARNING ("itemStyle not supported.");
    }

    setStyleName ("CommonPanelInverted");
    return layout;
}

<|endoftext|>


<commit_before>#line 2 "togo/gfx/renderer/private.ipp"
/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.
*/

#pragma once

#include <togo/config.hpp>
#include <togo/gfx/types.hpp>
#include <togo/gfx/renderer/types.hpp>
#include <togo/gfx/renderer/private.hpp>

namespace togo {
namespace gfx {

#define TOGO_GFX_RENDERER_TEARDOWN_RA_(ra, func_destroy)		\
	if (renderer->ra._num > 0) {								\
		for (auto const& slot : renderer->ra._slots) {			\
			if (slot.id._value != ID_VALUE_NULL) {				\
				renderer::func_destroy(renderer, slot.id);		\
			}													\
		}														\
	}

void renderer::teardown_base(gfx::Renderer* const renderer) {
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_buffers, destroy_buffer);
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_buffer_bindings, destroy_buffer_binding);
	//TOGO_GFX_RENDERER_TEARDOWN_RA_(_textures, destroy_texture);
	//TOGO_GFX_RENDERER_TEARDOWN_RA_(_uniforms, destroy_uniform);
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_shaders, destroy_shader);
	for (auto& pb_name_hash : renderer->_fixed_param_blocks) {
		pb_name_hash = gfx::PB_NAME_NULL;
	}
	renderer->_num_active_draw_param_blocks = 0;
}

} // namespace gfx
} // namespace togo
<commit_msg>gfx/renderer/private: destroy generator defs on teardown.<commit_after>#line 2 "togo/gfx/renderer/private.ipp"
/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.
*/

#pragma once

#include <togo/config.hpp>
#include <togo/gfx/types.hpp>
#include <togo/gfx/renderer/types.hpp>
#include <togo/gfx/renderer/private.hpp>

namespace togo {
namespace gfx {

#define TOGO_GFX_RENDERER_TEARDOWN_RA_(ra, func_destroy)		\
	if (renderer->ra._num > 0) {								\
		for (auto const& slot : renderer->ra._slots) {			\
			if (slot.id._value != ID_VALUE_NULL) {				\
				renderer::func_destroy(renderer, slot.id);		\
			}													\
		}														\
	}

void renderer::teardown_base(gfx::Renderer* const renderer) {
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_buffers, destroy_buffer);
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_buffer_bindings, destroy_buffer_binding);
	//TOGO_GFX_RENDERER_TEARDOWN_RA_(_textures, destroy_texture);
	//TOGO_GFX_RENDERER_TEARDOWN_RA_(_uniforms, destroy_uniform);
	TOGO_GFX_RENDERER_TEARDOWN_RA_(_shaders, destroy_shader);
	for (auto& pb_name_hash : renderer->_fixed_param_blocks) {
		pb_name_hash = gfx::PB_NAME_NULL;
	}
	renderer->_num_active_draw_param_blocks = 0;
	for (auto& entry : renderer->_generators) {
		entry.value.func_destroy(entry.value, renderer);
	}
	hash_map::clear(renderer->_generators);
}

} // namespace gfx
} // namespace togo
<|endoftext|>