<commit_before>// Copyright (c) 2014, Baidu.com, Inc. All Rights Reserved
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//
// Author: yanshiguang02@baidu.com

#include "logging.h"

#include <assert.h>
#include <boost/bind.hpp>
#include <queue>
#include <set>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <syscall.h>
#include <sys/time.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

#include "mutex.h"
#include "thread.h"
#include "timer.h"

namespace baidu {
namespace common {

int g_log_level = INFO;
int64_t g_log_size = 0;
int32_t g_log_count = 0;
FILE* g_log_file = stdout;
std::string g_log_file_name;
FILE* g_warning_file = NULL;
int64_t g_total_size_limit = 0;

std::queue<std::string> g_log_queue;
int64_t current_total_size = 0;

bool GetNewLog(bool append) {
    char buf[30];
    struct timeval tv;
    gettimeofday(&tv, NULL);
    const time_t seconds = tv.tv_sec;
    struct tm t;
    localtime_r(&seconds, &t);
    snprintf(buf, 30,
        "%02d-%02d.%02d:%02d:%02d.%06d",
        t.tm_mon + 1,
        t.tm_mday,
        t.tm_hour,
        t.tm_min,
        t.tm_sec,
        static_cast<int>(tv.tv_usec));
    std::string full_path(g_log_file_name + ".");
    full_path.append(buf);
    size_t idx = full_path.rfind('/');
    if (idx == std::string::npos) {
        idx = 0;
    } else {
        idx += 1;
    }
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(full_path.c_str(), mode);
    if (fp == NULL) {
        return false;
    }
    if (g_log_file != stdout) {
        fclose(g_log_file);
    }
    g_log_file = fp;
    remove(g_log_file_name.c_str());
    symlink(full_path.substr(idx).c_str(), g_log_file_name.c_str());
    g_log_queue.push(full_path);
    while ((g_log_count && static_cast<int64_t>(g_log_queue.size()) > g_log_count)
            || (g_total_size_limit && current_total_size > g_total_size_limit)) {
        std::string to_del = g_log_queue.front();
        struct stat sta;
        if (-1 == lstat(to_del.c_str(), &sta)) {
            return false;
        }
        remove(to_del.c_str());
        current_total_size -= sta.st_size;
        g_log_queue.pop();
    }
    return true;
}

void SetLogLevel(int level) {
    g_log_level = level;
}

class AsyncLogger {
public:
    AsyncLogger()
      : jobs_(&mu_), done_(&mu_), stopped_(false), size_(0) {
        thread_.Start(boost::bind(&AsyncLogger::AsyncWriter, this));
    }
    ~AsyncLogger() {
        stopped_ = true;
        {
            MutexLock lock(&mu_);
            jobs_.Signal();
        }
        thread_.Join();
        // close fd
    }
    void WriteLog(int log_level, const char* buffer, int32_t len) {
        std::string* log_str = new std::string(buffer, len);
        MutexLock lock(&mu_);
        buffer_queue_.push(make_pair(log_level, log_str));
        jobs_.Signal();
    }
    void AsyncWriter() {
        MutexLock lock(&mu_);
        while (1) {
            int loglen = 0;
            int wflen = 0;
            while (!buffer_queue_.empty()) {
                int log_level = buffer_queue_.front().first;
                std::string* str = buffer_queue_.front().second;
                buffer_queue_.pop();
                if (g_log_file != stdout && g_log_size && str &&
                        static_cast<int64_t>(size_ + str->length()) > g_log_size) {
                    current_total_size += static_cast<int64_t>(size_ + str->length());
                    mu_.Unlock();
                    GetNewLog(false);
                    mu_.Lock();
                    size_ = 0;
                }
                mu_.Unlock();
                if (str && !str->empty()) {
                    fwrite(str->data(), 1, str->size(), g_log_file);
                    loglen += str->size();
                    if (g_warning_file && log_level >= 8) {
                        fwrite(str->data(), 1, str->size(), g_warning_file);
                        wflen += str->size();
                    }
                    if (g_log_size) size_ += str->length();
                }
                delete str;
                mu_.Lock();
            }
            if (loglen) fflush(g_log_file);
            if (wflen) fflush(g_warning_file);
            if (stopped_) {
                break;
            }
            done_.Broadcast();
            jobs_.Wait();
        }
    }
    void Flush() {
        MutexLock lock(&mu_);
        buffer_queue_.push(std::make_pair(0, reinterpret_cast<std::string*>(NULL)));
        jobs_.Signal();
        done_.Wait();
    }
private:
    Mutex mu_;
    CondVar jobs_;
    CondVar done_;
    bool stopped_;
    int64_t size_;
    Thread thread_;
    std::queue<std::pair<int, std::string*> > buffer_queue_;
};

AsyncLogger g_logger;

bool SetWarningFile(const char* path, bool append) {
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(path, mode);
    if (fp == NULL) {
        return false;
    }
    if (g_warning_file) {
        fclose(g_warning_file);
    }
    g_warning_file = fp;
    return true;
}

bool RecoverHistory(const char* path) {
    std::string log_path(path);
    size_t idx = log_path.rfind('/');
    std::string dir = "./";
    std::string log(path);
    if (idx != std::string::npos) {
        dir = log_path.substr(0, idx + 1);
        log = log_path.substr(idx + 1);
    }
    struct dirent *entry = NULL;
    DIR *dir_ptr = opendir(dir.c_str());
    if (dir_ptr == NULL) {
        return false;
    }
    std::vector<std::string> loglist;
    while (entry = readdir(dir_ptr)) {
        if (std::string(entry->d_name).find(log) != std::string::npos) {
            std::string file_name = dir + std::string(entry->d_name);
            struct stat sta;
            if (-1 == lstat(file_name.c_str(), &sta)) {
                return false;
            }
            if (S_ISREG(sta.st_mode)) {
                loglist.push_back(dir + std::string(entry->d_name));
                current_total_size += sta.st_size;
            }
        }
    }
    closedir(dir_ptr);
    std::sort(loglist.begin(), loglist.end());
    for (std::vector<std::string>::iterator it = loglist.begin(); it != loglist.end();
            ++it) {
        if (*it != g_log_queue.front()) {
            g_log_queue.push(*it);
        }
    }
    while ( (g_log_count && static_cast<int64_t>(g_log_queue.size()) > g_log_count)
            || (g_total_size_limit && current_total_size > g_total_size_limit)) {
        std::string to_del = g_log_queue.front();

        struct stat sta;
        if (-1 == lstat(to_del.c_str(), &sta)) break;
        current_total_size -= sta.st_size;

        remove(to_del.c_str());
        g_log_queue.pop();
    }
    return true;
}

bool SetLogFile(const char* path, bool append) {
    g_log_file_name.assign(path);
    return GetNewLog(append);
}

bool SetLogSize(int size) {
    if (size < 0) {
        return false;
    }
    g_log_size = static_cast<int64_t>(size) << 20;
    return true;
}

bool SetLogCount(int count) {
    if (count < 0 || g_total_size_limit != 0) {
        return false;
    }
    g_log_count = count;
    if (!RecoverHistory(g_log_file_name.c_str())) {
        return false;
    }
    return true;
}

bool SetLogSizeLimit(int size) {
    if (size < 0 || g_log_count != 0) {
        return false;
    }
    g_total_size_limit = static_cast<int64_t>(size) << 20;
    if (!RecoverHistory(g_log_file_name.c_str())) {
        return false;
    }
    return true;
}

void Logv(int log_level, const char* format, va_list ap) {
    static __thread uint64_t thread_id = 0;
    if (thread_id == 0) {
        thread_id = syscall(__NR_gettid);
    }

    // We try twice: the first time with a fixed-size stack allocated buffer,
    // and the second time with a much larger dynamically allocated buffer.
    char buffer[500];
    for (int iter = 0; iter < 2; iter++) {
        char* base;
        int bufsize;
        if (iter == 0) {
            bufsize = sizeof(buffer);
            base = buffer;
        } else {
            bufsize = 30000;
            base = new char[bufsize];
        }
        char* p = base;
        char* limit = base + bufsize;

        int32_t rlen = timer::now_time_str(p, limit - p);
        p += rlen;
        p += snprintf(p, limit - p, " %lld ", static_cast<long long unsigned int>(thread_id));

        // Print the message
        if (p < limit) {
            va_list backup_ap;
            va_copy(backup_ap, ap);
            p += vsnprintf(p, limit - p, format, backup_ap);
            va_end(backup_ap);
        }

        // Truncate to available space if necessary
        if (p >= limit) {
            if (iter == 0) {
                continue;       // Try again with larger buffer
            } else {
                p = limit - 1;
            }
        }

        // Add newline if necessary
        if (p == base || p[-1] != '\n') {
            *p++ = '\n';
        }

        assert(p <= limit);
        //fwrite(base, 1, p - base, g_log_file);
        //fflush(g_log_file);
        //if (g_warning_file && log_level >= 8) {
        //    fwrite(base, 1, p - base, g_warning_file);
        //    fflush(g_warning_file);
        //}
        g_logger.WriteLog(log_level, base, p - base);
        if (log_level == FATAL) {
            g_logger.Flush();
        }
        if (base != buffer) {
            delete[] base;
        }
        break;
    }
}

void Log(int level, const char* fmt, ...) {
    va_list ap;
    va_start(ap, fmt);

    if (level >= g_log_level) {
        Logv(level, fmt, ap);
    }
    va_end(ap);
    if (level == FATAL) {
        abort();
    }
}

LogStream::LogStream(int level) : level_(level) {}

LogStream::~LogStream() {
    Log(level_, "%s", oss_.str().c_str());
}

} // namespace common
} // namespace baidu

/* vim: set expandtab ts=4 sw=4 sts=4 tw=100: */
<commit_msg>use set instead of queue<commit_after>// Copyright (c) 2014, Baidu.com, Inc. All Rights Reserved
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//
// Author: yanshiguang02@baidu.com

#include "logging.h"

#include <assert.h>
#include <boost/bind.hpp>
#include <queue>
#include <set>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <syscall.h>
#include <sys/time.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

#include "mutex.h"
#include "thread.h"
#include "timer.h"

namespace baidu {
namespace common {

int g_log_level = INFO;
int64_t g_log_size = 0;
int32_t g_log_count = 0;
FILE* g_log_file = stdout;
std::string g_log_file_name;
FILE* g_warning_file = NULL;
int64_t g_total_size_limit = 0;

std::set<std::string> g_log_set;
int64_t current_total_size = 0;

bool GetNewLog(bool append) {
    char buf[30];
    struct timeval tv;
    gettimeofday(&tv, NULL);
    const time_t seconds = tv.tv_sec;
    struct tm t;
    localtime_r(&seconds, &t);
    snprintf(buf, 30,
        "%02d-%02d.%02d:%02d:%02d.%06d",
        t.tm_mon + 1,
        t.tm_mday,
        t.tm_hour,
        t.tm_min,
        t.tm_sec,
        static_cast<int>(tv.tv_usec));
    std::string full_path(g_log_file_name + ".");
    full_path.append(buf);
    size_t idx = full_path.rfind('/');
    if (idx == std::string::npos) {
        idx = 0;
    } else {
        idx += 1;
    }
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(full_path.c_str(), mode);
    if (fp == NULL) {
        return false;
    }
    if (g_log_file != stdout) {
        fclose(g_log_file);
    }
    g_log_file = fp;
    remove(g_log_file_name.c_str());
    symlink(full_path.substr(idx).c_str(), g_log_file_name.c_str());
    g_log_set.insert(full_path);
    while ((g_log_count && static_cast<int64_t>(g_log_set.size()) > g_log_count)
            || (g_total_size_limit && current_total_size > g_total_size_limit)) {
        std::set<std::string>::iterator it = g_log_set.begin();
        if (it != g_log_set.end()) {
            struct stat sta;
            if (-1 == lstat(it->c_str(), &sta)) {
                return false;
            }
            remove(it->c_str());
            current_total_size -= sta.st_size;
            g_log_set.erase(it++);
        }
    }
    return true;
}

void SetLogLevel(int level) {
    g_log_level = level;
}

class AsyncLogger {
public:
    AsyncLogger()
      : jobs_(&mu_), done_(&mu_), stopped_(false), size_(0) {
        thread_.Start(boost::bind(&AsyncLogger::AsyncWriter, this));
    }
    ~AsyncLogger() {
        stopped_ = true;
        {
            MutexLock lock(&mu_);
            jobs_.Signal();
        }
        thread_.Join();
        // close fd
    }
    void WriteLog(int log_level, const char* buffer, int32_t len) {
        std::string* log_str = new std::string(buffer, len);
        MutexLock lock(&mu_);
        buffer_queue_.push(make_pair(log_level, log_str));
        jobs_.Signal();
    }
    void AsyncWriter() {
        MutexLock lock(&mu_);
        while (1) {
            int loglen = 0;
            int wflen = 0;
            while (!buffer_queue_.empty()) {
                int log_level = buffer_queue_.front().first;
                std::string* str = buffer_queue_.front().second;
                buffer_queue_.pop();
                if (g_log_file != stdout && g_log_size && str &&
                        static_cast<int64_t>(size_ + str->length()) > g_log_size) {
                    current_total_size += static_cast<int64_t>(size_ + str->length());
                    mu_.Unlock();
                    GetNewLog(false);
                    mu_.Lock();
                    size_ = 0;
                }
                mu_.Unlock();
                if (str && !str->empty()) {
                    fwrite(str->data(), 1, str->size(), g_log_file);
                    loglen += str->size();
                    if (g_warning_file && log_level >= 8) {
                        fwrite(str->data(), 1, str->size(), g_warning_file);
                        wflen += str->size();
                    }
                    if (g_log_size) size_ += str->length();
                }
                delete str;
                mu_.Lock();
            }
            if (loglen) fflush(g_log_file);
            if (wflen) fflush(g_warning_file);
            if (stopped_) {
                break;
            }
            done_.Broadcast();
            jobs_.Wait();
        }
    }
    void Flush() {
        MutexLock lock(&mu_);
        buffer_queue_.push(std::make_pair(0, reinterpret_cast<std::string*>(NULL)));
        jobs_.Signal();
        done_.Wait();
    }
private:
    Mutex mu_;
    CondVar jobs_;
    CondVar done_;
    bool stopped_;
    int64_t size_;
    Thread thread_;
    std::queue<std::pair<int, std::string*> > buffer_queue_;
};

AsyncLogger g_logger;

bool SetWarningFile(const char* path, bool append) {
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(path, mode);
    if (fp == NULL) {
        return false;
    }
    if (g_warning_file) {
        fclose(g_warning_file);
    }
    g_warning_file = fp;
    return true;
}

bool RecoverHistory(const char* path) {
    std::string log_path(path);
    size_t idx = log_path.rfind('/');
    std::string dir = "./";
    std::string log(path);
    if (idx != std::string::npos) {
        dir = log_path.substr(0, idx + 1);
        log = log_path.substr(idx + 1);
    }
    struct dirent *entry = NULL;
    DIR *dir_ptr = opendir(dir.c_str());
    if (dir_ptr == NULL) {
        return false;
    }
    std::vector<std::string> loglist;
    while (entry = readdir(dir_ptr)) {
        if (std::string(entry->d_name).find(log) != std::string::npos) {
            std::string file_name = dir + std::string(entry->d_name);
            struct stat sta;
            if (-1 == lstat(file_name.c_str(), &sta)) {
                return false;
            }
            if (S_ISREG(sta.st_mode)) {
                loglist.push_back(dir + std::string(entry->d_name));
                current_total_size += sta.st_size;
            }
        }
    }
    closedir(dir_ptr);
    std::sort(loglist.begin(), loglist.end());
    for (std::vector<std::string>::iterator it = loglist.begin(); it != loglist.end();
            ++it) {
        g_log_set.insert(*it);
    }
    while ( (g_log_count && static_cast<int64_t>(g_log_set.size()) > g_log_count)
            || (g_total_size_limit && current_total_size > g_total_size_limit)) {
        std::set<std::string>::iterator it = g_log_set.begin();
        if (it != g_log_set.end()) {
            struct stat sta;
            if (-1 == lstat(it->c_str(), &sta)) {
                return false;
            }
            remove(it->c_str());
            current_total_size -= sta.st_size;
            g_log_set.erase(it++);
        }
    }
    return true;
}

bool SetLogFile(const char* path, bool append) {
    g_log_file_name.assign(path);
    return GetNewLog(append);
}

bool SetLogSize(int size) {
    if (size < 0) {
        return false;
    }
    g_log_size = static_cast<int64_t>(size) << 20;
    return true;
}

bool SetLogCount(int count) {
    if (count < 0 || g_total_size_limit != 0) {
        return false;
    }
    g_log_count = count;
    if (!RecoverHistory(g_log_file_name.c_str())) {
        return false;
    }
    return true;
}

bool SetLogSizeLimit(int size) {
    if (size < 0 || g_log_count != 0) {
        return false;
    }
    g_total_size_limit = static_cast<int64_t>(size) << 20;
    if (!RecoverHistory(g_log_file_name.c_str())) {
        return false;
    }
    return true;
}

void Logv(int log_level, const char* format, va_list ap) {
    static __thread uint64_t thread_id = 0;
    if (thread_id == 0) {
        thread_id = syscall(__NR_gettid);
    }

    // We try twice: the first time with a fixed-size stack allocated buffer,
    // and the second time with a much larger dynamically allocated buffer.
    char buffer[500];
    for (int iter = 0; iter < 2; iter++) {
        char* base;
        int bufsize;
        if (iter == 0) {
            bufsize = sizeof(buffer);
            base = buffer;
        } else {
            bufsize = 30000;
            base = new char[bufsize];
        }
        char* p = base;
        char* limit = base + bufsize;

        int32_t rlen = timer::now_time_str(p, limit - p);
        p += rlen;
        p += snprintf(p, limit - p, " %lld ", static_cast<long long unsigned int>(thread_id));

        // Print the message
        if (p < limit) {
            va_list backup_ap;
            va_copy(backup_ap, ap);
            p += vsnprintf(p, limit - p, format, backup_ap);
            va_end(backup_ap);
        }

        // Truncate to available space if necessary
        if (p >= limit) {
            if (iter == 0) {
                continue;       // Try again with larger buffer
            } else {
                p = limit - 1;
            }
        }

        // Add newline if necessary
        if (p == base || p[-1] != '\n') {
            *p++ = '\n';
        }

        assert(p <= limit);
        //fwrite(base, 1, p - base, g_log_file);
        //fflush(g_log_file);
        //if (g_warning_file && log_level >= 8) {
        //    fwrite(base, 1, p - base, g_warning_file);
        //    fflush(g_warning_file);
        //}
        g_logger.WriteLog(log_level, base, p - base);
        if (log_level == FATAL) {
            g_logger.Flush();
        }
        if (base != buffer) {
            delete[] base;
        }
        break;
    }
}

void Log(int level, const char* fmt, ...) {
    va_list ap;
    va_start(ap, fmt);

    if (level >= g_log_level) {
        Logv(level, fmt, ap);
    }
    va_end(ap);
    if (level == FATAL) {
        abort();
    }
}

LogStream::LogStream(int level) : level_(level) {}

LogStream::~LogStream() {
    Log(level_, "%s", oss_.str().c_str());
}

} // namespace common
} // namespace baidu

/* vim: set expandtab ts=4 sw=4 sts=4 tw=100: */
<|endoftext|>


<commit_before>/**\file
 * \brief Matrix-esque terminal animation
 *
 * \copyright
 * Copyright (c) 2012-2013, ef.gy Project Members
 * \copyright
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: http://git.becquerel.org/jyujin/libefgy.git
 */

#include <iostream>

#include <ef.gy/vt100.h>
#include <ef.gy/random.h>
#include <unistd.h>
#include <chrono>

using namespace efgy;
using namespace std::chrono;

terminal::vt100<> output;
random::mersenneTwister<> rng(1337);
system_clock::time_point now;

class matrixStream
{
    public:
        class cell
        {
            public:
                cell(const unsigned long &pCharacter)
                    : character(pCharacter), created(now) {}

                unsigned long character;
                system_clock::time_point created;
        };

        matrixStream(const std::size_t &pLine, const std::size_t &pColumn)
            : line(pLine), column(pColumn), last(now) {}

        std::size_t line;
        std::size_t column;
        std::vector<cell> data;
        system_clock::time_point last;

        bool update (void)
        {
            if ((now - last) > std::chrono::microseconds(40))
            {
                last = now;
                switch (rng() % 4)
                {
                    case 0:
                        if (data.size() > 0)
                        {
                            data.erase(data.begin() + (rng() % data.size()));
                        }
                        else
                        {
                            data.push_back(cell(rng() % (1 << 7)));
                        }
                        break;
                    default:
                        data.push_back(cell(rng() % (1 << 7)));
                }
            }
            std::array<std::size_t,2> s = output.size();
            int i = line;
            for (cell &d : data)
            {
                if (i >= s[1])
                {
                    break;
                }
                output.target[i][column].content = d.character;
                output.target[i][column].foregroundColour = (now - d.created) > std::chrono::microseconds(120) ? 2 : 7;
                output.target[i][column].backgroundColour = 0;
                i++;
            }
            return true;
        }
};

int main (int argc, char **argv)
{
    output.resize(output.getOSDimensions());
    std::array<std::size_t,2> s = output.size();

    int i;

    std::vector<matrixStream> streams;

    while (1)
    {
        i++;
        now = system_clock::now();

        if (i % 100 == 0)
        {
            std::size_t l = rng() % s[1];
            std::size_t c = rng() % s[0];
            streams.push_back(matrixStream(l, c));
        }
        if (streams.size() > 200)
        {
            streams.erase(streams.begin());
        }

        for (matrixStream &s : streams)
        {
            if (rng() % 10 == 0)
            {
                s.update();
            }
        }

        std::cout << output.flush();
        usleep(10);
    }

    for (std::string s = output.flush(); s != ""; s = output.flush())
    {
        std::cout << s;
        i++;
    }

    std::cout << "\e[39;49;0m\n";

    return 0;
}
<commit_msg>some improvements to the matrix animation<commit_after>/**\file
 * \brief Matrix-esque terminal animation
 *
 * \copyright
 * Copyright (c) 2012-2013, ef.gy Project Members
 * \copyright
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: http://git.becquerel.org/jyujin/libefgy.git
 */

#include <iostream>

#include <ef.gy/vt100.h>
#include <ef.gy/random.h>
#include <unistd.h>
#include <chrono>

using namespace efgy;
using namespace std::chrono;

terminal::vt100<> output;
random::mersenneTwister<> rng(1337);
system_clock::time_point now;

class matrixStream
{
    public:
        class cell
        {
            public:
                cell(const unsigned long &pCharacter)
                    : character(pCharacter), created(now) {}

                unsigned long character;
                system_clock::time_point created;
        };

        matrixStream(const std::size_t &pLine, const std::size_t &pColumn)
            : line(pLine), column(pColumn), last(now), doDelete(false) {}

        std::size_t line;
        std::size_t column;
        std::vector<cell> data;
        system_clock::time_point last;
        bool doDelete;

        bool update (void)
        {
            if ((now - last) > std::chrono::milliseconds(10))
            {
                last = now;
                switch (rng() % 3)
                {
                    case 0:
                        if (data.size() > 0)
                        {
                            data.erase(data.begin() + (rng() % data.size()));
                        }
                        else
                        {
                            data.push_back(cell(rng() % (1 << 7)));
                        }
                        break;
                    default:
                        data.push_back(cell(rng() % (1 << 7)));
                }
            }
            std::array<std::size_t,2> s = output.size();
            if (data.size() > (s[1]/2))
            {
                if (line < s[1])
                {
                    if (rng() % 5 == 0)
                    {
                        output.target[line][column].content = ' ';
                        line++;
                    }
                }
                else
                {
                    doDelete = true;
                }
                data.erase(data.begin());
            }
            int i = line;
            for (cell &d : data)
            {
                if (i >= s[1])
                {
                    break;
                }
                output.target[i][column].content = d.character;
                output.target[i][column].foregroundColour = (now - d.created) > std::chrono::milliseconds(120) ? 2 : 7;
                output.target[i][column].backgroundColour = 0;
                i++;
            }
            return true;
        }
};

int main (int argc, char **argv)
{
    output.resize(output.getOSDimensions());
    std::array<std::size_t,2> s = output.size();

    int i;

    std::vector<matrixStream> streams;

    while (1)
    {
        i++;
        now = system_clock::now();

        if (i % 50 == 0)
        {
            std::size_t l = rng() % (s[1] / 3);
            std::size_t c = rng() %  s[0];
            streams.push_back(matrixStream(l, c));
        }
        if (streams.size() > 100)
        {
            streams.erase(streams.begin());
        }

        for (matrixStream &s : streams)
        {
            s.update();
        }

        for (unsigned int i = 0; i < streams.size(); i++)
        {
            if (streams[i].doDelete)
            {
                streams.erase(streams.begin() + i);
            }
        }

        std::cout << output.flush();
        usleep(50);
    }

    for (std::string s = output.flush(); s != ""; s = output.flush())
    {
        std::cout << s;
        i++;
    }

    std::cout << "\e[39;49;0m\n";

    return 0;
}
<|endoftext|>


<commit_before>/**\file
 * \ingroup example-programmes
 * \brief Matrix-esque terminal animation
 *
 * This is a terminal programme that uses libefgy's vt100 code to render a text
 * version of the matrix 'scrolling streams of text' animation. It's really
 * fairly simple but also kinda nice to see how the vt100 output is performing.
 *
 * \image html matrix.png "Screenshot of the programme running in Terminal.app"
 *
 * \copyright
 * Copyright (c) 2012-2013, ef.gy Project Members
 * \copyright
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: http://git.becquerel.org/jyujin/libefgy.git
 */

#include <iostream>

#include <ef.gy/vt100.h>
#include <ef.gy/random.h>
#include <chrono>
#include <csignal>
#include <cmath>
#include <sched.h>

using namespace efgy;
using namespace std::chrono;

terminal::vt100<> output;
random::mersenneTwister<> rng(1337);
system_clock::time_point now;

class matrixStream
{
    public:
        class cell
        {
            public:
                cell(const unsigned long &pCharacter)
                    : character(pCharacter), created(now) {}

                unsigned long character;
                system_clock::time_point created;
        };

        matrixStream(const std::size_t &pLine, const std::size_t &pColumn)
            : line(pLine), column(pColumn), last(now), doDelete(false) {}

        std::size_t line;
        std::size_t column;
        std::vector<cell> data;
        system_clock::time_point last;
        bool doDelete;

        bool update (void)
        {
            if ((now - last) > std::chrono::milliseconds(10))
            {
                last = now;
                switch (rng() % 3)
                {
                    case 0:
                        if (data.size() > 0)
                        {
                            data.erase(data.begin() + (rng() % data.size()));
                            break;
                        }
                    default:
                        data.push_back(cell(rng() % (1 << 7)));
                }
            }
            std::array<std::size_t,2> s = output.size();
            if (data.size() > (s[1]/2))
            {
                if (line < s[1])
                {
                    if (rng() % 5 == 0)
                    {
                        output.target[line][column].content = ' ';
                        line++;
                    }
                }
                else
                {
                    doDelete = true;
                }
                data.erase(data.begin());
            }
            int i = line;
            for (cell &d : data)
            {
                if (i >= s[1])
                {
                    break;
                }
                output.target[i][column].content = d.character;
                output.target[i][column].foregroundColour = (now - d.created) > std::chrono::milliseconds(120) ? 2 : 7;
                output.target[i][column].backgroundColour = 0;
                i++;
            }
            return true;
        }
};

void handle_interrupt(int signal)
{
    exit(1);
}

terminal::cell<long> postProcess
    (const terminal::terminal<long>&t,
     const std::size_t &l,
     const std::size_t &c)
{
    terminal::cell<long> rv = t.target[l][c];
    rv.content = rv.content == 0 ? ' ' : rv.content;
    rv.foregroundColour = rv.foregroundColour == 7 ? 7 : 2;
    rv.backgroundColour = 0;
    return rv;
}

terminal::cell<long> postProcessPolar
    (const terminal::terminal<long>&t,
     const std::size_t &pl,
     const std::size_t &pc)
{
    double l = pl;
    double c = pc;
    std::array<std::size_t,2> s = t.size();
    double hl = s[1]/2;
    double hc = s[0]/2;
    double loff = l - hl;
    double coff = c - hc;
    double r2 = loff*loff + coff*coff;
    double r = std::sqrt(r2);
    l = hl + loff + std::sin(r) * 1.0;
    c = hc + coff + std::cos(r) * 1.0;
    const std::size_t tl = (std::size_t)l < s[1] ? (std::size_t)l : (s[1] - 1);
    const std::size_t tc = (std::size_t)c < s[0] ? (std::size_t)c : (s[0] - 1);
    terminal::cell<long> rv = t.target[tl][tc];
    terminal::cell<long> cv = t.current[tl][tc];
    rv.content = rv.content == 0 ? (cv.content == 0 ? ' ' : cv.content) : rv.content;
    rv.foregroundColour = rv.foregroundColour == 7 ? 7 : 2;
    rv.backgroundColour = 0;
    return rv;
}

int main (int argc, char **argv)
{
    output.resize(output.getOSDimensions());
    std::array<std::size_t,2> s = output.size();

    std::size_t i;

    std::vector<matrixStream> streams;

    std::signal(SIGINT, handle_interrupt);

    while (1)
    {
        i++;
        now = system_clock::now();

        if ((streams.size() <= 100) && (i % 50 == 0))
        {
            std::size_t l = rng() % (s[1] / 3);
            std::size_t c = rng() %  s[0];
            streams.push_back(matrixStream(l, c));
        }

        for (matrixStream &s : streams)
        {
            s.update();
        }

        for (unsigned int i = 0; i < streams.size(); i++)
        {
            if (streams[i].doDelete)
            {
                streams.erase(streams.begin() + i);
            }
        }

        if (output.flush(postProcess) == 0)
        {
            sched_yield();
        }
    }

    while ((i = output.flush(postProcess)) > 0);

    return 0;
}
<commit_msg>started documenting the matrix demo<commit_after>/**\file
 * \ingroup example-programmes
 * \brief Matrix-esque terminal animation
 *
 * This is a terminal programme that uses libefgy's vt100 code to render a text
 * version of the matrix 'scrolling streams of text' animation. It's really
 * fairly simple but also kinda nice to see how the vt100 output is performing.
 *
 * \image html matrix.png "Screenshot of the programme running in Terminal.app"
 *
 * \copyright
 * Copyright (c) 2012-2013, ef.gy Project Members
 * \copyright
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: http://git.becquerel.org/jyujin/libefgy.git
 */

#include <iostream>

#include <ef.gy/vt100.h>
#include <ef.gy/random.h>
#include <chrono>
#include <csignal>
#include <cmath>
#include <sched.h>

using namespace efgy;
using namespace std::chrono;

/**\brief Data and functions of the matrix example programme
 *
 * Contains global variables and classes used by the 'matrix' demo animation.
 * This is a separate namespace to keep things all neat, clean and tidy.
 */
namespace thematrix
{
    /**\brief VT100 output buffer
     *
     * Encapsulates stdio to automatically generate the terminal escape
     * sequences to write things at specific positions with specific colours.
     */
    terminal::vt100<> output;

    /**\brief Random number generator
     *
     * A seeded instance of the mersenne twister RNG; used to position the
     * streams and to add and remove glyphs randomly.
     */
    random::mersenneTwister<> rng(1337);

    /**\brief Current time
     *
     * The point in time when processing of the current streams started; used
     * to determine when streams should be updated.
     */
    system_clock::time_point now;

    /**\brief A stream of data
     *
     * A 'stream' is what I dubbed the individual columns of text in the
     * animation. These are generated randomly and they mutate randomly as
     * well.
     */
    class stream
    {
        public:
            /**\brief A single glyph in a stream
             *
             * To create the animation, each of the glyphs in the output needs
             * to be tagges with the time it was created, in order to know
             * which colour to draw the glyph in.
             */
            class cell
            {
                public:
                    cell(const unsigned long &pCharacter)
                        : character(pCharacter), created(now) {}

                    unsigned long character;
                    system_clock::time_point created;
            };

            stream(const std::size_t &pLine, const std::size_t &pColumn)
                : line(pLine), column(pColumn), last(now), doDelete(false) {}

            std::size_t line;
            std::size_t column;
            std::vector<cell> data;
            system_clock::time_point last;
            bool doDelete;

            bool update (void)
            {
                if ((now - last) > std::chrono::milliseconds(10))
                {
                    last = now;
                    switch (rng() % 3)
                    {
                        case 0:
                            if (data.size() > 0)
                            {
                                data.erase(data.begin() + (rng() % data.size()));
                                break;
                            }
                        default:
                            data.push_back(cell(rng() % (1 << 7)));
                    }
                }
                std::array<std::size_t,2> s = output.size();
                if (data.size() > (s[1]/2))
                {
                    if (line < s[1])
                    {
                        if (rng() % 5 == 0)
                        {
                            output.target[line][column].content = ' ';
                            line++;
                        }
                    }
                    else
                    {
                        doDelete = true;
                    }
                    data.erase(data.begin());
                }
                int i = line;
                for (cell &d : data)
                {
                    if (i >= s[1])
                    {
                        break;
                    }
                    output.target[i][column].content = d.character;
                    output.target[i][column].foregroundColour = (now - d.created) > std::chrono::milliseconds(120) ? 2 : 7;
                    output.target[i][column].backgroundColour = 0;
                    i++;
                }
                return true;
            }
    };

    void handle_interrupt(int signal)
    {
        exit(1);
    }

    terminal::cell<long> postProcess
        (const terminal::terminal<long>&t,
         const std::size_t &l,
         const std::size_t &c)
    {
        terminal::cell<long> rv = t.target[l][c];
        rv.content = rv.content == 0 ? ' ' : rv.content;
        rv.foregroundColour = rv.foregroundColour == 7 ? 7 : 2;
        rv.backgroundColour = 0;
        return rv;
    }

    terminal::cell<long> postProcessPolar
        (const terminal::terminal<long>&t,
         const std::size_t &pl,
         const std::size_t &pc)
    {
        double l = pl;
        double c = pc;
        std::array<std::size_t,2> s = t.size();
        double hl = s[1]/2;
        double hc = s[0]/2;
        double loff = l - hl;
        double coff = c - hc;
        double r2 = loff*loff + coff*coff;
        double r = std::sqrt(r2);
        l = hl + loff + std::sin(r) * 1.0;
        c = hc + coff + std::cos(r) * 1.0;
        const std::size_t tl = (std::size_t)l < s[1] ? (std::size_t)l : (s[1] - 1);
        const std::size_t tc = (std::size_t)c < s[0] ? (std::size_t)c : (s[0] - 1);
        terminal::cell<long> rv = t.target[tl][tc];
        terminal::cell<long> cv = t.current[tl][tc];
        rv.content = rv.content == 0 ? (cv.content == 0 ? ' ' : cv.content) : rv.content;
        rv.foregroundColour = rv.foregroundColour == 7 ? 7 : 2;
        rv.backgroundColour = 0;
        return rv;
    }
};

using namespace thematrix;

int main (int argc, char **argv)
{
    output.resize(output.getOSDimensions());
    std::array<std::size_t,2> s = output.size();

    std::size_t i;

    std::vector<stream> streams;

    std::signal(SIGINT, handle_interrupt);

    while (1)
    {
        i++;
        now = system_clock::now();

        if ((streams.size() <= 100) && (i % 50 == 0))
        {
            std::size_t l = rng() % (s[1] / 3);
            std::size_t c = rng() %  s[0];
            streams.push_back(stream(l, c));
        }

        for (stream &s : streams)
        {
            s.update();
        }

        for (unsigned int i = 0; i < streams.size(); i++)
        {
            if (streams[i].doDelete)
            {
                streams.erase(streams.begin() + i);
            }
        }

        if (output.flush(postProcess) == 0)
        {
            sched_yield();
        }
    }

    while ((i = output.flush(postProcess)) > 0);

    return 0;
}
<|endoftext|>


<commit_before>#include "OrientationSensorsWrapper.h"
#include "SDLogDriver.h"
#include "DualEncoderDriver.h"
#include "MotorDriver.h"
#include "PlanarAccelerationModule.h"
#include "TireContactModule.h"
#include "FrontLiftedDetection.h"

#include "constants.h"

#include <Wire.h>
#include <SD.h>

enum {
	WAITING_FOR_COMMAND,
	PREPARE_TO_FIGHT,
	IN_COMBAT,
	RECALIBRATE,
	BRAINDEAD,
	TEST
} robot_state = WAITING_FOR_COMMAND;

uint32_t prepare_micros;

void log_info()
{
	log_data_pack.timestamp = millis();

	log_data_pack.acc_x = position.getAccX();
	log_data_pack.acc_y = position.getAccY();
	log_data_pack.acc_z = position.getAccZ();

	log_data_pack.gyro_x = position.getGyroX();
	log_data_pack.gyro_y = position.getGyroY();
	log_data_pack.gyro_z = position.getGyroZ();

	log_data_pack.ahrs_x = position.getRoll();
	log_data_pack.ahrs_y = position.getPitch();
	log_data_pack.ahrs_z = position.getYaw();

	log_data_pack.leftSpeed = leftEncoder.getSpeed();
	log_data_pack.rightSpeed = rightEncoder.getSpeed();

	logDataPack();
}

void setup()
{
	Serial.begin(115200);

	SD.begin(4);

	initLogger();

	initDualEncoders();
	initMotors();

	pinMode(LEFT_SENSOR_PIN, INPUT);
	pinMode(RIGHT_SENSOR_PIN, INPUT);

	Wire.begin();
	Wire.setClock(400000);

	position.init();

	tiresContactInit(TIRE_LOST_OF_CONTACT_DEGREES);

	initFrontLifted(FRONT_LIFTED_THRESHOLD);
}

void serialCommand()
{
	static int left = 0;
	static int right = 0;

	if(Serial.available() == 0) return;

	char cmd = Serial.read();
	if(cmd == 'l')
	{
		left = 0;
		if(Serial.available() > 0)
		{
			char dir = Serial.read();
			while(Serial.available() > 0)
			{
				char digit = Serial.read();
				left = left * 10 + (digit - '0');
			}

			if(dir == '-') left = -left;
		}

		setMotors(left, right);
	}
	else if(cmd == 'r')
	{
		right = 0;
		if(Serial.available() > 0)
		{
			char dir = Serial.read();
			while(Serial.available() > 0)
			{
				char digit = Serial.read();
				right = right * 10 + (digit - '0');
			}

			if(dir == '-') right = -right;
		}

		setMotors(left, right);
	}
	else if(cmd == 'd')
	{
		dumpLog();
	}
}

void readProximitySensors(int& left, int& right)
{
	left = digitalRead(LEFT_SENSOR_PIN) ^ 1;
	right = digitalRead(RIGHT_SENSOR_PIN) ^ 1;
}

void getCommand()
{
	static int left = 0;
	static int right = 0;

	static int last_left = 0;
	static int last_right = 0;

	static int command_counter = 0;

	readProximitySensors(left, right);

	if(robot_state == WAITING_FOR_COMMAND)
	{
		if(right)
		{
			if(left > last_left)
			{
				++command_counter;
				Serial.print("Command counter: ");
				Serial.println(command_counter);
			}
		}
		else if(right < last_right)
		{
			switch(command_counter)
			{
				case 0:
					robot_state = PREPARE_TO_FIGHT;
					prepare_micros = micros();
					break;
				case 1:
					robot_state = RECALIBRATE;
					position.calibrate();
					plannarAcceleration.calibrate();
					robot_state = WAITING_FOR_COMMAND;
					break;
				case 2:
					robot_state = TEST;
					setMotors(80, 80);
					break;
			}

			command_counter = 0;
		}
	}
	else if(left && right)
	{
		if(robot_state == PREPARE_TO_FIGHT)
			robot_state = WAITING_FOR_COMMAND;
		else
		{
			robot_state = BRAINDEAD;
			setMotors(0, 0);
		}
	}

	last_left = left;
	last_right = right;
}

void loop()
{
	static uint32_t last_sample_micros = 0;
	static uint32_t last_log_micros = 0;
	static uint32_t last_blink_micros = 0;

	uint32_t current_micros = micros();

	{ // blink control
		uint32_t full_cycle;
		uint32_t high_cycle;

		if(robot_state == BRAINDEAD)
		{
			full_cycle = MICROS_PER_SECOND * 2;
			high_cycle = MICROS_PER_SECOND;
		}
		else if(robot_state == PREPARE_TO_FIGHT)
		{
			full_cycle = MICROS_PER_SECOND / 10;
			high_cycle = MICROS_PER_SECOND / 20;
		}
		else
		{
			full_cycle = MICROS_PER_SECOND / 2;
			high_cycle = MICROS_PER_SECOND / 3;
		}

		if(current_micros - last_blink_micros >= full_cycle)
		{
			digitalWrite(LED_PIN, HIGH);
			last_blink_micros = current_micros;
		}
		else if(current_micros - last_blink_micros >= high_cycle)
		{
			digitalWrite(LED_PIN, LOW);
		}
	}

	if(robot_state == BRAINDEAD)
	{
		setMotors(0, 0);
		return;
	}

	if(robot_state == PREPARE_TO_FIGHT)
	{
		if(current_micros - prepare_micros >= MICROS_PER_SECOND * 5)
		{
			robot_state = IN_COMBAT;
		}
	}

	// Read sensors
	if(current_micros - last_sample_micros >= MICROS_PER_SECOND / SAMPLE_FREQUENCY)
	{
		position.read_sensors();

		getCommand();

		last_sample_micros = current_micros;
	}

	// Log data
	if(current_micros - last_log_micros >= MICROS_PER_SECOND / LOG_FREQUENCY)
	{
		log_info();
		last_log_micros = current_micros;

		Serial.print(getLeftTireContactState()); Serial.print(" ");
		Serial.print(getRightTireContactState()); Serial.print(" ");
		Serial.print(getFrontLiftedState()); Serial.println("");
	}

	serialCommand();
}
<commit_msg>Improve serial commands for motors<commit_after>#include "OrientationSensorsWrapper.h"
#include "SDLogDriver.h"
#include "DualEncoderDriver.h"
#include "MotorDriver.h"
#include "PlanarAccelerationModule.h"
#include "TireContactModule.h"
#include "FrontLiftedDetection.h"

#include "constants.h"

#include <Wire.h>
#include <SD.h>

enum {
	WAITING_FOR_COMMAND,
	PREPARE_TO_FIGHT,
	IN_COMBAT,
	RECALIBRATE,
	BRAINDEAD,
	TEST
} robot_state = WAITING_FOR_COMMAND;

uint32_t prepare_micros;

void log_info()
{
	log_data_pack.timestamp = millis();

	log_data_pack.acc_x = position.getAccX();
	log_data_pack.acc_y = position.getAccY();
	log_data_pack.acc_z = position.getAccZ();

	log_data_pack.gyro_x = position.getGyroX();
	log_data_pack.gyro_y = position.getGyroY();
	log_data_pack.gyro_z = position.getGyroZ();

	log_data_pack.ahrs_x = position.getRoll();
	log_data_pack.ahrs_y = position.getPitch();
	log_data_pack.ahrs_z = position.getYaw();

	log_data_pack.leftSpeed = leftEncoder.getSpeed();
	log_data_pack.rightSpeed = rightEncoder.getSpeed();

	logDataPack();
}

void setup()
{
	Serial.begin(115200);

	SD.begin(4);

	initLogger();

	initDualEncoders();
	initMotors();

	pinMode(LEFT_SENSOR_PIN, INPUT);
	pinMode(RIGHT_SENSOR_PIN, INPUT);

	Wire.begin();
	Wire.setClock(400000);

	position.init();

	tiresContactInit(TIRE_LOST_OF_CONTACT_DEGREES);

	initFrontLifted(FRONT_LIFTED_THRESHOLD);
}

void serialCommand()
{
	static int left = 0;
	static int right = 0;

	if(Serial.available() == 0) return;

	char cmd = Serial.read();
	if(cmd == 'l')
	{
		left = 0;
		if(Serial.available() > 0)
		{
			int dir = 1;
			if(Serial.peek() == '-')
			{
				dir = -1;
				Serial.read();
			}
			while(Serial.available() > 0)
			{
				char digit = Serial.read();
				left = left * 10 + (digit - '0');
			}
			left *= dir;
		}

		setMotors(left, right);
	}
	else if(cmd == 'r')
	{
		right = 0;
		if(Serial.available() > 0)
		{
			int dir = 1;
			if(Serial.peek() == '-')
			{
				dir = -1;
				Serial.read();
			}
			while(Serial.available() > 0)
			{
				char digit = Serial.read();
				right = right * 10 + (digit - '0');
			}
			right *= dir;
		}

		setMotors(left, right);
	}
	else if(cmd == 'd')
	{
		dumpLog();
	}
}

void readProximitySensors(int& left, int& right)
{
	left = digitalRead(LEFT_SENSOR_PIN) ^ 1;
	right = digitalRead(RIGHT_SENSOR_PIN) ^ 1;
}

void getCommand()
{
	static int left = 0;
	static int right = 0;

	static int last_left = 0;
	static int last_right = 0;

	static int command_counter = 0;

	readProximitySensors(left, right);

	if(robot_state == WAITING_FOR_COMMAND)
	{
		if(right)
		{
			if(left > last_left)
			{
				++command_counter;
				Serial.print("Command counter: ");
				Serial.println(command_counter);
			}
		}
		else if(right < last_right)
		{
			switch(command_counter)
			{
				case 0:
					robot_state = PREPARE_TO_FIGHT;
					prepare_micros = micros();
					break;
				case 1:
					robot_state = RECALIBRATE;
					position.calibrate();
					plannarAcceleration.calibrate();
					robot_state = WAITING_FOR_COMMAND;
					break;
				case 2:
					robot_state = TEST;
					setMotors(80, 80);
					break;
			}

			command_counter = 0;
		}
	}
	else if(left && right)
	{
		if(robot_state == PREPARE_TO_FIGHT)
			robot_state = WAITING_FOR_COMMAND;
		else
		{
			robot_state = BRAINDEAD;
			setMotors(0, 0);
		}
	}

	last_left = left;
	last_right = right;
}

void loop()
{
	static uint32_t last_sample_micros = 0;
	static uint32_t last_log_micros = 0;
	static uint32_t last_blink_micros = 0;

	uint32_t current_micros = micros();

	{ // blink control
		uint32_t full_cycle;
		uint32_t high_cycle;

		if(robot_state == BRAINDEAD)
		{
			full_cycle = MICROS_PER_SECOND * 2;
			high_cycle = MICROS_PER_SECOND;
		}
		else if(robot_state == PREPARE_TO_FIGHT)
		{
			full_cycle = MICROS_PER_SECOND / 10;
			high_cycle = MICROS_PER_SECOND / 20;
		}
		else
		{
			full_cycle = MICROS_PER_SECOND / 2;
			high_cycle = MICROS_PER_SECOND / 3;
		}

		if(current_micros - last_blink_micros >= full_cycle)
		{
			digitalWrite(LED_PIN, HIGH);
			last_blink_micros = current_micros;
		}
		else if(current_micros - last_blink_micros >= high_cycle)
		{
			digitalWrite(LED_PIN, LOW);
		}
	}

	if(robot_state == BRAINDEAD)
	{
		setMotors(0, 0);
		return;
	}

	if(robot_state == PREPARE_TO_FIGHT)
	{
		if(current_micros - prepare_micros >= MICROS_PER_SECOND * 5)
		{
			robot_state = IN_COMBAT;
		}
	}

	// Read sensors
	if(current_micros - last_sample_micros >= MICROS_PER_SECOND / SAMPLE_FREQUENCY)
	{
		position.read_sensors();

		getCommand();

		last_sample_micros = current_micros;
	}

	// Log data
	if(current_micros - last_log_micros >= MICROS_PER_SECOND / LOG_FREQUENCY)
	{
		log_info();
		last_log_micros = current_micros;

		Serial.print(getLeftTireContactState()); Serial.print(" ");
		Serial.print(getRightTireContactState()); Serial.print(" ");
		Serial.print(getFrontLiftedState()); Serial.println("");
	}

	serialCommand();
}
<|endoftext|>


<commit_before>#include "packer.hpp"

namespace vg {

Packer::Packer(void) : xgidx(nullptr) { }

Packer::Packer(xg::XG* xidx, size_t binsz) : xgidx(xidx), bin_size(binsz) {
    coverage_dynamic = gcsa::CounterArray(xgidx->seq_length, 8);
    if (binsz) n_bins = xgidx->seq_length / bin_size + 1;
}

Packer::~Packer(void) {
    close_edit_tmpfiles();
    remove_edit_tmpfiles();
}

void Packer::load_from_file(const string& file_name) {
    ifstream in(file_name);
    load(in);
}

void Packer::save_to_file(const string& file_name) {
    ofstream out(file_name);
    serialize(out);
}

void Packer::load(istream& in) {
    sdsl::read_member(bin_size, in);
    sdsl::read_member(n_bins, in);
    coverage_civ.load(in);
    edit_csas.resize(n_bins);
    for (size_t i = 0; i < n_bins; ++i) {
        edit_csas[i].load(in);
    }
    // We can only load compacted.
    is_compacted = true;
}

void Packer::merge_from_files(const vector<string>& file_names) {
#ifdef debug
    cerr << "Merging " << file_names.size() << " pack files" << endl;
#endif

    // load into our dynamic structures, then compact
    bool first = true;
    for (auto& file_name : file_names) {
        Packer c;
        ifstream f(file_name);
        c.load(f);
        // take bin size and counts from the first, assume they are all the same
        if (first) {
            bin_size = c.get_bin_size();
            n_bins = c.get_n_bins();
            ensure_edit_tmpfiles_open();
            first = false;
        } else {
            assert(bin_size == c.get_bin_size());
            assert(n_bins == c.get_n_bins());
        }
        c.write_edits(tmpfstreams);
        collect_coverage(c);
    }
}

void Packer::merge_from_dynamic(vector<Packer*>& packers) {
    // load dynamic packs into our dynamic structures, then compact
    bool first = true;
    for (auto& p : packers) {
        auto& c = *p;
        c.close_edit_tmpfiles(); // flush and close temporaries
        // take bin size and counts from the first, assume they are all the same
        if (first) {
            bin_size = c.get_bin_size();
            n_bins = c.get_n_bins();
            ensure_edit_tmpfiles_open();
            first = false;
        } else {
            assert(bin_size == c.get_bin_size());
            assert(n_bins == c.get_n_bins());
        }
        c.write_edits(tmpfstreams);
        collect_coverage(c);
    }
}

size_t Packer::get_bin_size(void) const {
    return bin_size;
}

size_t Packer::get_n_bins(void) const {
    return n_bins;
}

size_t Packer::bin_for_position(size_t i) const {
    if (bin_size > 0) {
        return i / bin_size;
    } else {
        return 0;
    }
}

void Packer::write_edits(vector<ofstream*>& out) const {
    for (size_t i = 0; i < n_bins; ++i) {
        write_edits(*out[i], i);
    }
}

void Packer::write_edits(ostream& out, size_t bin) const {
    if (is_compacted) {
        out << extract(edit_csas[bin], 0, edit_csas[bin].size()-2) << delim1; // chomp trailing null, add back delim
    } else {
        // uncompacted, so just cat the edit file for this bin onto out
        if (edit_tmpfile_names.size()) {
            ifstream edits(edit_tmpfile_names[bin], std::ios_base::binary);
            out << edits.rdbuf() << delim1;
        }
    }
}

void Packer::collect_coverage(const Packer& c) {
    // assume the same basis vector
    assert(!is_compacted);
    for (size_t i = 0; i < c.graph_length(); ++i) {
        coverage_dynamic.increment(i, c.coverage_at_position(i));
    }
}

size_t Packer::serialize(std::ostream& out,
                          sdsl::structure_tree_node* s,
                          std::string name) {
    make_compact();
    sdsl::structure_tree_node* child = sdsl::structure_tree::add_child(s, name, sdsl::util::class_name(*this));
    size_t written = 0;
    written += sdsl::write_member(bin_size, out, child, "bin_size_" + name);
    written += sdsl::write_member(edit_csas.size(), out, child, "n_bins_" + name);
    written += coverage_civ.serialize(out, child, "graph_coverage_" + name);
    for (auto& edit_csa : edit_csas) {
        written += edit_csa.serialize(out, child, "edit_csa_" + name);
    }
    sdsl::structure_tree::add_size(child, written);
    return written;
}

void Packer::make_compact(void) {
    // pack the dynamic countarry and edit coverage into the compact data structure
    if (is_compacted) {
#ifdef debug
        cerr << "Packer is already compact" << endl;
#endif
        return;
    } else {
#ifdef debug
        cerr << "Need to make packer compact" << endl;
#endif
    }
    // sync edit file
    close_edit_tmpfiles();
    // temporaries for construction
    size_t basis_length = coverage_dynamic.size();
    int_vector<> coverage_iv;
    util::assign(coverage_iv, int_vector<>(basis_length));
    for (size_t i = 0; i < coverage_dynamic.size(); ++i) {
        coverage_iv[i] = coverage_dynamic[i];
    }
    edit_csas.resize(edit_tmpfile_names.size());
    util::assign(coverage_civ, coverage_iv);
    construct_config::byte_algo_sa = SE_SAIS;
#pragma omp parallel for
    for (size_t i = 0; i < edit_tmpfile_names.size(); ++i) {
        construct(edit_csas[i], edit_tmpfile_names[i], 1);
    }
    // construct the record marker bitvector
    remove_edit_tmpfiles();
    is_compacted = true;
}

void Packer::make_dynamic(void) {
    if (!is_compacted) return;
    // unpack the compact represenation into the countarray
    assert(false); // not implemented
    is_compacted = false;
}

bool Packer::is_dynamic(void) {
    return !is_compacted;
}

void Packer::ensure_edit_tmpfiles_open(void) {
    if (tmpfstreams.empty()) {
        string base = "vg-pack_";
        string edit_tmpfile_name = tmpfilename(base);
        std::remove(edit_tmpfile_name.c_str()); // remove this; we'll use it as a base name
        // for as many bins as we have, make a temp file
        tmpfstreams.resize(n_bins);
        edit_tmpfile_names.resize(n_bins);
        for (size_t i = 0; i < n_bins; ++i) {
            edit_tmpfile_names[i] = edit_tmpfile_name+"_"+convert(i);
            tmpfstreams[i] = new ofstream;
            tmpfstreams[i]->open(edit_tmpfile_names[i], std::ios_base::binary);
            assert(tmpfstreams[i]->is_open());
        }
    }
}

void Packer::close_edit_tmpfiles(void) {
    if (!tmpfstreams.empty()) {
        for (auto& tmpfstream : tmpfstreams) {
            *tmpfstream << delim1; // pad
            tmpfstream->close();
            delete tmpfstream;
        }
        tmpfstreams.clear();
    }
}

void Packer::remove_edit_tmpfiles(void) {
    if (!edit_tmpfile_names.empty()) {
        for (auto& name : edit_tmpfile_names) {
            std::remove(name.c_str());
        }
        edit_tmpfile_names.clear();
    }
}

void Packer::add(const Alignment& aln, bool record_edits) {
    // open tmpfile if needed
    ensure_edit_tmpfiles_open();
    // count the nodes, edges, and edits
    for (auto& mapping : aln.path().mapping()) {
        if (!mapping.has_position()) {
#ifdef debug
            cerr << "Mapping has no position" << endl;
#endif
            continue;
        }
        size_t i = position_in_basis(mapping.position());
        for (auto& edit : mapping.edit()) {
            if (edit_is_match(edit)) {
#ifdef debug
                cerr << "Recording a match" << endl;
#endif
                if (mapping.position().is_reverse()) {
                    for (size_t j = 0; j < edit.from_length(); ++j) {
                        coverage_dynamic.increment(i-j);
                    }
                } else {
                    for (size_t j = 0; j < edit.from_length(); ++j) {
                        coverage_dynamic.increment(i+j);
                    }
                }
            } else if (record_edits) {
                // we represent things on the forward strand
                string pos_repr = pos_key(i);
                string edit_repr = edit_value(edit, mapping.position().is_reverse());
                size_t bin = bin_for_position(i);
                *tmpfstreams[bin] << pos_repr << edit_repr;
            }
            if (mapping.position().is_reverse()) {
                i -= edit.from_length();
            } else {
                i += edit.from_length();
            }
        }
    }
}

// find the position on the forward strand in the sequence vector
size_t Packer::position_in_basis(const Position& pos) const {
    // get position on the forward strand
    if (pos.is_reverse()) {
        return (int64_t)xg_node_start(pos.node_id(), xgidx)
            + (int64_t)reverse(pos, xg_node_length(pos.node_id(), xgidx)).offset() - 1;
    } else {
        return (int64_t)xg_node_start(pos.node_id(), xgidx) + (int64_t)pos.offset();
    }
}

string Packer::pos_key(size_t i) const {
    Position pos;
    size_t offset = 2;
    pos.set_node_id(i+offset);
    string pos_repr;
    pos.SerializeToString(&pos_repr);
    stringstream s;
    s << delim1 << delim2 << delim1 << escape_delims(pos_repr);
    return s.str();
}

string Packer::edit_value(const Edit& edit, bool revcomp) const {
    string edit_repr;
    if (revcomp) {
        reverse_complement_edit(edit).SerializeToString(&edit_repr);
    } else {
        edit.SerializeToString(&edit_repr);
    }
    stringstream s;
    s << delim1 << escape_delims(edit_repr);
    return s.str();
}

string Packer::escape_delims(const string& s) const {
    return escape_delim(escape_delim(s, delim1), delim2);
}

string Packer::unescape_delims(const string& s) const {
    return unescape_delim(unescape_delim(s, delim1), delim2);
}

string Packer::escape_delim(const string& s, char d) const {
    string escaped; escaped.reserve(s.size());
    for (size_t i = 0; i < s.size(); ++i) {
        char c = s[i];
        escaped.push_back(c);
        if (c == d) escaped.push_back(c);
    }
    return escaped;
}

string Packer::unescape_delim(const string& s, char d) const {
    string unescaped; unescaped.reserve(s.size());
    for (size_t i = 0; i < s.size()-1; ++i) {
        char c = s[i];
        char b = s[i+1];
        if (c == d && b == d) {
            unescaped.push_back(c);
        } else {
            unescaped.push_back(c);
            if (i == s.size()-2) unescaped.push_back(b);
        }
    }
    return unescaped;
}

size_t Packer::graph_length(void) const {
    if (is_compacted) {
        return coverage_civ.size();
    } else {
        return coverage_dynamic.size();
    }
}

size_t Packer::coverage_at_position(size_t i) const {
    if (is_compacted) {
        return coverage_civ[i];
    } else {
        return coverage_dynamic[i];
    }
}

vector<Edit> Packer::edits_at_position(size_t i) const {
    vector<Edit> edits;
    if (i == 0) return edits;
    string key = pos_key(i);
    size_t bin = bin_for_position(i);
    auto& edit_csa = edit_csas[bin];
    auto occs = locate(edit_csa, key);
    for (size_t i = 0; i < occs.size(); ++i) {
        // walk from after the key and delim1 to the next end-sep
        size_t b = occs[i] + key.size() + 1;
        size_t e = b;
        // look for an odd number of delims
        // run until we find a delim
        while (true) {
            while (extract(edit_csa, e, e)[0] != delim1) ++e;
            // now we are matching the delim... count them
            size_t f = e;
            while (extract(edit_csa, f, f)[0] == delim1) ++f;
            size_t c = f - e;
            e = f; // set pointer to last delim
            if (c % 2 != 0) {
                break;
            }
        }
        string value = unescape_delims(extract(edit_csa, b, e));
        Edit edit;
        edit.ParseFromString(value);
        edits.push_back(edit);
    }
    return edits;
}

ostream& Packer::as_table(ostream& out, bool show_edits) {
#ifdef debug
    cerr << "Packer table of " << coverage_civ.size() << " rows:" << endl;
#endif

    out << "seq.pos" << "\t"
        << "node.id" << "\t"
        << "node.offset" << "\t"
        << "coverage";
    if (show_edits) out << "\t" << "edits";
    out << endl;
    // write the coverage as a vector
    for (size_t i = 0; i < coverage_civ.size(); ++i) {
        id_t node_id = xgidx->node_at_seq_pos(i+1);
        size_t offset = i - xgidx->node_start(node_id);
        out << i << "\t" << node_id << "\t" << offset << "\t" << coverage_civ[i];
        if (show_edits) {
            out << "\t" << count(edit_csas[bin_for_position(i)], pos_key(i));
            for (auto& edit : edits_at_position(i)) out << " " << pb2json(edit);
        }
        out << endl;
    }
}

ostream& Packer::show_structure(ostream& out) {
    out << coverage_civ << endl; // graph coverage (compacted coverage_dynamic)
    for (auto& edit_csa : edit_csas) {
        out << edit_csa << endl;
    }
    //out << " i SA ISA PSI LF BWT    T[SA[i]..SA[i]-1]" << endl;
    //csXprintf(cout, "%2I %2S %3s %3P %2p %3B   %:3T", edit_csa);
    return out;
}

}
<commit_msg>resolve #1510<commit_after>#include "packer.hpp"

namespace vg {

Packer::Packer(void) : xgidx(nullptr) { }

Packer::Packer(xg::XG* xidx, size_t binsz) : xgidx(xidx), bin_size(binsz) {
    coverage_dynamic = gcsa::CounterArray(xgidx->seq_length, 8);
    if (binsz) n_bins = xgidx->seq_length / bin_size + 1;
}

Packer::~Packer(void) {
    close_edit_tmpfiles();
    remove_edit_tmpfiles();
}

void Packer::load_from_file(const string& file_name) {
    ifstream in(file_name);
    load(in);
}

void Packer::save_to_file(const string& file_name) {
    ofstream out(file_name);
    serialize(out);
}

void Packer::load(istream& in) {
    sdsl::read_member(bin_size, in);
    sdsl::read_member(n_bins, in);
    coverage_civ.load(in);
    edit_csas.resize(n_bins);
    for (size_t i = 0; i < n_bins; ++i) {
        edit_csas[i].load(in);
    }
    // We can only load compacted.
    is_compacted = true;
}

void Packer::merge_from_files(const vector<string>& file_names) {
#ifdef debug
    cerr << "Merging " << file_names.size() << " pack files" << endl;
#endif

    // load into our dynamic structures, then compact
    bool first = true;
    for (auto& file_name : file_names) {
        Packer c;
        ifstream f(file_name);
        c.load(f);
        // take bin size and counts from the first, assume they are all the same
        if (first) {
            bin_size = c.get_bin_size();
            n_bins = c.get_n_bins();
            ensure_edit_tmpfiles_open();
            first = false;
        } else {
            assert(bin_size == c.get_bin_size());
            assert(n_bins == c.get_n_bins());
        }
        c.write_edits(tmpfstreams);
        collect_coverage(c);
    }
}

void Packer::merge_from_dynamic(vector<Packer*>& packers) {
    // load dynamic packs into our dynamic structures, then compact
    bool first = true;
    for (auto& p : packers) {
        auto& c = *p;
        c.close_edit_tmpfiles(); // flush and close temporaries
        // take bin size and counts from the first, assume they are all the same
        if (first) {
            bin_size = c.get_bin_size();
            n_bins = c.get_n_bins();
            ensure_edit_tmpfiles_open();
            first = false;
        } else {
            assert(bin_size == c.get_bin_size());
            assert(n_bins == c.get_n_bins());
        }
        c.write_edits(tmpfstreams);
        collect_coverage(c);
    }
}

size_t Packer::get_bin_size(void) const {
    return bin_size;
}

size_t Packer::get_n_bins(void) const {
    return n_bins;
}

size_t Packer::bin_for_position(size_t i) const {
    if (bin_size > 0) {
        return i / bin_size;
    } else {
        return 0;
    }
}

void Packer::write_edits(vector<ofstream*>& out) const {
    for (size_t i = 0; i < n_bins; ++i) {
        write_edits(*out[i], i);
    }
}

void Packer::write_edits(ostream& out, size_t bin) const {
    if (is_compacted) {
        out << extract(edit_csas[bin], 0, edit_csas[bin].size()-2) << delim1; // chomp trailing null, add back delim
    } else {
        // uncompacted, so just cat the edit file for this bin onto out
        if (edit_tmpfile_names.size()) {
            ifstream edits(edit_tmpfile_names[bin], std::ios_base::binary);
            out << edits.rdbuf() << delim1;
        }
    }
}

void Packer::collect_coverage(const Packer& c) {
    // assume the same basis vector
    assert(!is_compacted);
    for (size_t i = 0; i < c.graph_length(); ++i) {
        coverage_dynamic.increment(i, c.coverage_at_position(i));
    }
}

size_t Packer::serialize(std::ostream& out,
                          sdsl::structure_tree_node* s,
                          std::string name) {
    make_compact();
    sdsl::structure_tree_node* child = sdsl::structure_tree::add_child(s, name, sdsl::util::class_name(*this));
    size_t written = 0;
    written += sdsl::write_member(bin_size, out, child, "bin_size_" + name);
    written += sdsl::write_member(edit_csas.size(), out, child, "n_bins_" + name);
    written += coverage_civ.serialize(out, child, "graph_coverage_" + name);
    for (auto& edit_csa : edit_csas) {
        written += edit_csa.serialize(out, child, "edit_csa_" + name);
    }
    sdsl::structure_tree::add_size(child, written);
    return written;
}

void Packer::make_compact(void) {
    // pack the dynamic countarry and edit coverage into the compact data structure
    if (is_compacted) {
#ifdef debug
        cerr << "Packer is already compact" << endl;
#endif
        return;
    } else {
#ifdef debug
        cerr << "Need to make packer compact" << endl;
#endif
    }
    // sync edit file
    close_edit_tmpfiles();
    // temporaries for construction
    size_t basis_length = coverage_dynamic.size();
    int_vector<> coverage_iv;
    util::assign(coverage_iv, int_vector<>(basis_length));
    for (size_t i = 0; i < coverage_dynamic.size(); ++i) {
        coverage_iv[i] = coverage_dynamic[i];
    }
    edit_csas.resize(edit_tmpfile_names.size());
    util::assign(coverage_civ, coverage_iv);
    construct_config::byte_algo_sa = SE_SAIS;
#pragma omp parallel for
    for (size_t i = 0; i < edit_tmpfile_names.size(); ++i) {
        construct(edit_csas[i], edit_tmpfile_names[i], 1);
    }
    // construct the record marker bitvector
    remove_edit_tmpfiles();
    is_compacted = true;
}

void Packer::make_dynamic(void) {
    if (!is_compacted) return;
    // unpack the compact represenation into the countarray
    assert(false); // not implemented
    is_compacted = false;
}

bool Packer::is_dynamic(void) {
    return !is_compacted;
}

void Packer::ensure_edit_tmpfiles_open(void) {
    if (tmpfstreams.empty()) {
        string base = "vg-pack_";
        string edit_tmpfile_name = tmpfilename(base);
        std::remove(edit_tmpfile_name.c_str()); // remove this; we'll use it as a base name
        // for as many bins as we have, make a temp file
        tmpfstreams.resize(n_bins);
        edit_tmpfile_names.resize(n_bins);
        for (size_t i = 0; i < n_bins; ++i) {
            edit_tmpfile_names[i] = edit_tmpfile_name+"_"+convert(i);
            tmpfstreams[i] = new ofstream;
            tmpfstreams[i]->open(edit_tmpfile_names[i], std::ios_base::binary);
            assert(tmpfstreams[i]->is_open());
        }
    }
}

void Packer::close_edit_tmpfiles(void) {
    if (!tmpfstreams.empty()) {
        for (auto& tmpfstream : tmpfstreams) {
            *tmpfstream << delim1; // pad
            tmpfstream->close();
            delete tmpfstream;
        }
        tmpfstreams.clear();
    }
}

void Packer::remove_edit_tmpfiles(void) {
    if (!edit_tmpfile_names.empty()) {
        for (auto& name : edit_tmpfile_names) {
            std::remove(name.c_str());
        }
        edit_tmpfile_names.clear();
    }
}

void Packer::add(const Alignment& aln, bool record_edits) {
    // open tmpfile if needed
    ensure_edit_tmpfiles_open();
    // count the nodes, edges, and edits
    for (auto& mapping : aln.path().mapping()) {
        if (!mapping.has_position()) {
#ifdef debug
            cerr << "Mapping has no position" << endl;
#endif
            continue;
        }
        // skip nodes outside of our graph, assuming this may be a subgraph
        if (!xgidx->has_node(mapping.position().node_id())) {
            continue;
        }
        size_t i = position_in_basis(mapping.position());
        for (auto& edit : mapping.edit()) {
            if (edit_is_match(edit)) {
#ifdef debug
                cerr << "Recording a match" << endl;
#endif
                if (mapping.position().is_reverse()) {
                    for (size_t j = 0; j < edit.from_length(); ++j) {
                        coverage_dynamic.increment(i-j);
                    }
                } else {
                    for (size_t j = 0; j < edit.from_length(); ++j) {
                        coverage_dynamic.increment(i+j);
                    }
                }
            } else if (record_edits) {
                // we represent things on the forward strand
                string pos_repr = pos_key(i);
                string edit_repr = edit_value(edit, mapping.position().is_reverse());
                size_t bin = bin_for_position(i);
                *tmpfstreams[bin] << pos_repr << edit_repr;
            }
            if (mapping.position().is_reverse()) {
                i -= edit.from_length();
            } else {
                i += edit.from_length();
            }
        }
    }
}

// find the position on the forward strand in the sequence vector
size_t Packer::position_in_basis(const Position& pos) const {
    // get position on the forward strand
    if (pos.is_reverse()) {
        return (int64_t)xg_node_start(pos.node_id(), xgidx)
            + (int64_t)reverse(pos, xg_node_length(pos.node_id(), xgidx)).offset() - 1;
    } else {
        return (int64_t)xg_node_start(pos.node_id(), xgidx) + (int64_t)pos.offset();
    }
}

string Packer::pos_key(size_t i) const {
    Position pos;
    size_t offset = 2;
    pos.set_node_id(i+offset);
    string pos_repr;
    pos.SerializeToString(&pos_repr);
    stringstream s;
    s << delim1 << delim2 << delim1 << escape_delims(pos_repr);
    return s.str();
}

string Packer::edit_value(const Edit& edit, bool revcomp) const {
    string edit_repr;
    if (revcomp) {
        reverse_complement_edit(edit).SerializeToString(&edit_repr);
    } else {
        edit.SerializeToString(&edit_repr);
    }
    stringstream s;
    s << delim1 << escape_delims(edit_repr);
    return s.str();
}

string Packer::escape_delims(const string& s) const {
    return escape_delim(escape_delim(s, delim1), delim2);
}

string Packer::unescape_delims(const string& s) const {
    return unescape_delim(unescape_delim(s, delim1), delim2);
}

string Packer::escape_delim(const string& s, char d) const {
    string escaped; escaped.reserve(s.size());
    for (size_t i = 0; i < s.size(); ++i) {
        char c = s[i];
        escaped.push_back(c);
        if (c == d) escaped.push_back(c);
    }
    return escaped;
}

string Packer::unescape_delim(const string& s, char d) const {
    string unescaped; unescaped.reserve(s.size());
    for (size_t i = 0; i < s.size()-1; ++i) {
        char c = s[i];
        char b = s[i+1];
        if (c == d && b == d) {
            unescaped.push_back(c);
        } else {
            unescaped.push_back(c);
            if (i == s.size()-2) unescaped.push_back(b);
        }
    }
    return unescaped;
}

size_t Packer::graph_length(void) const {
    if (is_compacted) {
        return coverage_civ.size();
    } else {
        return coverage_dynamic.size();
    }
}

size_t Packer::coverage_at_position(size_t i) const {
    if (is_compacted) {
        return coverage_civ[i];
    } else {
        return coverage_dynamic[i];
    }
}

vector<Edit> Packer::edits_at_position(size_t i) const {
    vector<Edit> edits;
    if (i == 0) return edits;
    string key = pos_key(i);
    size_t bin = bin_for_position(i);
    auto& edit_csa = edit_csas[bin];
    auto occs = locate(edit_csa, key);
    for (size_t i = 0; i < occs.size(); ++i) {
        // walk from after the key and delim1 to the next end-sep
        size_t b = occs[i] + key.size() + 1;
        size_t e = b;
        // look for an odd number of delims
        // run until we find a delim
        while (true) {
            while (extract(edit_csa, e, e)[0] != delim1) ++e;
            // now we are matching the delim... count them
            size_t f = e;
            while (extract(edit_csa, f, f)[0] == delim1) ++f;
            size_t c = f - e;
            e = f; // set pointer to last delim
            if (c % 2 != 0) {
                break;
            }
        }
        string value = unescape_delims(extract(edit_csa, b, e));
        Edit edit;
        edit.ParseFromString(value);
        edits.push_back(edit);
    }
    return edits;
}

ostream& Packer::as_table(ostream& out, bool show_edits) {
#ifdef debug
    cerr << "Packer table of " << coverage_civ.size() << " rows:" << endl;
#endif

    out << "seq.pos" << "\t"
        << "node.id" << "\t"
        << "node.offset" << "\t"
        << "coverage";
    if (show_edits) out << "\t" << "edits";
    out << endl;
    // write the coverage as a vector
    for (size_t i = 0; i < coverage_civ.size(); ++i) {
        id_t node_id = xgidx->node_at_seq_pos(i+1);
        size_t offset = i - xgidx->node_start(node_id);
        out << i << "\t" << node_id << "\t" << offset << "\t" << coverage_civ[i];
        if (show_edits) {
            out << "\t" << count(edit_csas[bin_for_position(i)], pos_key(i));
            for (auto& edit : edits_at_position(i)) out << " " << pb2json(edit);
        }
        out << endl;
    }
}

ostream& Packer::show_structure(ostream& out) {
    out << coverage_civ << endl; // graph coverage (compacted coverage_dynamic)
    for (auto& edit_csa : edit_csas) {
        out << edit_csa << endl;
    }
    //out << " i SA ISA PSI LF BWT    T[SA[i]..SA[i]-1]" << endl;
    //csXprintf(cout, "%2I %2S %3s %3P %2p %3B   %:3T", edit_csa);
    return out;
}

}
<|endoftext|>


<commit_before>#include "../include/parser.h"
#include "../include/exceptions.h"

#define IGNORE_PARSE_ERROR(FUN, ARG)\
try                                 \
{                                   \
    return FUN(ARG);                \
}                                   \
catch (ParseError e)                \
{                                   \
                                    \
}                                   \

inline char get_char(FileCursor& cursor)
{
    char c = cursor.get();
    if (cursor.eof())
        throw ParseError();
    return c;
}

//   sub-delims    = "!" / "$" / "&" / "'" / "(" / ")"
//                 / "*" / "+" / "," / ";" / "="
FileCursor parse_sub_delims(FileCursor cursor)
{
    char c = get_char(cursor);
    if(c == '!' || c == '$' || c == '&' || c == '\'' || c == '(' || c == ')' ||
        c == '*' || c == '+' || c == ',' || c == ';' || c == '=')
        return cursor;
    else
        throw ParseError();
}

//   gen-delims    = ":" / "/" / "?" / "#" / "[" / "]" / "@"
FileCursor parse_gen_delims(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c == ':' || c == '/' || c == '?' || c == '#' || c == '[' || c == ']' ||
            c =='@')
        return cursor;
    else
        throw ParseError();
}

//   reserved      = gen-delims / sub-delims
FileCursor parse_reserved(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_gen_delims, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    throw ParseError();
}

FileCursor parse_alpha(FileCursor cursor)
{
    char c = get_char(cursor);
    if(('a' <= c && c <= 'z') || ('A' <= c && c <= 'Z'))
        return cursor;
    else
        throw ParseError();
}

FileCursor parse_digit(FileCursor cursor)
{
    char c = get_char(cursor);
    if('0' <= c && c <= '9')
        return cursor;
    else
        throw ParseError();
}

//   unreserved    = ALPHA / DIGIT / "-" / "." / "_" / "~"
FileCursor parse_unreserved(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_alpha, cursor);
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    char c = get_char(cursor);
    if (c == '-' || c == '.' || c == '_' || c == '~')
        return cursor;
    else
        throw ParseError();
}

// hexdigit = DIGIT / 'A' - 'F' / 'a' - 'f'
FileCursor parse_hexdigit(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    char c = get_char(cursor);
    if(('A' <= c && c <= 'F') || ('a' <= c && c <= 'f'))
        return cursor;
    throw ParseError();
}

//   pct-encoded   = "%" HEXDIG HEXDIG
FileCursor parse_pct_encoded(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '%')
        throw ParseError();
    parse_hexdigit(cursor);
    return parse_hexdigit(cursor);
}

//   pchar         = unreserved / pct-encoded / sub-delims / ":" / "@"
FileCursor parse_pchar(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_pct_encoded, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    char c = get_char(cursor);
    if(c == ':' || c == '@')
        return cursor;
    else
        throw ParseError();
}

<commit_msg>Add parser functions.<commit_after>#include "../include/parser.h"
#include "../include/exceptions.h"

#define IGNORE_PARSE_ERROR(FUN, ARG)\
try                                 \
{                                   \
    return FUN(ARG);                \
}                                   \
catch (ParseError e)                \
{                                   \
                                    \
}                                   \

#define REPEAT_IGNORING(FUN, ARG)   \
while(true)                         \
{                                   \
    try                             \
    {                               \
        ARG = FUN(ARG);             \
    }                               \
    catch(ParseError)               \
    {                               \
        return ARG;                 \
    }                               \
}                                   \

inline char get_char(FileCursor& cursor)
{
    char c = cursor.get();
    if (cursor.eof())
        throw ParseError();
    return c;
}

//   sub-delims    = "!" / "$" / "&" / "'" / "(" / ")"
//                 / "*" / "+" / "," / ";" / "="
FileCursor parse_sub_delims(FileCursor cursor)
{
    char c = get_char(cursor);
    if(c == '!' || c == '$' || c == '&' || c == '\'' || c == '(' || c == ')' ||
        c == '*' || c == '+' || c == ',' || c == ';' || c == '=')
        return cursor;
    else
        throw ParseError();
}

//   gen-delims    = ":" / "/" / "?" / "#" / "[" / "]" / "@"
FileCursor parse_gen_delims(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c == ':' || c == '/' || c == '?' || c == '#' || c == '[' || c == ']' ||
            c =='@')
        return cursor;
    else
        throw ParseError();
}

//   reserved      = gen-delims / sub-delims
FileCursor parse_reserved(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_gen_delims, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    throw ParseError();
}

FileCursor parse_alpha(FileCursor cursor)
{
    char c = get_char(cursor);
    if(('a' <= c && c <= 'z') || ('A' <= c && c <= 'Z'))
        return cursor;
    else
        throw ParseError();
}

FileCursor parse_digit(FileCursor cursor)
{
    char c = get_char(cursor);
    if('0' <= c && c <= '9')
        return cursor;
    else
        throw ParseError();
}

//   unreserved    = ALPHA / DIGIT / "-" / "." / "_" / "~"
FileCursor parse_unreserved(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_alpha, cursor);
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    char c = get_char(cursor);
    if (c == '-' || c == '.' || c == '_' || c == '~')
        return cursor;
    else
        throw ParseError();
}

// hexdigit = DIGIT / 'A' - 'F' / 'a' - 'f'
FileCursor parse_hexdigit(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    char c = get_char(cursor);
    if(('A' <= c && c <= 'F') || ('a' <= c && c <= 'f'))
        return cursor;
    throw ParseError();
}

//   pct-encoded   = "%" HEXDIG HEXDIG
FileCursor parse_pct_encoded(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '%')
        throw ParseError();
    parse_hexdigit(cursor);
    return parse_hexdigit(cursor);
}

//   pchar         = unreserved / pct-encoded / sub-delims / ":" / "@"
FileCursor parse_pchar(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_pct_encoded, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    char c = get_char(cursor);
    if(c == ':' || c == '@')
        return cursor;
    else
        throw ParseError();
}

//query_fragment_element = pchar / "/" / "?"
FileCursor parse_query_fragment_element(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_pchar, cursor);
    char c = get_char(cursor);
    if(c == '/' || c == '?')
        return cursor;
    else
        throw ParseError();
}

//query         = *query_fragment_element
//fragment      = *query_fragment_element
FileCursor parse_query_fragment(FileCursor cursor)
{
    REPEAT_IGNORING(parse_query_fragment_element, cursor);
}

//   segment_nz_nc_element = unreserved / pct-encoded / sub-delims / "@"
FileCursor parse_segment_nz_nc_element(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_pct_encoded, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    char c = get_char(cursor);
    if (c == '@')
        return cursor;
    else
        throw ParseError();
}

// segment-nz-nc = 1*segment-nz-nc
FileCursor parse_segment_nz_nc(FileCursor cursor)
{
    cursor = parse_segment_nz_nc_element(cursor);
    REPEAT_IGNORING(parse_segment_nz_nc_element, cursor);
}

//  segment       = *pchar
FileCursor parse_segment(FileCursor cursor)
{
    REPEAT_IGNORING(parse_pchar, cursor);
}

//   segment-nz    = 1*pchar
FileCursor parse_segment_nz(FileCursor cursor)
{
    cursor = parse_pchar(cursor);
    REPEAT_IGNORING(parse_pchar, cursor);
}

//   slash_segment = '/' segment
FileCursor parse_slash_segment(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '/')
        throw ParseError();
    else
        IGNORE_PARSE_ERROR(parse_segment, cursor);
}

//   path-abempty  = *( "/" segment )
FileCursor parse_path_abempty(FileCursor cursor)
{
    REPEAT_IGNORING(parse_slash_segment, cursor);
}

//   path-absolute = "/" [ segment-nz *( "/" segment ) ]
FileCursor parse_path_absolute(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '/')
        throw ParseError();
    else
    {
        try
        {
            FileCursor cursor2(parse_segment_nz(cursor));
            REPEAT_IGNORING(parse_slash_segment, cursor2);
        }
        catch(ParseError)
        {
            return cursor;
        }
    }
}

//   path-noscheme = segment-nz-nc *( "/" segment )
FileCursor parse_path_noscheme(FileCursor cursor)
{
    FileCursor cursor2(parse_segment_nz_nc(cursor));
    REPEAT_IGNORING(parse_slash_segment, cursor2);
}

//   path-rootless = segment-nz *( "/" segment )
FileCursor parse_path_rootless(FileCursor cursor)
{
    FileCursor cursor2(parse_segment_nz(cursor));
    REPEAT_IGNORING(parse_slash_segment, cursor2);
}

//   path-empty    = 0<pchar>
FileCursor parse_path_empty(FileCursor cursor)
{
    return cursor;
}

//   path          = path-abempty    ; begins with "/" or is empty
//                 / path-absolute   ; begins with "/" but not "//"
//                 / path-noscheme   ; begins with a non-colon segment
//                 / path-rootless   ; begins with a segment
//                 / path-empty      ; zero characters
FileCursor parse_path(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_path_abempty, cursor);
    IGNORE_PARSE_ERROR(parse_path_absolute, cursor);
    IGNORE_PARSE_ERROR(parse_path_noscheme, cursor);
    IGNORE_PARSE_ERROR(parse_path_rootless, cursor);
    IGNORE_PARSE_ERROR(parse_path_empty, cursor);
    throw ParseError();
}
// 250_255 = "25" 0-5
FileCursor parse_250_255(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '2')
        throw ParseError();
    c = get_char(cursor);
    if (c != '5')
        throw ParseError();
    c = get_char(cursor);
    if ('0' <= c && c <= '5')
        return cursor;
    else
        throw ParseError();
}

// 200_249 = "2" 0-4 DIGIT
FileCursor parse_200_249(FileCursor cursor)
{
    char c = get_char(cursor);
    if (c != '2')
        throw ParseError();
    c = get_char(cursor);
    if ('0' <= c && c <= '4')
    {
        return parse_digit(cursor);
    }
    else
        throw ParseError();
}

// 100_199 = "1" DIGIT DIGIT
FileCursor parse_100_199(FileCursor cursor)
{
    char c = get_char(cursor);
    if(c != '1')
        throw ParseError();
    return parse_digit(parse_digit(cursor));
}

// 10_99 = 1-9 DIGIT
FileCursor parse_10_99(FileCursor cursor)
{
    char c = get_char(cursor);
    if ('1' <= c && c <= '9')
    {
        return parse_digit(cursor);
    }
    else
        throw ParseError();
}

//dec-octet     = DIGIT          ; 0-9
//                 / 10_99       ; 10-99
//                 / 100_199     ; 100-199
//                 / 200_249     ; 200-249
//                 / 250_255     ; 250-255

FileCursor parse_dec_octet(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_250_255, cursor);
    IGNORE_PARSE_ERROR(parse_200_249, cursor);
    IGNORE_PARSE_ERROR(parse_100_199, cursor);
    IGNORE_PARSE_ERROR(parse_10_99, cursor);
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    throw ParseError();
}

//   reg_name_segment = *( unreserved / pct-encoded / sub-delims )
FileCursor parse_reg_name_segment(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_pct_encoded, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    throw ParseError();
}

// reg_name = *(reg_name_segment)
FileCursor parse_reg_name(FileCursor cursor)
{
    REPEAT_IGNORING(parse_reg_name_segment, cursor);
}

//   IPv4address   = dec-octet "." dec-octet "." dec-octet "." dec-octet
FileCursor parse_ipv4address(FileCursor cursor)
{
    for (int i=0; i<3; i++)
    {
        cursor = parse_dec_octet(cursor);
        if (get_char(cursor) != '.')
            throw ParseError();
    }
    return parse_dec_octet(cursor);
}

//   h16           = 1*4HEXDIG
FileCursor parse_h16(FileCursor cursor)
{
    cursor = parse_hexdigit(cursor);
    for (int i=0; i<3; i++)
    {
        try
        {
            cursor = parse_hexdigit(cursor);
        }
        catch(ParseError)
        {
            return cursor;
        }
    }
    return cursor;
}

//   ls32          = ( h16 ":" h16 ) / IPv4address
FileCursor parse_ls32(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_ipv4address, cursor);
    cursor = parse_h16(cursor);
    if (get_char(cursor) != ':')
        throw ParseError();
    return parse_h16(cursor);
}

// h16_colon = h16 ':'
FileCursor parse_h16_colon(FileCursor cursor)
{
    cursor = parse_h16(cursor);
    if(get_char(cursor) == ':')
        return cursor;
    else
        throw ParseError();
}

FileCursor parse_n_h16_colon(FileCursor cursor, int n)
{
    for (int i=0; i<n; i++)
    {
        cursor = parse_h16_colon(cursor);
    }
    return cursor;
}

FileCursor parse_lt_n_h16_colon(FileCursor cursor, int n)
{
    for (int i=0; i<n; i++)
    {
        try
        {
            cursor = parse_h16_colon(cursor);
        }
        catch(ParseError)
        {
            return cursor;
        }
    }
    return cursor;
}

// double_colon = ":" ":"
FileCursor parse_double_colon(FileCursor cursor)
{
    if (get_char(cursor) != ':')
        throw ParseError();
    else
        if (get_char(cursor) != ':')
            throw ParseError();
        else
            return cursor;
}

//   IPv6address1   =                           6( h16 ":" ) ls32
//                 /                       "::" 5( h16 ":" ) ls32
//                 / [               h16 ] "::" 4( h16 ":" ) ls32
//                 / [ *1( h16 ":" ) h16 ] "::" 3( h16 ":" ) ls32
//                 / [ *2( h16 ":" ) h16 ] "::" 2( h16 ":" ) ls32
//                 / [ *3( h16 ":" ) h16 ] "::"    h16 ":"   ls32
//                 / [ *4( h16 ":" ) h16 ] "::"              ls32
//                 / [ *5( h16 ":" ) h16 ] "::"              h16
//                 / [ *6( h16 ":" ) h16 ] "::"
FileCursor parse_ipv6address_case(FileCursor cursor, int n)
{
    if (n > 1)
    {

        try
        {
            cursor = parse_h16(parse_lt_n_h16_colon(cursor, n - 2));
        }
        catch(ParseError)
        {
        }
    }
    if (n > 0)
        cursor = parse_double_colon(cursor);

    if (6 - n > 0)
        cursor = parse_n_h16_colon(cursor, 6 - n);

    if (n < 7)
        cursor = parse_ls32(cursor);
    else if ( n == 7)
        cursor = parse_h16(cursor);

    return cursor;
}

FileCursor parse_ipv6address(FileCursor cursor)
{
    for (int i=0; i<9; i++)
    {
        try
        {
            return parse_ipv6address_case(cursor, i);
        }
        catch(ParseError)
        {
        }
    }
    throw ParseError();
}

FileCursor parse_unreserved_subdelims_colon(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    if(get_char(cursor) == ':')
        return cursor;
    else
        throw ParseError();
}

FileCursor parse_at_least_one_unreserved_subdelims_colon(FileCursor cursor)
{
    cursor = parse_unreserved_subdelims_colon(cursor);
    REPEAT_IGNORING(parse_unreserved, cursor);
}

FileCursor parse_at_least_one_hexdigit(FileCursor cursor)
{
    cursor = parse_hexdigit(cursor);
    REPEAT_IGNORING(parse_hexdigit, cursor);
}

//IPvFuture = "v" 1*HEXDIG "." 1*( unreserved / sub-delims / ":" )
FileCursor parse_ipvfuture(FileCursor cursor)
{
    if(get_char(cursor) != 'v')
        throw ParseError();
    cursor = parse_at_least_one_hexdigit(cursor);
    if(get_char(cursor) != '.')
        throw ParseError();
    return parse_at_least_one_unreserved_subdelims_colon(cursor);
}

//ipv6address_ipfuture =  IPv6address / IPvFuture
FileCursor parse_ipv6address_ipfuture(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_ipv6address, cursor);
    IGNORE_PARSE_ERROR(parse_ipvfuture, cursor);
    throw ParseError();
}

//   IP-literal    = "[" ipv6address_ipfuture "]"
FileCursor parse_ip_literal(FileCursor cursor)
{
    if(get_char(cursor) != '[')
        throw ParseError();
    cursor = parse_ipv6address_ipfuture(cursor);
    if(get_char(cursor) == ']')
        return cursor;
    else
        throw ParseError();
}

//port          = *DIGIT
FileCursor parse_port(FileCursor cursor)
{
    REPEAT_IGNORING(parse_digit, cursor);
}

//host = IP-literal / IPv4address / reg-name
FileCursor parse_host(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_ip_literal, cursor);
    IGNORE_PARSE_ERROR(parse_ipv4address, cursor);
    IGNORE_PARSE_ERROR(parse_reg_name, cursor);
    throw ParseError();
}

//userinfo_segment = unreserved / pct-encoded / sub-delims / ":"
FileCursor parse_userinfo_segment(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_unreserved, cursor);
    IGNORE_PARSE_ERROR(parse_pct_encoded, cursor);
    IGNORE_PARSE_ERROR(parse_sub_delims, cursor);
    if(get_char(cursor) == ':')
        return cursor;
    else
        throw ParseError();
}

//userinfo      = *user_info_segment
FileCursor parse_userinfo(FileCursor cursor)
{
    REPEAT_IGNORING(parse_userinfo_segment, cursor);
}

//authority     = [ userinfo "@" ] host [ ":" port ]
FileCursor parse_authority(FileCursor cursor)
{
    try
    {
        FileCursor copy(parse_userinfo(cursor));
        if(get_char(copy) != '@')
            throw ParseError();
        cursor = copy;
    }
    catch(ParseError)
    {
    }
    cursor = parse_host(cursor);
    try
    {
        FileCursor copy2(cursor);
        if(get_char(copy2) != ':')
            throw ParseError();
        cursor = parse_port(copy2);
    }
    catch(ParseError)
    {
    }
    return cursor;
}

//parse_hier_part_case = "//" authority path-abempty
FileCursor parse_hier_part_case(FileCursor cursor)
{
    if (get_char(cursor) != '/')
        throw ParseError();
    if (get_char(cursor) != '/')
        throw ParseError();
    cursor = parse_authority(cursor);
    return parse_path_abempty(cursor);
}

//hier-part     = hier_part_case
//              / path-absolute
//              / path-rootless
//              / path-empty
FileCursor parse_hier_part(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_hier_part_case, cursor);
    IGNORE_PARSE_ERROR(parse_path_absolute, cursor);
    IGNORE_PARSE_ERROR(parse_path_rootless, cursor);
    IGNORE_PARSE_ERROR(parse_path_empty, cursor);
    throw ParseError();
}

// scheme_element =  ALPHA / DIGIT / "+" / "-" / "."
FileCursor parse_scheme_element(FileCursor cursor)
{
    IGNORE_PARSE_ERROR(parse_alpha, cursor);
    IGNORE_PARSE_ERROR(parse_digit, cursor);
    char c = get_char(cursor);
    if (c == '+' || c == '-' || c == '.')
        return cursor;
    else
        throw ParseError();
}

//scheme        = ALPHA *scheme_element
FileCursor parse_scheme(FileCursor cursor)
{
    cursor = parse_alpha(cursor);
    REPEAT_IGNORING(parse_scheme_element, cursor);
}

//uri = scheme ":" hier-part [ "?" query ] [ "#" fragment ]
FileCursor parse_uri(FileCursor cursor)
{
    cursor = parse_scheme(cursor);
    if(get_char(cursor) != ':')
        throw ParseError();
    cursor = parse_hier_part(cursor);
    try
    {
        FileCursor copy(cursor);
        if (get_char(copy) != '?')
            throw ParseError();
        cursor = parse_query_fragment(copy);
    }
    catch(ParseError)
    {
    }
    try
    {
        FileCursor copy2(cursor);
        if (get_char(copy2) != '#')
            throw ParseError();
        cursor = parse_query_fragment(cursor);
    }
    catch(ParseError)
    {
    }
    return cursor;
}
<|endoftext|>