<commit_before>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "domain.h"
#include <vespa/vespalib/util/stringfmt.h>
#include <vespa/vespalib/util/closuretask.h>
#include <vespa/fastos/file.h>
#include <algorithm>

#include <vespa/log/log.h>
LOG_SETUP(".transactionlog.domain");

using vespalib::string;
using vespalib::make_string;
using vespalib::LockGuard;
using vespalib::makeTask;
using vespalib::makeClosure;
using vespalib::Monitor;
using vespalib::MonitorGuard;
using search::common::FileHeaderContext;
using std::runtime_error;

namespace search::transactionlog {

Domain::Domain(const string &domainName, const string & baseDir, Executor & commitExecutor,
               Executor & sessionExecutor, uint64_t domainPartSize, DomainPart::Crc defaultCrcType,
               const FileHeaderContext &fileHeaderContext) :
    _defaultCrcType(defaultCrcType),
    _commitExecutor(commitExecutor),
    _sessionExecutor(sessionExecutor),
    _sessionId(1),
    _syncMonitor(),
    _pendingSync(false),
    _name(domainName),
    _domainPartSize(domainPartSize),
    _parts(),
    _lock(),
    _sessionLock(),
    _sessions(),
    _maxSessionRunTime(),
    _baseDir(baseDir),
    _fileHeaderContext(fileHeaderContext),
    _markedDeleted(false)
{
    int retval(0);
    if ((retval = makeDirectory(_baseDir.c_str())) != 0) {
        throw runtime_error(make_string("Failed creating basedirectory %s r(%d), e(%d)", _baseDir.c_str(), retval, errno));
    }
    if ((retval = makeDirectory(dir().c_str())) != 0) {
        throw runtime_error(make_string("Failed creating domaindir %s r(%d), e(%d)", dir().c_str(), retval, errno));
    }
    SerialNumList partIdVector = scanDir();
    const int64_t lastPart = partIdVector.empty() ? 0 : partIdVector.back();
    for (const int64_t partId : partIdVector) {
        if ( partId != -1) {
            _sessionExecutor.execute(makeTask(makeClosure(this, &Domain::addPart, partId, partId == lastPart)));
        }
    }
    _sessionExecutor.sync();
    if (_parts.empty() || _parts.crbegin()->second->isClosed()) {
        _parts[lastPart].reset(new DomainPart(_name, dir(), lastPart, _defaultCrcType, _fileHeaderContext, false));
    }
}

void Domain::addPart(int64_t partId, bool isLastPart) {
    DomainPart::SP dp(new DomainPart(_name, dir(), partId, _defaultCrcType, _fileHeaderContext, isLastPart));
    if (dp->size() == 0) {
        // Only last domain part is allowed to be truncated down to
        // empty size.
        assert(isLastPart);
        dp->erase(dp->range().to() + 1);
    } else {
        {
            LockGuard guard(_lock);
            _parts[partId] = dp;
        }
        if (! isLastPart) {
            dp->close();
        }
    }
}

class Sync : public vespalib::Executor::Task
{
public:
    Sync(Monitor &syncMonitor, const DomainPart::SP &dp, bool &pendingSync) :
        _syncMonitor(syncMonitor),
        _dp(dp),
        _pendingSync(pendingSync)
    { }
private:
    void run() override {
        _dp->sync();
        MonitorGuard guard(_syncMonitor);
        _pendingSync = false;
        guard.broadcast();
    }

    Monitor           & _syncMonitor;
    DomainPart::SP      _dp;
    bool              & _pendingSync;
};

Domain::~Domain() { }

DomainInfo
Domain::getDomainInfo() const
{
    LockGuard guard(_lock);
    DomainInfo info(SerialNumRange(begin(guard), end(guard)), size(guard), byteSize(guard), _maxSessionRunTime);
    for (const auto &entry: _parts) {
        const DomainPart &part = *entry.second;
        info.parts.emplace_back(PartInfo(part.range(), part.size(), part.byteSize(), part.fileName()));
    }
    return info;
}

SerialNum
Domain::begin() const
{
    return begin(LockGuard(_lock));
}

SerialNum
Domain::begin(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    SerialNum s(0);
    if ( ! _parts.empty() ) {
        s = _parts.begin()->second->range().from();
    }
    return s;
}

SerialNum
Domain::end() const
{
    return end(LockGuard(_lock));
}

SerialNum
Domain::end(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    SerialNum s(0);
    if ( ! _parts.empty() ) {
        s = _parts.rbegin()->second->range().to();
    }
    return s;
}

size_t
Domain::byteSize() const
{
    return byteSize(LockGuard(_lock));
}

size_t
Domain::byteSize(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    size_t size = 0;
    for (const auto &entry : _parts) {
        const DomainPart &part = *entry.second;
        size += part.byteSize();
    }
    return size;
}

SerialNum
Domain::getSynced() const
{
    SerialNum s(0);
    LockGuard guard(_lock);
    if (_parts.empty()) {
        return s;
    }
    DomainPartList::const_iterator it(_parts.end());
    --it;
    s = it->second->getSynced();
    if (s == 0 && it != _parts.begin()) {
        --it;
        s = it->second->getSynced();
    }
    return s;
}

void
Domain::triggerSyncNow()
{
    MonitorGuard guard(_syncMonitor);
    if (!_pendingSync) {
        _pendingSync = true;
        DomainPart::SP dp(_parts.rbegin()->second);
        _sessionExecutor.execute(Sync::UP(new Sync(_syncMonitor, dp, _pendingSync)));
    }
}

DomainPart::SP Domain::findPart(SerialNum s)
{
    LockGuard guard(_lock);
    DomainPartList::iterator it(_parts.upper_bound(s));
    if (!_parts.empty() && it != _parts.begin()) {
        DomainPartList::iterator prev(it);
        --prev;
        if (prev->second->range().to() > s) {
            return prev->second;
        }
    }
    if (it != _parts.end()) {
        return it->second;
    }
    return DomainPart::SP();
}

uint64_t Domain::size() const
{
    return size(LockGuard(_lock));
}

uint64_t Domain::size(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    uint64_t sz(0);
    for (const auto & part : _parts) {
        sz += part.second->size();
    }
    return sz;
}

SerialNum Domain::findOldestActiveVisit() const
{
    SerialNum oldestActive(std::numeric_limits<SerialNum>::max());
    LockGuard guard(_sessionLock);
    for (const auto & pair : _sessions) {
        Session * session(pair.second.get());
        if (!session->inSync()) {
            oldestActive = std::min(oldestActive, session->range().from());
        }
    }
    return oldestActive;
}

void Domain::cleanSessions()
{
    if ( _sessions.empty()) {
        return;
    }
    LockGuard guard(_sessionLock);
    for (SessionList::iterator it(_sessions.begin()), mt(_sessions.end()); it != mt; ) {
        Session * session(it->second.get());
        if (session->inSync()) {
            _sessions.erase(it++);
        } else if (session->finished()) {
            _sessions.erase(it++);
        } else {
            it++;
        }
    }
}

namespace {

void waitPendingSync(vespalib::Monitor &syncMonitor, bool &pendingSync)
{
    MonitorGuard guard(syncMonitor);
    while (pendingSync) {
        guard.wait();
    }
}

}

void Domain::commit(const Packet & packet)
{
    DomainPart::SP dp(_parts.rbegin()->second);
    vespalib::nbostream_longlivedbuf is(packet.getHandle().c_str(), packet.getHandle().size());
    Packet::Entry entry;
    entry.deserialize(is);
    if (dp->byteSize() > _domainPartSize) {
        waitPendingSync(_syncMonitor, _pendingSync);
        triggerSyncNow();
        waitPendingSync(_syncMonitor, _pendingSync);
        dp->close();
        dp.reset(new DomainPart(_name, dir(), entry.serial(), _defaultCrcType, _fileHeaderContext, false));
        {
            LockGuard guard(_lock);
            _parts[entry.serial()] = dp;
        }
        dp = _parts.rbegin()->second;
    }
    dp->commit(entry.serial(), packet);
    cleanSessions();
}

bool Domain::erase(SerialNum to)
{
    bool retval(true);
    /// Do not erase the last element
    for (DomainPartList::iterator it(_parts.begin()); (_parts.size() > 1) && (it->second.get()->range().to() < to); it = _parts.begin()) {
        DomainPart::SP dp(it->second);
        {
            LockGuard guard(_lock);
            _parts.erase(it);
        }
        retval = retval && dp->erase(to);
    }
    if (_parts.begin()->second->range().to() >= to) {
        _parts.begin()->second->erase(to);
    }
    return retval;
}

int Domain::visit(const Domain::SP & domain, SerialNum from, SerialNum to,
                  FRT_Supervisor & supervisor, FNET_Connection *conn)
{
    assert(this == domain.get());
    cleanSessions();
    SerialNumRange range(from, to);
    Session * session = new Session(_sessionId++, range, domain, supervisor, conn);
    LockGuard guard(_sessionLock);
    _sessions[session->id()] = Session::SP(session);
    return session->id();
}

int Domain::startSession(int sessionId)
{
    int retval(-1);
    LockGuard guard(_sessionLock);
    SessionList::iterator found = _sessions.find(sessionId);
    if (found != _sessions.end()) {
        found->second->setStartTime(std::chrono::steady_clock::now());
        if ( execute(Session::createTask(found->second)).get() == nullptr ) {
            retval = 0;
        } else {
            _sessions.erase(sessionId);
        }
    }
    return retval;
}

int Domain::closeSession(int sessionId)
{
    _commitExecutor.sync();
    int retval(-1);
    DurationSeconds sessionRunTime(0);
    {
        LockGuard guard(_sessionLock);
        SessionList::iterator found = _sessions.find(sessionId);
        if (found != _sessions.end()) {
            sessionRunTime = (std::chrono::steady_clock::now() - found->second->getStartTime());
            retval = 1;
            _sessionExecutor.sync();
        }
    }
    if (retval == 1) {
        FastOS_Thread::Sleep(10);
        LockGuard guard(_sessionLock);
        SessionList::iterator found = _sessions.find(sessionId);
        if (found != _sessions.end()) {
            _sessions.erase(sessionId);
            retval = 0;
        } else {
            retval = 0;
        }
    }
    {
        LockGuard guard(_lock);
        if (sessionRunTime > _maxSessionRunTime) {
            _maxSessionRunTime = sessionRunTime;
        }
    }
    return retval;
}

Domain::SerialNumList
Domain::scanDir()
{
    SerialNumList res;

    FastOS_DirectoryScan dirScan(dir().c_str());

    const char *wantPrefix = _name.c_str();
    size_t wantPrefixLen = strlen(wantPrefix);

    while (dirScan.ReadNext()) {
        const char *ename = dirScan.GetName();
        if (strcmp(ename, ".") == 0 ||
            strcmp(ename, "..") == 0)
            continue;
        if (strncmp(ename, wantPrefix, wantPrefixLen) != 0)
            continue;
        if (ename[wantPrefixLen] != '-')
            continue;
        const char *p = ename + wantPrefixLen + 1;
        uint64_t num = strtoull(p, NULL, 10);
        string checkName = make_string("%s-%016" PRIu64, _name.c_str(), num);
        if (strcmp(checkName.c_str(), ename) != 0)
            continue;
        res.push_back(static_cast<SerialNum>(num));
    }
    std::sort(res.begin(), res.end());
    return res;
}

}
<commit_msg>Use commit executor as that is the writer.<commit_after>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "domain.h"
#include <vespa/vespalib/util/stringfmt.h>
#include <vespa/vespalib/util/closuretask.h>
#include <vespa/fastos/file.h>
#include <algorithm>

#include <vespa/log/log.h>
LOG_SETUP(".transactionlog.domain");

using vespalib::string;
using vespalib::make_string;
using vespalib::LockGuard;
using vespalib::makeTask;
using vespalib::makeClosure;
using vespalib::Monitor;
using vespalib::MonitorGuard;
using search::common::FileHeaderContext;
using std::runtime_error;

namespace search::transactionlog {

Domain::Domain(const string &domainName, const string & baseDir, Executor & commitExecutor,
               Executor & sessionExecutor, uint64_t domainPartSize, DomainPart::Crc defaultCrcType,
               const FileHeaderContext &fileHeaderContext) :
    _defaultCrcType(defaultCrcType),
    _commitExecutor(commitExecutor),
    _sessionExecutor(sessionExecutor),
    _sessionId(1),
    _syncMonitor(),
    _pendingSync(false),
    _name(domainName),
    _domainPartSize(domainPartSize),
    _parts(),
    _lock(),
    _sessionLock(),
    _sessions(),
    _maxSessionRunTime(),
    _baseDir(baseDir),
    _fileHeaderContext(fileHeaderContext),
    _markedDeleted(false)
{
    int retval(0);
    if ((retval = makeDirectory(_baseDir.c_str())) != 0) {
        throw runtime_error(make_string("Failed creating basedirectory %s r(%d), e(%d)", _baseDir.c_str(), retval, errno));
    }
    if ((retval = makeDirectory(dir().c_str())) != 0) {
        throw runtime_error(make_string("Failed creating domaindir %s r(%d), e(%d)", dir().c_str(), retval, errno));
    }
    SerialNumList partIdVector = scanDir();
    const int64_t lastPart = partIdVector.empty() ? 0 : partIdVector.back();
    for (const int64_t partId : partIdVector) {
        if ( partId != -1) {
            _sessionExecutor.execute(makeTask(makeClosure(this, &Domain::addPart, partId, partId == lastPart)));
        }
    }
    _sessionExecutor.sync();
    if (_parts.empty() || _parts.crbegin()->second->isClosed()) {
        _parts[lastPart].reset(new DomainPart(_name, dir(), lastPart, _defaultCrcType, _fileHeaderContext, false));
    }
}

void Domain::addPart(int64_t partId, bool isLastPart) {
    DomainPart::SP dp(new DomainPart(_name, dir(), partId, _defaultCrcType, _fileHeaderContext, isLastPart));
    if (dp->size() == 0) {
        // Only last domain part is allowed to be truncated down to
        // empty size.
        assert(isLastPart);
        dp->erase(dp->range().to() + 1);
    } else {
        {
            LockGuard guard(_lock);
            _parts[partId] = dp;
        }
        if (! isLastPart) {
            dp->close();
        }
    }
}

class Sync : public vespalib::Executor::Task
{
public:
    Sync(Monitor &syncMonitor, const DomainPart::SP &dp, bool &pendingSync) :
        _syncMonitor(syncMonitor),
        _dp(dp),
        _pendingSync(pendingSync)
    { }
private:
    void run() override {
        _dp->sync();
        MonitorGuard guard(_syncMonitor);
        _pendingSync = false;
        guard.broadcast();
    }

    Monitor           & _syncMonitor;
    DomainPart::SP      _dp;
    bool              & _pendingSync;
};

Domain::~Domain() { }

DomainInfo
Domain::getDomainInfo() const
{
    LockGuard guard(_lock);
    DomainInfo info(SerialNumRange(begin(guard), end(guard)), size(guard), byteSize(guard), _maxSessionRunTime);
    for (const auto &entry: _parts) {
        const DomainPart &part = *entry.second;
        info.parts.emplace_back(PartInfo(part.range(), part.size(), part.byteSize(), part.fileName()));
    }
    return info;
}

SerialNum
Domain::begin() const
{
    return begin(LockGuard(_lock));
}

SerialNum
Domain::begin(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    SerialNum s(0);
    if ( ! _parts.empty() ) {
        s = _parts.begin()->second->range().from();
    }
    return s;
}

SerialNum
Domain::end() const
{
    return end(LockGuard(_lock));
}

SerialNum
Domain::end(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    SerialNum s(0);
    if ( ! _parts.empty() ) {
        s = _parts.rbegin()->second->range().to();
    }
    return s;
}

size_t
Domain::byteSize() const
{
    return byteSize(LockGuard(_lock));
}

size_t
Domain::byteSize(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    size_t size = 0;
    for (const auto &entry : _parts) {
        const DomainPart &part = *entry.second;
        size += part.byteSize();
    }
    return size;
}

SerialNum
Domain::getSynced() const
{
    SerialNum s(0);
    LockGuard guard(_lock);
    if (_parts.empty()) {
        return s;
    }
    DomainPartList::const_iterator it(_parts.end());
    --it;
    s = it->second->getSynced();
    if (s == 0 && it != _parts.begin()) {
        --it;
        s = it->second->getSynced();
    }
    return s;
}

void
Domain::triggerSyncNow()
{
    MonitorGuard guard(_syncMonitor);
    if (!_pendingSync) {
        _pendingSync = true;
        DomainPart::SP dp(_parts.rbegin()->second);
        _commitExecutor.execute(Sync::UP(new Sync(_syncMonitor, dp, _pendingSync)));
    }
}

DomainPart::SP Domain::findPart(SerialNum s)
{
    LockGuard guard(_lock);
    DomainPartList::iterator it(_parts.upper_bound(s));
    if (!_parts.empty() && it != _parts.begin()) {
        DomainPartList::iterator prev(it);
        --prev;
        if (prev->second->range().to() > s) {
            return prev->second;
        }
    }
    if (it != _parts.end()) {
        return it->second;
    }
    return DomainPart::SP();
}

uint64_t Domain::size() const
{
    return size(LockGuard(_lock));
}

uint64_t Domain::size(const LockGuard & guard) const
{
    (void) guard;
    assert(guard.locks(_lock));
    uint64_t sz(0);
    for (const auto & part : _parts) {
        sz += part.second->size();
    }
    return sz;
}

SerialNum Domain::findOldestActiveVisit() const
{
    SerialNum oldestActive(std::numeric_limits<SerialNum>::max());
    LockGuard guard(_sessionLock);
    for (const auto & pair : _sessions) {
        Session * session(pair.second.get());
        if (!session->inSync()) {
            oldestActive = std::min(oldestActive, session->range().from());
        }
    }
    return oldestActive;
}

void Domain::cleanSessions()
{
    if ( _sessions.empty()) {
        return;
    }
    LockGuard guard(_sessionLock);
    for (SessionList::iterator it(_sessions.begin()), mt(_sessions.end()); it != mt; ) {
        Session * session(it->second.get());
        if (session->inSync()) {
            _sessions.erase(it++);
        } else if (session->finished()) {
            _sessions.erase(it++);
        } else {
            it++;
        }
    }
}

namespace {

void waitPendingSync(vespalib::Monitor &syncMonitor, bool &pendingSync)
{
    MonitorGuard guard(syncMonitor);
    while (pendingSync) {
        guard.wait();
    }
}

}

void Domain::commit(const Packet & packet)
{
    DomainPart::SP dp(_parts.rbegin()->second);
    vespalib::nbostream_longlivedbuf is(packet.getHandle().c_str(), packet.getHandle().size());
    Packet::Entry entry;
    entry.deserialize(is);
    if (dp->byteSize() > _domainPartSize) {
        waitPendingSync(_syncMonitor, _pendingSync);
        triggerSyncNow();
        waitPendingSync(_syncMonitor, _pendingSync);
        dp->close();
        dp.reset(new DomainPart(_name, dir(), entry.serial(), _defaultCrcType, _fileHeaderContext, false));
        {
            LockGuard guard(_lock);
            _parts[entry.serial()] = dp;
        }
        dp = _parts.rbegin()->second;
    }
    dp->commit(entry.serial(), packet);
    cleanSessions();
}

bool Domain::erase(SerialNum to)
{
    bool retval(true);
    /// Do not erase the last element
    for (DomainPartList::iterator it(_parts.begin()); (_parts.size() > 1) && (it->second.get()->range().to() < to); it = _parts.begin()) {
        DomainPart::SP dp(it->second);
        {
            LockGuard guard(_lock);
            _parts.erase(it);
        }
        retval = retval && dp->erase(to);
    }
    if (_parts.begin()->second->range().to() >= to) {
        _parts.begin()->second->erase(to);
    }
    return retval;
}

int Domain::visit(const Domain::SP & domain, SerialNum from, SerialNum to,
                  FRT_Supervisor & supervisor, FNET_Connection *conn)
{
    assert(this == domain.get());
    cleanSessions();
    SerialNumRange range(from, to);
    Session * session = new Session(_sessionId++, range, domain, supervisor, conn);
    LockGuard guard(_sessionLock);
    _sessions[session->id()] = Session::SP(session);
    return session->id();
}

int Domain::startSession(int sessionId)
{
    int retval(-1);
    LockGuard guard(_sessionLock);
    SessionList::iterator found = _sessions.find(sessionId);
    if (found != _sessions.end()) {
        found->second->setStartTime(std::chrono::steady_clock::now());
        if ( execute(Session::createTask(found->second)).get() == nullptr ) {
            retval = 0;
        } else {
            _sessions.erase(sessionId);
        }
    }
    return retval;
}

int Domain::closeSession(int sessionId)
{
    _commitExecutor.sync();
    int retval(-1);
    DurationSeconds sessionRunTime(0);
    {
        LockGuard guard(_sessionLock);
        SessionList::iterator found = _sessions.find(sessionId);
        if (found != _sessions.end()) {
            sessionRunTime = (std::chrono::steady_clock::now() - found->second->getStartTime());
            retval = 1;
            _sessionExecutor.sync();
        }
    }
    if (retval == 1) {
        FastOS_Thread::Sleep(10);
        LockGuard guard(_sessionLock);
        SessionList::iterator found = _sessions.find(sessionId);
        if (found != _sessions.end()) {
            _sessions.erase(sessionId);
            retval = 0;
        } else {
            retval = 0;
        }
    }
    {
        LockGuard guard(_lock);
        if (sessionRunTime > _maxSessionRunTime) {
            _maxSessionRunTime = sessionRunTime;
        }
    }
    return retval;
}

Domain::SerialNumList
Domain::scanDir()
{
    SerialNumList res;

    FastOS_DirectoryScan dirScan(dir().c_str());

    const char *wantPrefix = _name.c_str();
    size_t wantPrefixLen = strlen(wantPrefix);

    while (dirScan.ReadNext()) {
        const char *ename = dirScan.GetName();
        if (strcmp(ename, ".") == 0 ||
            strcmp(ename, "..") == 0)
            continue;
        if (strncmp(ename, wantPrefix, wantPrefixLen) != 0)
            continue;
        if (ename[wantPrefixLen] != '-')
            continue;
        const char *p = ename + wantPrefixLen + 1;
        uint64_t num = strtoull(p, NULL, 10);
        string checkName = make_string("%s-%016" PRIu64, _name.c_str(), num);
        if (strcmp(checkName.c_str(), ename) != 0)
            continue;
        res.push_back(static_cast<SerialNum>(num));
    }
    std::sort(res.begin(), res.end());
    return res;
}

}
<|endoftext|>


<commit_before>/*
  WiFiServer.cpp - TCP/IP server for esp8266, mostly compatible
                   with Arduino WiFi shield library

  Copyright (c) 2014 Ivan Grokhotkov. All rights reserved.
  This file is part of the esp8266 core for Arduino environment.

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

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#define LWIP_INTERNAL

extern "C" {
    #include "osapi.h"
    #include "ets_sys.h"
}

#include "debug.h"
#include "ESP8266WiFi.h"
#include "WiFiClient.h"
#include "WiFiServer.h"
#include "lwip/opt.h"
#include "lwip/tcp.h"
#include "lwip/inet.h"
#include "include/ClientContext.h"

WiFiServer::WiFiServer(IPAddress addr, uint16_t port)
: _port(port)
, _addr(addr)
, _pcb(nullptr)
, _unclaimed(nullptr)
, _discarded(nullptr)
{
}

WiFiServer::WiFiServer(uint16_t port)
: _port(port)
, _addr((uint32_t) IPADDR_ANY)
, _pcb(nullptr)
, _unclaimed(nullptr)
, _discarded(nullptr)
{
}

void WiFiServer::begin() {
    err_t err;
    tcp_pcb* pcb = tcp_new();
    if (!pcb)
        return;

    ip_addr_t local_addr;
    local_addr.addr = (uint32_t) _addr;
    err = tcp_bind(pcb, &local_addr, _port);

    if (err != ERR_OK) {
        tcp_close(pcb);
        return;
    }

    tcp_pcb* listen_pcb = tcp_listen(pcb);
    if (!listen_pcb) {
        tcp_close(pcb);
        return;
    }
    _pcb = listen_pcb;
    tcp_accept(listen_pcb, &WiFiServer::_s_accept);
    tcp_arg(listen_pcb, (void*) this);
}

void WiFiServer::setNoDelay(bool nodelay) {
    _noDelay = nodelay;
}

bool WiFiServer::getNoDelay() {
    return _noDelay;
}

bool WiFiServer::hasClient() {
    if (_unclaimed)
        return true;
    return false;
}

WiFiClient WiFiServer::available(byte* status) {
    if (_unclaimed) {
        WiFiClient result(_unclaimed);
        _unclaimed = _unclaimed->next();
        result.setNoDelay(_noDelay);
        DEBUGV("WS:av\r\n");
        return result;
    }

    optimistic_yield(1000);
    return WiFiClient();
}

uint8_t WiFiServer::status()  {
    if (!_pcb)
        return CLOSED;
    return _pcb->state;
}

void WiFiServer::close() {
    if (!_pcb) {
      return;
    }
    tcp_close(_pcb);
}

void WiFiServer::stop() {
    close();
}

size_t WiFiServer::write(uint8_t b) {
    return write(&b, 1);
}

size_t WiFiServer::write(const uint8_t *buffer, size_t size) {
    // write to all clients
    // not implemented
    return 0;
}

template<typename T>
T* slist_append_tail(T* head, T* item) {
    if (!head)
        return item;
    T* last = head;
    while(last->next())
        last = last->next();
    last->next(item);
    return head;
}

int8_t WiFiServer::_accept(tcp_pcb* apcb, int8_t err) {
    DEBUGV("WS:ac\r\n");
    ClientContext* client = new ClientContext(apcb, &WiFiServer::_s_discard, this);
    _unclaimed = slist_append_tail(_unclaimed, client);
    tcp_accepted(_pcb);
    return ERR_OK;
}

void WiFiServer::_discard(ClientContext* client) {
    // _discarded = slist_append_tail(_discarded, client);
    DEBUGV("WS:dis\r\n");
}

int8_t WiFiServer::_s_accept(void *arg, tcp_pcb* newpcb, int8_t err) {
    return reinterpret_cast<WiFiServer*>(arg)->_accept(newpcb, err);
}

void WiFiServer::_s_discard(void* server, ClientContext* ctx) {
    reinterpret_cast<WiFiServer*>(server)->_discard(ctx);
}
<commit_msg>WiFiServer: fix error when calling `close` more than once<commit_after>/*
  WiFiServer.cpp - TCP/IP server for esp8266, mostly compatible
                   with Arduino WiFi shield library

  Copyright (c) 2014 Ivan Grokhotkov. All rights reserved.
  This file is part of the esp8266 core for Arduino environment.

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

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#define LWIP_INTERNAL

extern "C" {
    #include "osapi.h"
    #include "ets_sys.h"
}

#include "debug.h"
#include "ESP8266WiFi.h"
#include "WiFiClient.h"
#include "WiFiServer.h"
#include "lwip/opt.h"
#include "lwip/tcp.h"
#include "lwip/inet.h"
#include "include/ClientContext.h"

WiFiServer::WiFiServer(IPAddress addr, uint16_t port)
: _port(port)
, _addr(addr)
, _pcb(nullptr)
, _unclaimed(nullptr)
, _discarded(nullptr)
{
}

WiFiServer::WiFiServer(uint16_t port)
: _port(port)
, _addr((uint32_t) IPADDR_ANY)
, _pcb(nullptr)
, _unclaimed(nullptr)
, _discarded(nullptr)
{
}

void WiFiServer::begin() {
    close();
    err_t err;
    tcp_pcb* pcb = tcp_new();
    if (!pcb)
        return;

    ip_addr_t local_addr;
    local_addr.addr = (uint32_t) _addr;
    err = tcp_bind(pcb, &local_addr, _port);

    if (err != ERR_OK) {
        tcp_close(pcb);
        return;
    }

    tcp_pcb* listen_pcb = tcp_listen(pcb);
    if (!listen_pcb) {
        tcp_close(pcb);
        return;
    }
    _pcb = listen_pcb;
    tcp_accept(listen_pcb, &WiFiServer::_s_accept);
    tcp_arg(listen_pcb, (void*) this);
}

void WiFiServer::setNoDelay(bool nodelay) {
    _noDelay = nodelay;
}

bool WiFiServer::getNoDelay() {
    return _noDelay;
}

bool WiFiServer::hasClient() {
    if (_unclaimed)
        return true;
    return false;
}

WiFiClient WiFiServer::available(byte* status) {
    if (_unclaimed) {
        WiFiClient result(_unclaimed);
        _unclaimed = _unclaimed->next();
        result.setNoDelay(_noDelay);
        DEBUGV("WS:av\r\n");
        return result;
    }

    optimistic_yield(1000);
    return WiFiClient();
}

uint8_t WiFiServer::status()  {
    if (!_pcb)
        return CLOSED;
    return _pcb->state;
}

void WiFiServer::close() {
    if (!_pcb) {
      return;
    }
    tcp_close(_pcb);
    _pcb = nullptr;
}

void WiFiServer::stop() {
    close();
}

size_t WiFiServer::write(uint8_t b) {
    return write(&b, 1);
}

size_t WiFiServer::write(const uint8_t *buffer, size_t size) {
    // write to all clients
    // not implemented
    return 0;
}

template<typename T>
T* slist_append_tail(T* head, T* item) {
    if (!head)
        return item;
    T* last = head;
    while(last->next())
        last = last->next();
    last->next(item);
    return head;
}

int8_t WiFiServer::_accept(tcp_pcb* apcb, int8_t err) {
    DEBUGV("WS:ac\r\n");
    ClientContext* client = new ClientContext(apcb, &WiFiServer::_s_discard, this);
    _unclaimed = slist_append_tail(_unclaimed, client);
    tcp_accepted(_pcb);
    return ERR_OK;
}

void WiFiServer::_discard(ClientContext* client) {
    // _discarded = slist_append_tail(_discarded, client);
    DEBUGV("WS:dis\r\n");
}

int8_t WiFiServer::_s_accept(void *arg, tcp_pcb* newpcb, int8_t err) {
    return reinterpret_cast<WiFiServer*>(arg)->_accept(newpcb, err);
}

void WiFiServer::_s_discard(void* server, ClientContext* ctx) {
    reinterpret_cast<WiFiServer*>(server)->_discard(ctx);
}
<|endoftext|>


<commit_before>// Local-Hyperion includes
#include "LedDevicePhilipsHue.h"

// jsoncpp includes
#include <json/json.h>

// qt includes
#include <QtCore/qmath.h>
#include <QUrl>
#include <QHttpRequestHeader>
#include <QEventLoop>

#include <set>

bool operator ==(CiColor p1, CiColor p2) {
	return (p1.x == p2.x) && (p1.y == p2.y) && (p1.bri == p2.bri);
}

bool operator !=(CiColor p1, CiColor p2) {
	return !(p1 == p2);
}

PhilipsHueLight::PhilipsHueLight(unsigned int id, QString originalState, QString modelId) :
		id(id), originalState(originalState) {
	// Hue system model ids (http://www.developers.meethue.com/documentation/supported-lights).
	// Light strips, color iris, ...
	const std::set<QString> GAMUT_A_MODEL_IDS = { "LLC001", "LLC005", "LLC006", "LLC007", "LLC010", "LLC011", "LLC012",
			"LLC013", "LST001" };
	// Hue bulbs, spots, ...
	const std::set<QString> GAMUT_B_MODEL_IDS = { "LCT001", "LCT002", "LCT003", "LLM001" };
	// Find id in the sets and set the appropiate color space.
	if (GAMUT_A_MODEL_IDS.find(modelId) != GAMUT_A_MODEL_IDS.end()) {
		colorSpace.red = {0.703f, 0.296f};
		colorSpace.green = {0.2151f, 0.7106f};
		colorSpace.blue = {0.138f, 0.08f};
	} else if (GAMUT_B_MODEL_IDS.find(modelId) != GAMUT_B_MODEL_IDS.end()) {
		colorSpace.red = {0.675f, 0.322f};
		colorSpace.green = {0.4091f, 0.518f};
		colorSpace.blue = {0.167f, 0.04f};
	} else {
		colorSpace.red = {1.0f, 0.0f};
		colorSpace.green = {0.0f, 1.0f};
		colorSpace.blue = {0.0f, 0.0f};
	}
	// Initialize black color.
	black = rgbToCiColor(0.0f, 0.0f, 0.0f);
	// Initialize color with black
	color = {black.x, black.y, black.bri};
}

float PhilipsHueLight::crossProduct(CiColor p1, CiColor p2) {
	return p1.x * p2.y - p1.y * p2.x;
}

bool PhilipsHueLight::isPointInLampsReach(CiColor p) {
	CiColor v1 = { colorSpace.green.x - colorSpace.red.x, colorSpace.green.y - colorSpace.red.y };
	CiColor v2 = { colorSpace.blue.x - colorSpace.red.x, colorSpace.blue.y - colorSpace.red.y };
	CiColor q = { p.x - colorSpace.red.x, p.y - colorSpace.red.y };
	float s = crossProduct(q, v2) / crossProduct(v1, v2);
	float t = crossProduct(v1, q) / crossProduct(v1, v2);
	if ((s >= 0.0f) && (t >= 0.0f) && (s + t <= 1.0f)) {
		return true;
	}
	return false;
}

CiColor PhilipsHueLight::getClosestPointToPoint(CiColor a, CiColor b, CiColor p) {
	CiColor AP = { p.x - a.x, p.y - a.y };
	CiColor AB = { b.x - a.x, b.y - a.y };
	float ab2 = AB.x * AB.x + AB.y * AB.y;
	float ap_ab = AP.x * AB.x + AP.y * AB.y;
	float t = ap_ab / ab2;
	if (t < 0.0f) {
		t = 0.0f;
	} else if (t > 1.0f) {
		t = 1.0f;
	}
	return {a.x + AB.x * t, a.y + AB.y * t};
}

float PhilipsHueLight::getDistanceBetweenTwoPoints(CiColor p1, CiColor p2) {
	// Horizontal difference.
	float dx = p1.x - p2.x;
	// Vertical difference.
	float dy = p1.y - p2.y;
	// Absolute value.
	return sqrt(dx * dx + dy * dy);
}

CiColor PhilipsHueLight::rgbToCiColor(float red, float green, float blue) {
	// Apply gamma correction.
	float r = (red > 0.04045f) ? powf((red + 0.055f) / (1.0f + 0.055f), 2.4f) : (red / 12.92f);
	float g = (green > 0.04045f) ? powf((green + 0.055f) / (1.0f + 0.055f), 2.4f) : (green / 12.92f);
	float b = (blue > 0.04045f) ? powf((blue + 0.055f) / (1.0f + 0.055f), 2.4f) : (blue / 12.92f);
	// Convert to XYZ space.
	float X = r * 0.649926f + g * 0.103455f + b * 0.197109f;
	float Y = r * 0.234327f + g * 0.743075f + b * 0.022598f;
	float Z = r * 0.0000000f + g * 0.053077f + b * 1.035763f;
	// Convert to x,y space.
	float cx = X / (X + Y + Z);
	float cy = Y / (X + Y + Z);
	if (isnan(cx)) {
		cx = 0.0f;
	}
	if (isnan(cy)) {
		cy = 0.0f;
	}
	// Brightness is simply Y in the XYZ space.
	CiColor xy = { cx, cy, Y };
	// Check if the given XY value is within the color reach of our lamps.
	if (!isPointInLampsReach(xy)) {
		// It seems the color is out of reach let's find the closes color we can produce with our lamp and send this XY value out.
		CiColor pAB = getClosestPointToPoint(colorSpace.red, colorSpace.green, xy);
		CiColor pAC = getClosestPointToPoint(colorSpace.blue, colorSpace.red, xy);
		CiColor pBC = getClosestPointToPoint(colorSpace.green, colorSpace.blue, xy);
		// Get the distances per point and see which point is closer to our Point.
		float dAB = getDistanceBetweenTwoPoints(xy, pAB);
		float dAC = getDistanceBetweenTwoPoints(xy, pAC);
		float dBC = getDistanceBetweenTwoPoints(xy, pBC);
		float lowest = dAB;
		CiColor closestPoint = pAB;
		if (dAC < lowest) {
			lowest = dAC;
			closestPoint = pAC;
		}
		if (dBC < lowest) {
			lowest = dBC;
			closestPoint = pBC;
		}
		// Change the xy value to a value which is within the reach of the lamp.
		xy.x = closestPoint.x;
		xy.y = closestPoint.y;
	}
	return xy;
}

LedDevicePhilipsHue::LedDevicePhilipsHue(const std::string& output, const std::string& username, bool switchOffOnBlack,
		int transitiontime, std::vector<unsigned int> lightIds) :
		host(output.c_str()), username(username.c_str()), switchOffOnBlack(switchOffOnBlack), transitiontime(
				transitiontime), lightIds(lightIds) {
	http = new QHttp(host);
	timer.setInterval(3000);
	timer.setSingleShot(true);
	connect(&timer, SIGNAL(timeout()), this, SLOT(restoreStates()));
}

LedDevicePhilipsHue::~LedDevicePhilipsHue() {
	delete http;
}

int LedDevicePhilipsHue::write(const std::vector<ColorRgb> & ledValues) {
	// Save light states if not done before.
	if (!areStatesSaved()) {
		saveStates((unsigned int) ledValues.size());
		switchOn((unsigned int) ledValues.size());
	}
	// If there are less states saved than colors given, then maybe something went wrong before.
	if (lights.size() != ledValues.size()) {
		restoreStates();
		return 0;
	}
	// Iterate through colors and set light states.
	unsigned int idx = 0;
	for (const ColorRgb& color : ledValues) {
		// Get lamp.
		PhilipsHueLight& lamp = lights.at(idx);
		// Scale colors from [0, 255] to [0, 1] and convert to xy space.
		CiColor xy = lamp.rgbToCiColor(color.red / 255.0f, color.green / 255.0f, color.blue / 255.0f);
		// Write color if color has been changed.
		if (xy != lamp.color) {
			// Switch on if the lamp has been previously switched off.
			if (switchOffOnBlack && lamp.color == lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": true}"));
			}
			// Send adjust color and brightness command in JSON format.
			// We have to set the transition time each time.
			put(getStateRoute(lamp.id),
					QString("{\"xy\": [%1, %2], \"bri\": %3, \"transitiontime\": %4}").arg(xy.x).arg(xy.y).arg(
							qRound(xy.bri * 255.0f)).arg(transitiontime));

		}
		// Switch lamp off if switchOffOnBlack is enabled and the lamp is currently on.
		if (switchOffOnBlack) {
			// From black to a color.
			if (lamp.color == lamp.black && xy != lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": true}"));
			}
			// From a color to black.
			else if (lamp.color != lamp.black && xy == lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": false}"));
			}
		}
		// Remember last color.
		lamp.color = xy;
		// Next light id.
		idx++;
	}
	timer.start();
	return 0;
}

int LedDevicePhilipsHue::switchOff() {
	timer.stop();
	// If light states have been saved before, ...
	if (areStatesSaved()) {
		// ... restore them.
		restoreStates();
	}
	return 0;
}

void LedDevicePhilipsHue::put(QString route, QString content) {
	QString url = QString("/api/%1/%2").arg(username).arg(route);
	QHttpRequestHeader header("PUT", url);
	header.setValue("Host", host);
	header.setValue("Accept-Encoding", "identity");
	header.setValue("Connection", "keep-alive");
	header.setValue("Content-Length", QString("%1").arg(content.size()));
	QEventLoop loop;
	// Connect requestFinished signal to quit slot of the loop.
	loop.connect(http, SIGNAL(requestFinished(int, bool)), SLOT(quit()));
	// Perfrom request
	http->request(header, content.toAscii());
	// Go into the loop until the request is finished.
	loop.exec();
}

QByteArray LedDevicePhilipsHue::get(QString route) {
	QString url = QString("/api/%1/%2").arg(username).arg(route);
	// Event loop to block until request finished.
	QEventLoop loop;
	// Connect requestFinished signal to quit slot of the loop.
	loop.connect(http, SIGNAL(requestFinished(int, bool)), SLOT(quit()));
	// Perfrom request
	http->get(url);
	// Go into the loop until the request is finished.
	loop.exec();
	// Read all data of the response.
	return http->readAll();
}

QString LedDevicePhilipsHue::getStateRoute(unsigned int lightId) {
	return QString("lights/%1/state").arg(lightId);
}

QString LedDevicePhilipsHue::getRoute(unsigned int lightId) {
	return QString("lights/%1").arg(lightId);
}

void LedDevicePhilipsHue::saveStates(unsigned int nLights) {
	// Clear saved lamps.
	lights.clear();
	// Use json parser to parse reponse.
	Json::Reader reader;
	Json::FastWriter writer;
	// Create light ids if none supplied by the user.
	if (lightIds.size() != nLights) {
		lightIds.clear();
		for (unsigned int i = 0; i < nLights; i++) {
			lightIds.push_back(i + 1);
		}
	}
	// Iterate lights.
	for (unsigned int i = 0; i < nLights; i++) {
		// Read the response.
		QByteArray response = get(getRoute(lightIds.at(i)));
		// Parse JSON.
		Json::Value json;
		if (!reader.parse(QString(response).toStdString(), json)) {
			// Error occured, break loop.
			break;
		}
		// Get state object values which are subject to change.
		Json::Value state(Json::objectValue);
		state["on"] = json["state"]["on"];
		if (json["state"]["on"] == true) {
			state["xy"] = json["state"]["xy"];
			state["bri"] = json["state"]["bri"];
		}
		// Determine the model id.
		QString modelId = QString(writer.write(json["modelid"]).c_str()).trimmed().replace("\"", "");
		QString originalState = QString(writer.write(state).c_str()).trimmed();
		// Save state object.
		lights.push_back(PhilipsHueLight(lightIds.at(i), originalState, modelId));
	}
}

void LedDevicePhilipsHue::switchOn(unsigned int nLights) {
	for (PhilipsHueLight light : lights) {
		put(getStateRoute(light.id), "{\"on\": true}");
	}
}

void LedDevicePhilipsHue::restoreStates() {
	for (PhilipsHueLight light : lights) {
		put(getStateRoute(light.id), light.originalState);
	}
	// Clear saved light states.
	lights.clear();
}

bool LedDevicePhilipsHue::areStatesSaved() {
	return !lights.empty();
}
<commit_msg>Added some more hue devices and Gamut-Profile<commit_after>// Local-Hyperion includes
#include "LedDevicePhilipsHue.h"

// jsoncpp includes
#include <json/json.h>

// qt includes
#include <QtCore/qmath.h>
#include <QUrl>
#include <QHttpRequestHeader>
#include <QEventLoop>

#include <set>

bool operator ==(CiColor p1, CiColor p2) {
	return (p1.x == p2.x) && (p1.y == p2.y) && (p1.bri == p2.bri);
}

bool operator !=(CiColor p1, CiColor p2) {
	return !(p1 == p2);
}

PPhilipsHueLight::PhilipsHueLight(unsigned int id, QString originalState, QString modelId) :
    id(id), originalState(originalState) {
        // Hue system model ids (http://www.developers.meethue.com/documentation/supported-lights).
        // Light strips, color iris, ...
  const std::set<QString> GAMUT_A_MODEL_IDS = { "LLC001", "LLC005", "LLC006", "LLC007", "LLC010", "LLC011", "LLC012",
                        "LLC013", "LLC014", "LST001" };
  // Hue bulbs, spots, ...
  const std::set<QString> GAMUT_B_MODEL_IDS = { "LCT001", "LCT002", "LCT003", "LCT007", "LLM001" };
  // Hue Lightstrip plus, go ...
  const std::set<QString> GAMUT_C_MODEL_IDS = { "LLC020", "LST002" };
  // Find id in the sets and set the appropiate color space.
      if (GAMUT_A_MODEL_IDS.find(modelId) != GAMUT_A_MODEL_IDS.end()) {
              colorSpace.red = {0.703f, 0.296f};
              colorSpace.green = {0.2151f, 0.7106f};
              colorSpace.blue = {0.138f, 0.08f};
      } else if (GAMUT_B_MODEL_IDS.find(modelId) != GAMUT_B_MODEL_IDS.end()) {
              colorSpace.red = {0.675f, 0.322f};
              colorSpace.green = {0.4091f, 0.518f};
              colorSpace.blue = {0.167f, 0.04f};
      } else if (GAMUT_C_MODEL_IDS.find(modelId) != GAMUT_B_MODEL_IDS.end()) {
              colorSpace.red = {0.675f, 0.322f};
              colorSpace.green = {0.2151f, 0.7106f};
              colorSpace.blue = {0.167f, 0.04f};
	} else {
		colorSpace.red = {1.0f, 0.0f};
		colorSpace.green = {0.0f, 1.0f};
		colorSpace.blue = {0.0f, 0.0f};
	}
	// Initialize black color.
	black = rgbToCiColor(0.0f, 0.0f, 0.0f);
	// Initialize color with black
	color = {black.x, black.y, black.bri};
}

float PhilipsHueLight::crossProduct(CiColor p1, CiColor p2) {
	return p1.x * p2.y - p1.y * p2.x;
}

bool PhilipsHueLight::isPointInLampsReach(CiColor p) {
	CiColor v1 = { colorSpace.green.x - colorSpace.red.x, colorSpace.green.y - colorSpace.red.y };
	CiColor v2 = { colorSpace.blue.x - colorSpace.red.x, colorSpace.blue.y - colorSpace.red.y };
	CiColor q = { p.x - colorSpace.red.x, p.y - colorSpace.red.y };
	float s = crossProduct(q, v2) / crossProduct(v1, v2);
	float t = crossProduct(v1, q) / crossProduct(v1, v2);
	if ((s >= 0.0f) && (t >= 0.0f) && (s + t <= 1.0f)) {
		return true;
	}
	return false;
}

CiColor PhilipsHueLight::getClosestPointToPoint(CiColor a, CiColor b, CiColor p) {
	CiColor AP = { p.x - a.x, p.y - a.y };
	CiColor AB = { b.x - a.x, b.y - a.y };
	float ab2 = AB.x * AB.x + AB.y * AB.y;
	float ap_ab = AP.x * AB.x + AP.y * AB.y;
	float t = ap_ab / ab2;
	if (t < 0.0f) {
		t = 0.0f;
	} else if (t > 1.0f) {
		t = 1.0f;
	}
	return {a.x + AB.x * t, a.y + AB.y * t};
}

float PhilipsHueLight::getDistanceBetweenTwoPoints(CiColor p1, CiColor p2) {
	// Horizontal difference.
	float dx = p1.x - p2.x;
	// Vertical difference.
	float dy = p1.y - p2.y;
	// Absolute value.
	return sqrt(dx * dx + dy * dy);
}

CiColor PhilipsHueLight::rgbToCiColor(float red, float green, float blue) {
	// Apply gamma correction.
	float r = (red > 0.04045f) ? powf((red + 0.055f) / (1.0f + 0.055f), 2.4f) : (red / 12.92f);
	float g = (green > 0.04045f) ? powf((green + 0.055f) / (1.0f + 0.055f), 2.4f) : (green / 12.92f);
	float b = (blue > 0.04045f) ? powf((blue + 0.055f) / (1.0f + 0.055f), 2.4f) : (blue / 12.92f);
	// Convert to XYZ space.
	float X = r * 0.649926f + g * 0.103455f + b * 0.197109f;
	float Y = r * 0.234327f + g * 0.743075f + b * 0.022598f;
	float Z = r * 0.0000000f + g * 0.053077f + b * 1.035763f;
	// Convert to x,y space.
	float cx = X / (X + Y + Z);
	float cy = Y / (X + Y + Z);
	if (isnan(cx)) {
		cx = 0.0f;
	}
	if (isnan(cy)) {
		cy = 0.0f;
	}
	// Brightness is simply Y in the XYZ space.
	CiColor xy = { cx, cy, Y };
	// Check if the given XY value is within the color reach of our lamps.
	if (!isPointInLampsReach(xy)) {
		// It seems the color is out of reach let's find the closes color we can produce with our lamp and send this XY value out.
		CiColor pAB = getClosestPointToPoint(colorSpace.red, colorSpace.green, xy);
		CiColor pAC = getClosestPointToPoint(colorSpace.blue, colorSpace.red, xy);
		CiColor pBC = getClosestPointToPoint(colorSpace.green, colorSpace.blue, xy);
		// Get the distances per point and see which point is closer to our Point.
		float dAB = getDistanceBetweenTwoPoints(xy, pAB);
		float dAC = getDistanceBetweenTwoPoints(xy, pAC);
		float dBC = getDistanceBetweenTwoPoints(xy, pBC);
		float lowest = dAB;
		CiColor closestPoint = pAB;
		if (dAC < lowest) {
			lowest = dAC;
			closestPoint = pAC;
		}
		if (dBC < lowest) {
			lowest = dBC;
			closestPoint = pBC;
		}
		// Change the xy value to a value which is within the reach of the lamp.
		xy.x = closestPoint.x;
		xy.y = closestPoint.y;
	}
	return xy;
}

LedDevicePhilipsHue::LedDevicePhilipsHue(const std::string& output, const std::string& username, bool switchOffOnBlack,
		int transitiontime, std::vector<unsigned int> lightIds) :
		host(output.c_str()), username(username.c_str()), switchOffOnBlack(switchOffOnBlack), transitiontime(
				transitiontime), lightIds(lightIds) {
	http = new QHttp(host);
	timer.setInterval(3000);
	timer.setSingleShot(true);
	connect(&timer, SIGNAL(timeout()), this, SLOT(restoreStates()));
}

LedDevicePhilipsHue::~LedDevicePhilipsHue() {
	delete http;
}

int LedDevicePhilipsHue::write(const std::vector<ColorRgb> & ledValues) {
	// Save light states if not done before.
	if (!areStatesSaved()) {
		saveStates((unsigned int) ledValues.size());
		switchOn((unsigned int) ledValues.size());
	}
	// If there are less states saved than colors given, then maybe something went wrong before.
	if (lights.size() != ledValues.size()) {
		restoreStates();
		return 0;
	}
	// Iterate through colors and set light states.
	unsigned int idx = 0;
	for (const ColorRgb& color : ledValues) {
		// Get lamp.
		PhilipsHueLight& lamp = lights.at(idx);
		// Scale colors from [0, 255] to [0, 1] and convert to xy space.
		CiColor xy = lamp.rgbToCiColor(color.red / 255.0f, color.green / 255.0f, color.blue / 255.0f);
		// Write color if color has been changed.
		if (xy != lamp.color) {
			// Switch on if the lamp has been previously switched off.
			if (switchOffOnBlack && lamp.color == lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": true}"));
			}
			// Send adjust color and brightness command in JSON format.
			// We have to set the transition time each time.
			put(getStateRoute(lamp.id),
					QString("{\"xy\": [%1, %2], \"bri\": %3, \"transitiontime\": %4}").arg(xy.x).arg(xy.y).arg(
							qRound(xy.bri * 255.0f)).arg(transitiontime));

		}
		// Switch lamp off if switchOffOnBlack is enabled and the lamp is currently on.
		if (switchOffOnBlack) {
			// From black to a color.
			if (lamp.color == lamp.black && xy != lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": true}"));
			}
			// From a color to black.
			else if (lamp.color != lamp.black && xy == lamp.black) {
				put(getStateRoute(lamp.id), QString("{\"on\": false}"));
			}
		}
		// Remember last color.
		lamp.color = xy;
		// Next light id.
		idx++;
	}
	timer.start();
	return 0;
}

int LedDevicePhilipsHue::switchOff() {
	timer.stop();
	// If light states have been saved before, ...
	if (areStatesSaved()) {
		// ... restore them.
		restoreStates();
	}
	return 0;
}

void LedDevicePhilipsHue::put(QString route, QString content) {
	QString url = QString("/api/%1/%2").arg(username).arg(route);
	QHttpRequestHeader header("PUT", url);
	header.setValue("Host", host);
	header.setValue("Accept-Encoding", "identity");
	header.setValue("Connection", "keep-alive");
	header.setValue("Content-Length", QString("%1").arg(content.size()));
	QEventLoop loop;
	// Connect requestFinished signal to quit slot of the loop.
	loop.connect(http, SIGNAL(requestFinished(int, bool)), SLOT(quit()));
	// Perfrom request
	http->request(header, content.toAscii());
	// Go into the loop until the request is finished.
	loop.exec();
}

QByteArray LedDevicePhilipsHue::get(QString route) {
	QString url = QString("/api/%1/%2").arg(username).arg(route);
	// Event loop to block until request finished.
	QEventLoop loop;
	// Connect requestFinished signal to quit slot of the loop.
	loop.connect(http, SIGNAL(requestFinished(int, bool)), SLOT(quit()));
	// Perfrom request
	http->get(url);
	// Go into the loop until the request is finished.
	loop.exec();
	// Read all data of the response.
	return http->readAll();
}

QString LedDevicePhilipsHue::getStateRoute(unsigned int lightId) {
	return QString("lights/%1/state").arg(lightId);
}

QString LedDevicePhilipsHue::getRoute(unsigned int lightId) {
	return QString("lights/%1").arg(lightId);
}

void LedDevicePhilipsHue::saveStates(unsigned int nLights) {
	// Clear saved lamps.
	lights.clear();
	// Use json parser to parse reponse.
	Json::Reader reader;
	Json::FastWriter writer;
	// Create light ids if none supplied by the user.
	if (lightIds.size() != nLights) {
		lightIds.clear();
		for (unsigned int i = 0; i < nLights; i++) {
			lightIds.push_back(i + 1);
		}
	}
	// Iterate lights.
	for (unsigned int i = 0; i < nLights; i++) {
		// Read the response.
		QByteArray response = get(getRoute(lightIds.at(i)));
		// Parse JSON.
		Json::Value json;
		if (!reader.parse(QString(response).toStdString(), json)) {
			// Error occured, break loop.
			break;
		}
		// Get state object values which are subject to change.
		Json::Value state(Json::objectValue);
		state["on"] = json["state"]["on"];
		if (json["state"]["on"] == true) {
			state["xy"] = json["state"]["xy"];
			state["bri"] = json["state"]["bri"];
		}
		// Determine the model id.
		QString modelId = QString(writer.write(json["modelid"]).c_str()).trimmed().replace("\"", "");
		QString originalState = QString(writer.write(state).c_str()).trimmed();
		// Save state object.
		lights.push_back(PhilipsHueLight(lightIds.at(i), originalState, modelId));
	}
}

void LedDevicePhilipsHue::switchOn(unsigned int nLights) {
	for (PhilipsHueLight light : lights) {
		put(getStateRoute(light.id), "{\"on\": true}");
	}
}

void LedDevicePhilipsHue::restoreStates() {
	for (PhilipsHueLight light : lights) {
		put(getStateRoute(light.id), light.originalState);
	}
	// Clear saved light states.
	lights.clear();
}

bool LedDevicePhilipsHue::areStatesSaved() {
	return !lights.empty();
}
<|endoftext|>


<commit_before><commit_msg>coverity#735757 Unchecked dynamic_cast<commit_after><|endoftext|>


<commit_before>//
// Copyright (C) 2007 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2007 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// Author: Alexander Chemeris <Alexander DOT Chemeris AT SIPez DOT com>

#include <cppunit/extensions/HelperMacros.h>
#include <cppunit/TestCase.h>
#include <sipxunit/TestUtilities.h>

#include <mp/MpOutputDeviceManager.h>
#include <mp/MpAudioBuf.h>
#include <mp/MpSineWaveGeneratorDeviceDriver.h>
#include <os/OsTask.h>
#include <os/OsEvent.h>

#define TEST_SAMPLES_PER_FRAME_SIZE   80
#define BUFFER_NUM                    500
#define TEST_SAMPLES_PER_SECOND       8000
#define TEST_SAMPLE_DATA_SIZE         (TEST_SAMPLES_PER_SECOND*1)
#define TEST_SAMPLE_DATA_MAGNITUDE    32000
#define TEST_SAMPLE_DATA_PERIOD       (1000000/60) //in microseconds 60 Hz

#define CREATE_TEST_RUNS_NUMBER              3
#define ENABLE_DISABLE_TEST_RUNS_NUMBER      5
#define ENABLE_DISABLE_FAST_TEST_RUNS_NUMBER 10
#define DIRECT_WRITE_TEST_RUNS_NUMBER        3
#define TICKER_TEST_WRITE_RUNS_NUMBER        3

#undef USE_TEST_DRIVER

#ifdef USE_TEST_DRIVER // USE_TEST_DRIVER [
#include <mp/MpodBufferRecorder.h>
#define OUTPUT_DRIVER MpodBufferRecorder
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS "default", TEST_SAMPLE_DATA_SIZE*1000/TEST_SAMPLES_PER_SECOND

#elif defined(WIN32) // USE_TEST_DRIVER ][ WIN32
#include <mp/MpodWinMM.h>
#define OUTPUT_DRIVER MpodWinMM
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS MpodWinMM::getDefaultDeviceName()

#elif defined(__pingtel_on_posix__) // WIN32 ][ __pingtel_on_posix__
#include <mp/MpodOSS.h>
#define OUTPUT_DRIVER MpodOSS
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS "/dev/dsp"

#else // __pingtel_on_posix__ ]
#error Unknown platform!
#endif

//static MpAudioSample sampleData[TEST_SAMPLE_DATA_SIZE];
//static UtlBoolean sampleDataInitialized=FALSE;

static void calculateSampleData(int frequency,
                                MpAudioSample sampleData[],
                                int samplesPerFrame,
                                int samplesPerSecond)
{
   for (int i=0; i<TEST_SAMPLE_DATA_SIZE; i++)
   {
      sampleData[i] =
         MpSineWaveGeneratorDeviceDriver::calculateSample(0,
                                                          TEST_SAMPLE_DATA_MAGNITUDE,
                                                          1000000 / frequency,
                                                          i,
                                                          samplesPerFrame,
                                                          samplesPerSecond);
   }
}

/**
 * Unittest for MpOutputDeviceDriver
 */
class MpOutputDeviceDriverTest : public CppUnit::TestCase
{
   CPPUNIT_TEST_SUITE(MpOutputDeviceDriverTest);
   CPPUNIT_TEST(testCreate);
   CPPUNIT_TEST(testEnableDisable);
   CPPUNIT_TEST(testEnableDisableFast);
   CPPUNIT_TEST(testDirectWrite);
   CPPUNIT_TEST(testTickerNotification);
   CPPUNIT_TEST_SUITE_END();

public:

   void setUp()
   {
      // Create pool for data buffers
      mpPool = new MpBufPool(TEST_SAMPLES_PER_FRAME_SIZE * sizeof(MpAudioSample)
                             + MpArrayBuf::getHeaderSize(), BUFFER_NUM);
      CPPUNIT_ASSERT(mpPool != NULL);

      // Create pool for buffer headers
      mpHeadersPool = new MpBufPool(sizeof(MpAudioBuf), BUFFER_NUM);
      CPPUNIT_ASSERT(mpHeadersPool != NULL);

      // Set mpHeadersPool as default pool for audio and data pools.
      MpAudioBuf::smpDefaultPool = mpHeadersPool;
      MpDataBuf::smpDefaultPool = mpHeadersPool;
   }

   void tearDown()
   {
      if (mpPool != NULL)
      {
         delete mpPool;
      }
      if (mpHeadersPool != NULL)
      {
         delete mpHeadersPool;
      }
   }

   void testCreate()
   {
      for (int i=0; i<CREATE_TEST_RUNS_NUMBER; i++)
      {
         OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);

         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testEnableDisable()
   {
      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<ENABLE_DISABLE_TEST_RUNS_NUMBER; i++)
      {
         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         OsTask::delay(50);

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testEnableDisableFast()
   {
      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<ENABLE_DISABLE_FAST_TEST_RUNS_NUMBER; i++)
      {
         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testDirectWrite()
   {
      MpAudioSample sampleData[TEST_SAMPLE_DATA_SIZE];
      calculateSampleData(440, sampleData, 80, 8000);

      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<DIRECT_WRITE_TEST_RUNS_NUMBER; i++)
      {
         MpFrameTime frameTime=0;

         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         // Write some data to device.
         for (int frame=0; frame<TEST_SAMPLE_DATA_SIZE/TEST_SAMPLES_PER_FRAME_SIZE; frame++)
         {
            OsTask::delay(1000*TEST_SAMPLES_PER_FRAME_SIZE/TEST_SAMPLES_PER_SECOND);
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS,
                                 driver.pushFrame(TEST_SAMPLES_PER_FRAME_SIZE,
                                                  sampleData + TEST_SAMPLES_PER_FRAME_SIZE*frame,
                                                  frameTime));

            frameTime += TEST_SAMPLES_PER_FRAME_SIZE*1000/TEST_SAMPLES_PER_SECOND;
         }

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testTickerNotification()
   {
      OsEvent notificationEvent;
      int sampleRates[]={8000, 16000, 32000, 48000};
      int numRates = 4;
      int frequencies[] = {1000, 2000, 4000, 8000, 16000, 20000};
      int numFreqs = 6;

      int rateIndex = 0;
      MpAudioSample* sampleData = new MpAudioSample[sampleRates[rateIndex]];

      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<numFreqs; i++)
      {
         printf("Frequency: %d (Hz) Sample rate: %d/sec.\n",
            frequencies[i], sampleRates[rateIndex]);
         calculateSampleData(frequencies[i], sampleData, sampleRates[rateIndex]/100, sampleRates[rateIndex]);
         MpFrameTime frameTime=0;

         driver.enableDevice(sampleRates[rateIndex]/100, sampleRates[rateIndex], 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         CPPUNIT_ASSERT_EQUAL(OS_SUCCESS, driver.setTickerNotification(&notificationEvent));

         // Write some data to device.
         for (int frame=0; frame<100; frame++)
         {
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS, notificationEvent.wait(OsTime(1000)));
            notificationEvent.reset();
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS,
                                 driver.pushFrame(sampleRates[rateIndex]/100,
                                                  sampleData + sampleRates[rateIndex]/100*frame,
                                                  frameTime));

            frameTime += sampleRates[rateIndex]/100*1000/sampleRates[rateIndex];
         }

         CPPUNIT_ASSERT(driver.setTickerNotification(NULL) == OS_SUCCESS);

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
      delete[] sampleData;
   }

protected:
   MpBufPool *mpPool;         ///< Pool for data buffers
   MpBufPool *mpHeadersPool;  ///< Pool for buffers headers

};

CPPUNIT_TEST_SUITE_REGISTRATION(MpOutputDeviceDriverTest);
<commit_msg>Fix calculateSampleDataSize in MpOutputDriverTest to take the size of the buffer passed in, instead of assuming the size is TEST_SAMPLE_DATA_SIZE.<commit_after>//
// Copyright (C) 2007 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2007 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// Author: Alexander Chemeris <Alexander DOT Chemeris AT SIPez DOT com>

#include <cppunit/extensions/HelperMacros.h>
#include <cppunit/TestCase.h>
#include <sipxunit/TestUtilities.h>

#include <mp/MpOutputDeviceManager.h>
#include <mp/MpAudioBuf.h>
#include <mp/MpSineWaveGeneratorDeviceDriver.h>
#include <os/OsTask.h>
#include <os/OsEvent.h>

#define TEST_SAMPLES_PER_FRAME_SIZE   80
#define BUFFER_NUM                    500
#define TEST_SAMPLES_PER_SECOND       8000
#define TEST_SAMPLE_DATA_SIZE         (TEST_SAMPLES_PER_SECOND*1)
#define TEST_SAMPLE_DATA_MAGNITUDE    32000
#define TEST_SAMPLE_DATA_PERIOD       (1000000/60) //in microseconds 60 Hz

#define CREATE_TEST_RUNS_NUMBER              3
#define ENABLE_DISABLE_TEST_RUNS_NUMBER      5
#define ENABLE_DISABLE_FAST_TEST_RUNS_NUMBER 10
#define DIRECT_WRITE_TEST_RUNS_NUMBER        3
#define TICKER_TEST_WRITE_RUNS_NUMBER        3

#undef USE_TEST_DRIVER

#ifdef USE_TEST_DRIVER // USE_TEST_DRIVER [
#include <mp/MpodBufferRecorder.h>
#define OUTPUT_DRIVER MpodBufferRecorder
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS "default", TEST_SAMPLE_DATA_SIZE*1000/TEST_SAMPLES_PER_SECOND

#elif defined(WIN32) // USE_TEST_DRIVER ][ WIN32
#include <mp/MpodWinMM.h>
#define OUTPUT_DRIVER MpodWinMM
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS MpodWinMM::getDefaultDeviceName()

#elif defined(__pingtel_on_posix__) // WIN32 ][ __pingtel_on_posix__
#include <mp/MpodOSS.h>
#define OUTPUT_DRIVER MpodOSS
#define OUTPUT_DRIVER_CONSTRUCTOR_PARAMS "/dev/dsp"

#else // __pingtel_on_posix__ ]
#error Unknown platform!
#endif

//static MpAudioSample sampleData[TEST_SAMPLE_DATA_SIZE];
//static UtlBoolean sampleDataInitialized=FALSE;

static void calculateSampleData(int frequency,
                                MpAudioSample sampleData[],
                                int sampleDataSz,
                                int samplesPerFrame,
                                int samplesPerSecond)
{
   for (int i=0; i<sampleDataSz; i++)
   {
      sampleData[i] =
         MpSineWaveGeneratorDeviceDriver::calculateSample(0,
                                                          TEST_SAMPLE_DATA_MAGNITUDE,
                                                          1000000 / frequency,
                                                          i,
                                                          samplesPerFrame,
                                                          samplesPerSecond);
   }
}

/**
 * Unittest for MpOutputDeviceDriver
 */
class MpOutputDeviceDriverTest : public CppUnit::TestCase
{
   CPPUNIT_TEST_SUITE(MpOutputDeviceDriverTest);
   CPPUNIT_TEST(testCreate);
   CPPUNIT_TEST(testEnableDisable);
   CPPUNIT_TEST(testEnableDisableFast);
   CPPUNIT_TEST(testDirectWrite);
   CPPUNIT_TEST(testTickerNotification);
   CPPUNIT_TEST_SUITE_END();

public:

   void setUp()
   {
      // Create pool for data buffers
      mpPool = new MpBufPool(TEST_SAMPLES_PER_FRAME_SIZE * sizeof(MpAudioSample)
                             + MpArrayBuf::getHeaderSize(), BUFFER_NUM);
      CPPUNIT_ASSERT(mpPool != NULL);

      // Create pool for buffer headers
      mpHeadersPool = new MpBufPool(sizeof(MpAudioBuf), BUFFER_NUM);
      CPPUNIT_ASSERT(mpHeadersPool != NULL);

      // Set mpHeadersPool as default pool for audio and data pools.
      MpAudioBuf::smpDefaultPool = mpHeadersPool;
      MpDataBuf::smpDefaultPool = mpHeadersPool;
   }

   void tearDown()
   {
      if (mpPool != NULL)
      {
         delete mpPool;
      }
      if (mpHeadersPool != NULL)
      {
         delete mpHeadersPool;
      }
   }

   void testCreate()
   {
      for (int i=0; i<CREATE_TEST_RUNS_NUMBER; i++)
      {
         OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);

         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testEnableDisable()
   {
      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<ENABLE_DISABLE_TEST_RUNS_NUMBER; i++)
      {
         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         OsTask::delay(50);

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testEnableDisableFast()
   {
      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<ENABLE_DISABLE_FAST_TEST_RUNS_NUMBER; i++)
      {
         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testDirectWrite()
   {
      MpAudioSample sampleData[TEST_SAMPLE_DATA_SIZE];
      calculateSampleData(440, sampleData, TEST_SAMPLE_DATA_SIZE, 80, 8000);

      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<DIRECT_WRITE_TEST_RUNS_NUMBER; i++)
      {
         MpFrameTime frameTime=0;

         driver.enableDevice(TEST_SAMPLES_PER_FRAME_SIZE, TEST_SAMPLES_PER_SECOND, 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         // Write some data to device.
         for (int frame=0; frame<TEST_SAMPLE_DATA_SIZE/TEST_SAMPLES_PER_FRAME_SIZE; frame++)
         {
            OsTask::delay(1000*TEST_SAMPLES_PER_FRAME_SIZE/TEST_SAMPLES_PER_SECOND);
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS,
                                 driver.pushFrame(TEST_SAMPLES_PER_FRAME_SIZE,
                                                  sampleData + TEST_SAMPLES_PER_FRAME_SIZE*frame,
                                                  frameTime));

            frameTime += TEST_SAMPLES_PER_FRAME_SIZE*1000/TEST_SAMPLES_PER_SECOND;
         }

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
   }

   void testTickerNotification()
   {
      OsEvent notificationEvent;
      int sampleRates[]={8000, 16000, 32000, 48000};
      int numRates = sizeof(sampleRates)/sizeof(int);
      int frequencies[] = {1000, 2000, 4000, 8000, 16000, 20000, 24000, 28000};
      int numFreqs = sizeof(frequencies)/sizeof(int);

      int rateIndex = 0;
      int sampleDataSz = sampleRates[rateIndex];
      MpAudioSample* sampleData = new MpAudioSample[sampleDataSz];

      OUTPUT_DRIVER driver(OUTPUT_DRIVER_CONSTRUCTOR_PARAMS);
      CPPUNIT_ASSERT(!driver.isEnabled());

      for (int i=0; i<numFreqs; i++)
      {
         printf("Frequency: %d (Hz) Sample rate: %d/sec.\n",
            frequencies[i], sampleRates[rateIndex]);
         calculateSampleData(frequencies[i], sampleData, sampleDataSz,
                             sampleRates[rateIndex]/100, sampleRates[rateIndex]);
         MpFrameTime frameTime=0;

         driver.enableDevice(sampleRates[rateIndex]/100, sampleRates[rateIndex], 0);
         CPPUNIT_ASSERT(driver.isEnabled());

         CPPUNIT_ASSERT_EQUAL(OS_SUCCESS, driver.setTickerNotification(&notificationEvent));

         // Write some data to device.
         for (int frame=0; frame<100; frame++)
         {
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS, notificationEvent.wait(OsTime(1000)));
            notificationEvent.reset();
            CPPUNIT_ASSERT_EQUAL(OS_SUCCESS,
                                 driver.pushFrame(sampleRates[rateIndex]/100,
                                                  sampleData + sampleRates[rateIndex]/100*frame,
                                                  frameTime));

            frameTime += sampleRates[rateIndex]/100*1000/sampleRates[rateIndex];
         }

         CPPUNIT_ASSERT(driver.setTickerNotification(NULL) == OS_SUCCESS);

         driver.disableDevice();
         CPPUNIT_ASSERT(!driver.isEnabled());
      }
      delete[] sampleData;
   }

protected:
   MpBufPool *mpPool;         ///< Pool for data buffers
   MpBufPool *mpHeadersPool;  ///< Pool for buffers headers

};

CPPUNIT_TEST_SUITE_REGISTRATION(MpOutputDeviceDriverTest);
<|endoftext|>


<commit_before>#include "drake/RigidBodyManipulator.h"
#include "drake/Polynomial.h"
#include "drake/PiecewisePolynomial.h"
#include "control/ExponentialForm.hpp"
#include "drake/QPCommon.h"
#include "drake/lcmt_qp_controller_input.hpp"
#include <lcm/lcm-cpp.hpp>

enum SupportLogicType {
  REQUIRE_SUPPORT, ONLY_IF_FORCE_SENSED, ONLY_IF_KINEMATIC, KINEMATIC_OR_SENSED, PREVENT_SUPPORT
};

#define QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT 4

enum FootID {RIGHT, LEFT};

std::map<FootID, std::string> footIDToName = {std::make_pair(RIGHT, "right"),
                                              std::make_pair(LEFT, "left")};

std::map<std::string, FootID> footNameToID = {std::make_pair("right", RIGHT),
                                              std::make_pair("left", LEFT)};

std::map<FootID, FootID> otherFoot = {std::make_pair(RIGHT, LEFT),
                                      std::make_pair(LEFT, RIGHT)};

struct BangBangIntercept {
  double tf;
  double tswitch;
  double u;
};

typedef Matrix<double, 7, 1> XYZQuat;

struct FootState {
  Isometry3d pose;
  XYZQuat velocity;
  bool contact;
  double terrain_height;
  Vector3d terrain_normal;
};

struct InterceptPlan {
  double tf;
  double tswitch;
  Isometry3d pose_next;
  Isometry3d icp_next;
  Isometry3d cop;
  FootID swing_foot;
  FootID stance_foot;
  Isometry3d stance_pose;
  double error;
};

typedef std::map<FootID, FootState> FootStateMap;
typedef std::map<FootID, Matrix<double, 3, 4>> VertMap;

struct BipedDescription {
  std::map<FootID, Matrix<double, 3, 4>> reachable_vertices;
  std::map<FootID, Matrix<double, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT>> foot_vertices;
  double u_max; // foot acceleration bounds used for computing ICP intercepts
  double omega; // characteristic frequency of the linear inverted pendulum system. Defined as sqrt(g / height)
};

BipedDescription getAtlasDefaults() {
  BipedDescription biped;
  biped.reachable_vertices[RIGHT] << -0.35, 0.35, 0.35, -0.35,
                                    -0.2, -0.2, -0.45, -0.45,
                                    0, 0, 0, 0;
  biped.reachable_vertices[LEFT] << -0.35, 0.35, 0.35, -0.35,
                                    0.2, 0.2, 0.45, 0.45,
                                    0, 0, 0, 0;
  biped.foot_vertices[RIGHT] << -0.12, 0.12, 0.12, -0.12,
                               -0.06, -0.06, 0.06, 0.06,
                               0, 0, 0, 0;
  biped.foot_vertices[LEFT] << -0.12, 0.12, 0.12, -0.12,
                              -0.06, -0.06, 0.06, 0.06,
                              0, 0, 0, 0;
  biped.omega = sqrt(9.81 / 1.098);
  biped.u_max = 20;
  return biped;
}

class QPReactiveRecoveryPlan {

  private:
    std::unique_ptr<PiecewisePolynomial<double>> last_swing_plan;
    InterceptPlan last_intercept_plan;
    double t_start = 0;
    bool initialized = false;
    BipedDescription biped;
    std::map<FootID, int> foot_frame_ids;
    std::map<FootID, int> foot_body_ids;
    VectorXd q_des;
    MatrixXd S;
    RobotPropertyCache robot_property_cache;

    void findFootSoleFrames();
    FootStateMap getFootStates(const VectorXd &v, const std::vector<bool>& contact_force_detected);
    void getCaptureInput(double t_global, const FootStateMap &foot_states, const Isometry3d &icp, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    void getInterceptInput(double t_global, const FootStateMap &foot_states, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    std::shared_ptr<lcm::LCM> LCMHandle;
    void initLCM();
    void publishForVisualization(double t_global, const Isometry3d &icp);
    void setupQPInputDefaults(double t_global, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    void encodeSupportData(const int body_id, const FootState &foot_state, const SupportLogicType &support_logic, drake::lcmt_support_data &support_data);
    void encodeBodyMotionData(int body_or_frame_id, PiecewisePolynomial<double> spline, drake::lcmt_body_motion_data &body_motion);
    std::unique_ptr<PiecewisePolynomial<double>> straightToGoalTrajectory(double t_global, const InterceptPlan &intercept_plan, const std::map<FootID, FootState> &foot_states);
    std::unique_ptr<PiecewisePolynomial<double>> upOverAndDownTrajectory(double t_global, const InterceptPlan &intercept_plan, const FootStateMap &foot_states);
    Matrix3Xd heelToeContacts(int body_id);

	public:
    RigidBodyManipulator* robot;
    double capture_max_flyfoot_height = 0.025;
    double capture_shrink_factor = 0.8;
    double desired_icp_offset = 0.1;
    double min_step_duration = 0.4;
    double foot_hull_cop_shrink_factor = 0.5;
    double max_considerable_foot_swing = 0.15;
    double post_execution_delay = 0.4;
    double mu = 0.5;
    double pelvis_height_above_sole = 0.84;
    double swing_height_above_terrain = 0.03;

    QPReactiveRecoveryPlan(RigidBodyManipulator *robot, const RobotPropertyCache &rpc);
    QPReactiveRecoveryPlan(RigidBodyManipulator *robot, const RobotPropertyCache &rpc, BipedDescription biped);

		static VectorXd closestPointInConvexHull(const Ref<const VectorXd> &x, const Ref<const MatrixXd> &V);

    static Isometry3d closestPoseInConvexHull(const Isometry3d &pose, const Ref<const MatrixXd> &V);

    static std::vector<double> expIntercept(const ExponentialForm &expform, double l0, double ld0, double u, int degree);

    // a polynomial representing position as a function of time subject to initial position x0, initial velocity xd0, final velocity of 0, and acceleration of +u followed by acceleration of -u.
    static Polynomial<double> bangBangPolynomial(double x0, double xd0, double u);

    // bang-bang policy intercepts from initial state [x0, xd0] to final state [xf, 0] at max acceleration u_max
    static std::vector<BangBangIntercept> bangBangIntercept(double x0, double xd0, double xf, double u_max);

    static Isometry3d getTWorldToLocal(const Isometry3d &icp, const Isometry3d &cop);

    static double getMinTimeToXprimeAxis(const FootState foot_state, const BipedDescription &biped, Isometry3d &T_world_to_local);

    static ExponentialForm icpTrajectory(double x_ic, double x_cop, double omega);

    static std::unique_ptr<PiecewisePolynomial<double>> freeKnotTimesSpline(double t0, double tf, const Ref<const MatrixXd> &xs, const Ref<const VectorXd> xd0, const Ref<const VectorXd> xdf);

    std::unique_ptr<PiecewisePolynomial<double>> swingTrajectory(double t_global, const InterceptPlan &intercept_plan, const std::map<FootID, FootState> &foot_states);

    void resetInitialization();

    double icpError(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices);

    bool isICPCaptured(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices);

    std::vector<InterceptPlan> getInterceptsWithCoP(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp, const Isometry3d &cop);

    std::vector<InterceptPlan> getInterceptPlansForFoot(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp);

    std::vector<InterceptPlan> getInterceptPlans(const std::map<FootID, FootState> &foot_states, const Isometry3d &icp);

    void publishQPControllerInput(double t_global, const VectorXd &q, const VectorXd &v, const std::vector<bool>& contact_force_detected);

    Vector2d getICP(const VectorXd &v);

    void setS(const MatrixXd &S) {
      this->S = S;
    }

    void setRobot(RigidBodyManipulator *robot);

    void setQDes(const Ref<const VectorXd> &q_des) {
      this->q_des = q_des;
    }
};

<commit_msg>more liberal flyfoot height<commit_after>#include "drake/RigidBodyManipulator.h"
#include "drake/Polynomial.h"
#include "drake/PiecewisePolynomial.h"
#include "control/ExponentialForm.hpp"
#include "drake/QPCommon.h"
#include "drake/lcmt_qp_controller_input.hpp"
#include <lcm/lcm-cpp.hpp>

enum SupportLogicType {
  REQUIRE_SUPPORT, ONLY_IF_FORCE_SENSED, ONLY_IF_KINEMATIC, KINEMATIC_OR_SENSED, PREVENT_SUPPORT
};

#define QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT 4

enum FootID {RIGHT, LEFT};

std::map<FootID, std::string> footIDToName = {std::make_pair(RIGHT, "right"),
                                              std::make_pair(LEFT, "left")};

std::map<std::string, FootID> footNameToID = {std::make_pair("right", RIGHT),
                                              std::make_pair("left", LEFT)};

std::map<FootID, FootID> otherFoot = {std::make_pair(RIGHT, LEFT),
                                      std::make_pair(LEFT, RIGHT)};

struct BangBangIntercept {
  double tf;
  double tswitch;
  double u;
};

typedef Matrix<double, 7, 1> XYZQuat;

struct FootState {
  Isometry3d pose;
  XYZQuat velocity;
  bool contact;
  double terrain_height;
  Vector3d terrain_normal;
};

struct InterceptPlan {
  double tf;
  double tswitch;
  Isometry3d pose_next;
  Isometry3d icp_next;
  Isometry3d cop;
  FootID swing_foot;
  FootID stance_foot;
  Isometry3d stance_pose;
  double error;
};

typedef std::map<FootID, FootState> FootStateMap;
typedef std::map<FootID, Matrix<double, 3, 4>> VertMap;

struct BipedDescription {
  std::map<FootID, Matrix<double, 3, 4>> reachable_vertices;
  std::map<FootID, Matrix<double, 3, QP_REACTIVE_RECOVERY_VERTICES_PER_FOOT>> foot_vertices;
  double u_max; // foot acceleration bounds used for computing ICP intercepts
  double omega; // characteristic frequency of the linear inverted pendulum system. Defined as sqrt(g / height)
};

BipedDescription getAtlasDefaults() {
  BipedDescription biped;
  biped.reachable_vertices[RIGHT] << -0.35, 0.35, 0.35, -0.35,
                                    -0.2, -0.2, -0.45, -0.45,
                                    0, 0, 0, 0;
  biped.reachable_vertices[LEFT] << -0.35, 0.35, 0.35, -0.35,
                                    0.2, 0.2, 0.45, 0.45,
                                    0, 0, 0, 0;
  biped.foot_vertices[RIGHT] << -0.12, 0.12, 0.12, -0.12,
                               -0.06, -0.06, 0.06, 0.06,
                               0, 0, 0, 0;
  biped.foot_vertices[LEFT] << -0.12, 0.12, 0.12, -0.12,
                              -0.06, -0.06, 0.06, 0.06,
                              0, 0, 0, 0;
  biped.omega = sqrt(9.81 / 1.098);
  biped.u_max = 20;
  return biped;
}

class QPReactiveRecoveryPlan {

  private:
    std::unique_ptr<PiecewisePolynomial<double>> last_swing_plan;
    InterceptPlan last_intercept_plan;
    double t_start = 0;
    bool initialized = false;
    BipedDescription biped;
    std::map<FootID, int> foot_frame_ids;
    std::map<FootID, int> foot_body_ids;
    VectorXd q_des;
    MatrixXd S;
    RobotPropertyCache robot_property_cache;

    void findFootSoleFrames();
    FootStateMap getFootStates(const VectorXd &v, const std::vector<bool>& contact_force_detected);
    void getCaptureInput(double t_global, const FootStateMap &foot_states, const Isometry3d &icp, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    void getInterceptInput(double t_global, const FootStateMap &foot_states, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    std::shared_ptr<lcm::LCM> LCMHandle;
    void initLCM();
    void publishForVisualization(double t_global, const Isometry3d &icp);
    void setupQPInputDefaults(double t_global, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input);
    void encodeSupportData(const int body_id, const FootState &foot_state, const SupportLogicType &support_logic, drake::lcmt_support_data &support_data);
    void encodeBodyMotionData(int body_or_frame_id, PiecewisePolynomial<double> spline, drake::lcmt_body_motion_data &body_motion);
    std::unique_ptr<PiecewisePolynomial<double>> straightToGoalTrajectory(double t_global, const InterceptPlan &intercept_plan, const std::map<FootID, FootState> &foot_states);
    std::unique_ptr<PiecewisePolynomial<double>> upOverAndDownTrajectory(double t_global, const InterceptPlan &intercept_plan, const FootStateMap &foot_states);
    Matrix3Xd heelToeContacts(int body_id);

	public:
    RigidBodyManipulator* robot;
    double capture_max_flyfoot_height = 0.05;
    double capture_shrink_factor = 0.8;
    double desired_icp_offset = 0.1;
    double min_step_duration = 0.4;
    double foot_hull_cop_shrink_factor = 0.5;
    double max_considerable_foot_swing = 0.15;
    double post_execution_delay = 0.4;
    double mu = 0.5;
    double pelvis_height_above_sole = 0.84;
    double swing_height_above_terrain = 0.03;

    QPReactiveRecoveryPlan(RigidBodyManipulator *robot, const RobotPropertyCache &rpc);
    QPReactiveRecoveryPlan(RigidBodyManipulator *robot, const RobotPropertyCache &rpc, BipedDescription biped);

		static VectorXd closestPointInConvexHull(const Ref<const VectorXd> &x, const Ref<const MatrixXd> &V);

    static Isometry3d closestPoseInConvexHull(const Isometry3d &pose, const Ref<const MatrixXd> &V);

    static std::vector<double> expIntercept(const ExponentialForm &expform, double l0, double ld0, double u, int degree);

    // a polynomial representing position as a function of time subject to initial position x0, initial velocity xd0, final velocity of 0, and acceleration of +u followed by acceleration of -u.
    static Polynomial<double> bangBangPolynomial(double x0, double xd0, double u);

    // bang-bang policy intercepts from initial state [x0, xd0] to final state [xf, 0] at max acceleration u_max
    static std::vector<BangBangIntercept> bangBangIntercept(double x0, double xd0, double xf, double u_max);

    static Isometry3d getTWorldToLocal(const Isometry3d &icp, const Isometry3d &cop);

    static double getMinTimeToXprimeAxis(const FootState foot_state, const BipedDescription &biped, Isometry3d &T_world_to_local);

    static ExponentialForm icpTrajectory(double x_ic, double x_cop, double omega);

    static std::unique_ptr<PiecewisePolynomial<double>> freeKnotTimesSpline(double t0, double tf, const Ref<const MatrixXd> &xs, const Ref<const VectorXd> xd0, const Ref<const VectorXd> xdf);

    std::unique_ptr<PiecewisePolynomial<double>> swingTrajectory(double t_global, const InterceptPlan &intercept_plan, const std::map<FootID, FootState> &foot_states);

    void resetInitialization();

    double icpError(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices);

    bool isICPCaptured(const Ref<const Vector2d> &r_ic, const FootStateMap &foot_states, const VertMap &foot_vertices);

    std::vector<InterceptPlan> getInterceptsWithCoP(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp, const Isometry3d &cop);

    std::vector<InterceptPlan> getInterceptPlansForFoot(const FootID &swing_foot, const std::map<FootID, FootState> &foot_states, const Isometry3d &icp);

    std::vector<InterceptPlan> getInterceptPlans(const std::map<FootID, FootState> &foot_states, const Isometry3d &icp);

    void publishQPControllerInput(double t_global, const VectorXd &q, const VectorXd &v, const std::vector<bool>& contact_force_detected);

    Vector2d getICP(const VectorXd &v);

    void setS(const MatrixXd &S) {
      this->S = S;
    }

    void setRobot(RigidBodyManipulator *robot);

    void setQDes(const Ref<const VectorXd> &q_des) {
      this->q_des = q_des;
    }
};

<|endoftext|>