<commit_before>#pragma once

#include <vector>

#include "value.hpp"

namespace {

enum class character_class
{
    literal,
    whitespace,
    quote,
    structure,
    invalid
};

character_class classify(char c)
{
    switch (c)
    {
        case ' ':
        case '\t':
        case '\r':
        case '\n':
            return character_class::whitespace;
        case '"':
            return character_class::quote;
        case '[':
        case ',':
        case ']':
        case '{':
        case ':':
        case '}':
            return character_class::structure;
        default:
            return character_class::literal;
    }
}

uint8_t hex2int(char c)
{
    if (c >= 'A' && c <= 'F')
    {
        return 10 + (c - 'A');
    }
    else if (c >= 'a' && c <= 'f')
    {
        return 10 + (c - 'a');
    }
    else if (c >= '0' && c <= '9')
    {
        return c - '0';
    }

    throw std::runtime_error("bad hex char");
}

}

namespace rana {

class parse_error : public std::runtime_error
{
public:
    parse_error(std::size_t line, std::size_t column, const char *buffer, std::size_t buffer_length, const std::string &message) : std::runtime_error(create_message(line, column, buffer, buffer_length, message))
    {
    }

private:
	static std::string create_message(std::size_t line, std::size_t column, const char *buffer, std::size_t buffer_length, const std::string &message)
    {
        std::string error_message = "during parsing of JSON string at line " + std::to_string(line) + ", column " + std::to_string(column) + ": ";
        error_message += " error: " + message + "\n";

        int current_line = 1;
        int i = 0;
        int line_start = 0;
        int line_end = 0;

        while (current_line <= line && i < buffer_length)
        {
            i++;

            if (buffer[i] == '\n')
            {
                current_line++;
                line_start = i;
            }
        }

        while (i < buffer_length)
        {
            if (buffer[i] == '\n')
            {
                line_end = i;
                break;
            }

            i++;
            line_end = i;
        }

        error_message += std::string(buffer + line_start, buffer + line_end);
        error_message += "\n" + std::string(column == 0 ? 0 : column - 1, ' ') + '^';

        return error_message;
    }
};

class parser
{
public:
	parser() : buffer_(nullptr), buffer_position_(0), line_(1), column_(0), root_(nullptr), popped_container_(false), state_(state::normal), surrogate_pair_({ 0, 0 }), unicode_nibble_(0)
    {
    }

	~parser() = default;

	void parse_stream(std::istream &stream, value &result)
	{
		result = value::invalid();
		root_ = &result;
		stack_.push_back(root_);

		if (stream)
		{
			char c;
			bool still_parsing;

			do
			{
				stream.get(c);
				still_parsing = scan(c);
			} while (still_parsing && stream);
		}
	}

    void parse_string(const char *string, std::size_t string_length, value &result);

	void parse_string(const std::string &string, value &result)
    {
        parser p;

        p.set_buffer(string.data(), string.size());
        p.parse_next(result);

        if (result.is_valid())
        {
            value next;
            p.parse_next(next);

            if (next.is_valid())
            {
                throw p.make_error("found multiple top-level JSON values");
            }

            if (p.buffer_length_ > p.buffer_position_)
            {
                throw p.make_error("found invalid trailing characters in JSON string");
            }

            return;
        }

        throw p.make_error("bad JSON");
    }

    void parse_file(const std::string &filename, value &result)
    {
        std::fstream file(filename, std::ios::in);
        std::stringstream ss;
        ss << file.rdbuf();
        parse_string(ss.str(), result);
    }

private:
	void parse_next(value &result)
    {
        result = value::invalid();
        root_ = &result;
        stack_.push_back(root_);

        if (buffer_position_ < buffer_length_)
        {
            char c;
            bool still_parsing;

            do
            {
                c = buffer_[buffer_position_++];
                still_parsing = scan(c);
            } while (still_parsing);
        }
    }

	void set_buffer(const char *buffer, std::size_t buffer_length)
    {
        buffer_ = buffer;
        buffer_length_ = buffer_length;
        buffer_position_ = 0;
    }

	void handle_structural_token(char c)
    {
        switch (c)
        {
            case '[':
            {
                if (stack_.size() == 1 && !root_->is_valid())
                {
                    *root_ = value::array();
                }
                else
                {
		    push(value::array().copy());
                }

                popped_container_ = false;
                break;
            }
            case ']':
            {
                if (!top().is_array())
                {
                    throw make_error("unmatched ]");
                }

                if (!token_string_.empty())
                {
                    top().emplace_array(parse_token());
                }

                stack_.pop_back();
                popped_container_ = true;
                break;
            }
            case '{':
            {
                if (stack_.size() == 1 && !root_->is_valid())
                {
                    *root_ = value::object();
                }
                else
                {
		    push(value::object().copy());
                }

                popped_container_ = false;
                break;
            }
            case '}':
            {
                if (!top().is_object())
                {
                    throw make_error("unmatched }");
                }

                if (!token_string_.empty())
                {
                    if (!key_.first)
                    {
                        throw make_error("Objects must consist of key:value pairs");
                    }

		    push(parse_token());
                }
                else if (key_.first)
                {
                    throw std::runtime_error("missing value after :");
                }

                stack_.pop_back();
                popped_container_ = true;
                break;
            }
            case ',':
            {
                if (!token_string_.empty())
                {
		    push(parse_token());
                }
                else if (!popped_container_)
                {
                    throw make_error("Expected value before ','");
                }

                popped_container_ = false;
                break;
            }
            case ':':
            {
                if (stack_.empty() || !top().is_object())
                {
                    throw make_error(": not part of an object");
                }

                key_.first = true;
                key_.second = parse_string();

                popped_container_ = false;
                break;
            }
        }
    }

    value parse_token()
    {
        switch (token_string_[0])
        {
            case '"':
                return value(parse_string());
            case 't':
            case 'f':
            case 'n':
                return parse_literal();
        }

        return parse_number();
    }

	value parse_number()
    {
        try
        {
            double number = std::stod(token_string_);
            token_string_ = "";
            return value(number);
        }
        catch (std::invalid_argument)
        {
            throw make_error("expected a number, found \"" + token_string_ + "\"");
        }
    }

	value parse_literal()
    {
        if (token_string_[0] == 't')
        {
            if (token_string_ != "true")
            {
                throw make_error("expected true");
            }

            token_string_ = "";
            return value(true);
        }

        if (token_string_[0] == 'f')
        {
            if (token_string_ != "false")
            {
                throw make_error("expected false");
            }

            token_string_ = "";
            return value(false);
        }

        if (token_string_[0] == 'n')
        {
            if (token_string_ != "null")
            {
                throw make_error("expected null");
            }

            token_string_ = "";
            return value::null();
        }

        throw make_error("expected one of: [true, false, null]");
    }

	std::string parse_string()
    {
        if (token_string_.front() != '"' || token_string_.back() != '"')
        {
            throw std::runtime_error("expected string surrounded in quotes, found " + token_string_);
        }

        std::string string(token_string_.substr(1, token_string_.size() - 2));
        token_string_ = "";

        return string;
    }

	bool scan(char c)
    {
        if (c == '\0')
        {
            return false;
        }

        column_++;

        if (c == '\n')
        {
            line_++;
            column_ = 0;
        }

        if (state_ == state::normal)
        {
            switch (classify(c))
            {
                case character_class::literal:
                    token_string_.append(1, c);
                    break;
                case character_class::whitespace:
                    break;
                case character_class::quote:
                    token_string_.append(1, '"');
                    state_ = state::string;
                    break;
                case character_class::structure:
                    handle_structural_token(c);
                    break;
                case character_class::invalid:
                    throw make_error("Invalid character");
            }
        }
        else if (state_ == state::string)
        {
            if (c == '\\' && state_ != state::string_escape)
            {
                state_ = state::string_escape;
            }
            else if (c == '"')
            {
                token_string_.append(1, '"');

                if (state_ == state::string_escape)
                {
                    state_ = state::string;
                }
                else
                {
                    state_ = state::normal;
                }
            }
            else
            {
                if (unicode_nibble_ > 0)
                {
                    if (unicode_nibble_ == 4)
                    {
                        if (surrogate_pair_.first <= 0x7F)
                        {
                            token_string_.append(1, surrogate_pair_.first & 0xFF);
                        }
                        else
                        {

                        }
                    }

                    surrogate_pair_ = { 0, 0 };
                    unicode_nibble_ = 0;
                }

                unsigned char u = c;

                if(u <= 0x7F)
                {
                    token_string_.append(1, c);
                }
                else
                {
                    token_string_.append(1, 0xC0 + (u >> 6));
                    token_string_.append(1, 0x80 + (u & 0x3F));
                }

                state_ = state::string;
            }
        }
        else if (state_ == state::string_escape)
        {
            if (c == 'u')
            {
                state_ = state::unicode;
            }
            else
            {
                if (c == 'n')
                {
                    token_string_.append(1, '\n');
                }
                else if (c == 'r')
                {
                    token_string_.append(1, '\r');
                }
                else if (c == '\\')
                {
                    token_string_.append(1, '\\');
                }
                else if (c == '"')
                {
                    token_string_.append(1, '"');
                }
                else if (c == '/')
                {
                    token_string_.append(1, '/');
                }
                else if (c == 'f')
                {
                    token_string_.append(1, '\f');
                }
                else if (c == 'b')
                {
                    token_string_.append(1, '\b');
                }
                else if (c == 't')
                {
                    token_string_.append(1, '\t');
                }
                else
                {
                    throw std::runtime_error("");
                }

                state_ = state::string;
            }
        }
        else if (state_ == state::unicode)
        {
            if (unicode_nibble_++ < 4)
            {
                surrogate_pair_.first <<= 8;
                surrogate_pair_.first += hex2int(c);
            }
            else
            {
                surrogate_pair_.second <<= 8;
                surrogate_pair_.second += hex2int(c);
            }

            if (unicode_nibble_ % 4 == 0)
            {
                state_ = state::string;
            }
        }

        if (stack_.empty())
        {
            return false;
        }

        return true;
    }

	parse_error make_error(const std::string &message)
    {
        return parse_error(line_, column_, buffer_, buffer_length_, message);
    }

	void push(value &&v)
    {
        if (stack_.empty())
        {
            throw make_error("stack empty");
        }

        if (!top().is_container())
        {
            throw make_error("not container");
        }

        value *inserted = nullptr;

        if (top().is_object())
        {
            if (!key_.first)
            {
                throw make_error("missing key for value");
            }

            auto inserted_iter = top().emplace_object(std::move(key_.second), parse_token()).first;
            inserted = &inserted_iter->second;
            key_.first = false;
            key_.second.clear();
        }
        else if (top().is_array())
        {
            inserted = &*top().emplace_array(parse_token());
        }
        else
        {
            throw std::runtime_error("top of stack not container");
        }

        if (inserted == nullptr)
        {
            throw "";
        }

        if (inserted->is_container())
        {
            stack_.push_back(inserted);
        }
    }

	value &top() { return *stack_.back(); }

	const char *buffer_;
	std::size_t buffer_length_;
	std::size_t buffer_position_;
	std::vector<value *> stack_;
	std::size_t line_;
	std::size_t column_;
	std::string token_string_;
	value *root_;
	std::pair<bool, std::string> key_;
	bool popped_container_;
	std::pair<uint32_t, uint32_t> surrogate_pair_;
	std::size_t unicode_nibble_;

	enum class state
	{
		normal,
		string,
		string_escape,
		unicode
	};

	state state_;
};

}
<commit_msg>fix push argument<commit_after>#pragma once

#include <vector>

#include "value.hpp"

namespace {

enum class character_class
{
    literal,
    whitespace,
    quote,
    structure,
    invalid
};

character_class classify(char c)
{
    switch (c)
    {
        case ' ':
        case '\t':
        case '\r':
        case '\n':
            return character_class::whitespace;
        case '"':
            return character_class::quote;
        case '[':
        case ',':
        case ']':
        case '{':
        case ':':
        case '}':
            return character_class::structure;
        default:
            return character_class::literal;
    }
}

uint8_t hex2int(char c)
{
    if (c >= 'A' && c <= 'F')
    {
        return 10 + (c - 'A');
    }
    else if (c >= 'a' && c <= 'f')
    {
        return 10 + (c - 'a');
    }
    else if (c >= '0' && c <= '9')
    {
        return c - '0';
    }

    throw std::runtime_error("bad hex char");
}

}

namespace rana {

class parse_error : public std::runtime_error
{
public:
    parse_error(std::size_t line, std::size_t column, const char *buffer, std::size_t buffer_length, const std::string &message) : std::runtime_error(create_message(line, column, buffer, buffer_length, message))
    {
    }

private:
	static std::string create_message(std::size_t line, std::size_t column, const char *buffer, std::size_t buffer_length, const std::string &message)
    {
        std::string error_message = "during parsing of JSON string at line " + std::to_string(line) + ", column " + std::to_string(column) + ": ";
        error_message += " error: " + message + "\n";

        int current_line = 1;
        int i = 0;
        int line_start = 0;
        int line_end = 0;

        while (current_line <= line && i < buffer_length)
        {
            i++;

            if (buffer[i] == '\n')
            {
                current_line++;
                line_start = i;
            }
        }

        while (i < buffer_length)
        {
            if (buffer[i] == '\n')
            {
                line_end = i;
                break;
            }

            i++;
            line_end = i;
        }

        error_message += std::string(buffer + line_start, buffer + line_end);
        error_message += "\n" + std::string(column == 0 ? 0 : column - 1, ' ') + '^';

        return error_message;
    }
};

class parser
{
public:
	parser() : buffer_(nullptr), buffer_position_(0), line_(1), column_(0), root_(nullptr), popped_container_(false), state_(state::normal), surrogate_pair_({ 0, 0 }), unicode_nibble_(0)
    {
    }

	~parser() = default;

	void parse_stream(std::istream &stream, value &result)
	{
		result = value::invalid();
		root_ = &result;
		stack_.push_back(root_);

		if (stream)
		{
			char c;
			bool still_parsing;

			do
			{
				stream.get(c);
				still_parsing = scan(c);
			} while (still_parsing && stream);
		}
	}

    void parse_string(const char *string, std::size_t string_length, value &result);

	void parse_string(const std::string &string, value &result)
    {
        parser p;

        p.set_buffer(string.data(), string.size());
        p.parse_next(result);

        if (result.is_valid())
        {
            value next;
            p.parse_next(next);

            if (next.is_valid())
            {
                throw p.make_error("found multiple top-level JSON values");
            }

            if (p.buffer_length_ > p.buffer_position_)
            {
                throw p.make_error("found invalid trailing characters in JSON string");
            }

            return;
        }

        throw p.make_error("bad JSON");
    }

    void parse_file(const std::string &filename, value &result)
    {
        std::fstream file(filename, std::ios::in);
        std::stringstream ss;
        ss << file.rdbuf();
        parse_string(ss.str(), result);
    }

private:
	void parse_next(value &result)
    {
        result = value::invalid();
        root_ = &result;
        stack_.push_back(root_);

        if (buffer_position_ < buffer_length_)
        {
            char c;
            bool still_parsing;

            do
            {
                c = buffer_[buffer_position_++];
                still_parsing = scan(c);
            } while (still_parsing);
        }
    }

	void set_buffer(const char *buffer, std::size_t buffer_length)
    {
        buffer_ = buffer;
        buffer_length_ = buffer_length;
        buffer_position_ = 0;
    }

	void handle_structural_token(char c)
    {
        switch (c)
        {
            case '[':
            {
                if (stack_.size() == 1 && !root_->is_valid())
                {
                    *root_ = value::array();
                }
                else
                {
		    push(value::array().copy());
                }

                popped_container_ = false;
                break;
            }
            case ']':
            {
                if (!top().is_array())
                {
                    throw make_error("unmatched ]");
                }

                if (!token_string_.empty())
                {
                    top().emplace_array(parse_token());
                }

                stack_.pop_back();
                popped_container_ = true;
                break;
            }
            case '{':
            {
                if (stack_.size() == 1 && !root_->is_valid())
                {
                    *root_ = value::object();
                }
                else
                {
		    push(value::object().copy());
                }

                popped_container_ = false;
                break;
            }
            case '}':
            {
                if (!top().is_object())
                {
                    throw make_error("unmatched }");
                }

                if (!token_string_.empty())
                {
                    if (!key_.first)
                    {
                        throw make_error("Objects must consist of key:value pairs");
                    }

		    push(parse_token());
                }
                else if (key_.first)
                {
                    throw std::runtime_error("missing value after :");
                }

                stack_.pop_back();
                popped_container_ = true;
                break;
            }
            case ',':
            {
                if (!token_string_.empty())
                {
		    push(parse_token());
                }
                else if (!popped_container_)
                {
                    throw make_error("Expected value before ','");
                }

                popped_container_ = false;
                break;
            }
            case ':':
            {
                if (stack_.empty() || !top().is_object())
                {
                    throw make_error(": not part of an object");
                }

                key_.first = true;
                key_.second = parse_string();

                popped_container_ = false;
                break;
            }
        }
    }

    value parse_token()
    {
        switch (token_string_[0])
        {
            case '"':
                return value(parse_string());
            case 't':
            case 'f':
            case 'n':
                return parse_literal();
        }

        return parse_number();
    }

	value parse_number()
    {
        try
        {
            double number = std::stod(token_string_);
            token_string_ = "";
            return value(number);
        }
        catch (std::invalid_argument)
        {
            throw make_error("expected a number, found \"" + token_string_ + "\"");
        }
    }

	value parse_literal()
    {
        if (token_string_[0] == 't')
        {
            if (token_string_ != "true")
            {
                throw make_error("expected true");
            }

            token_string_ = "";
            return value(true);
        }

        if (token_string_[0] == 'f')
        {
            if (token_string_ != "false")
            {
                throw make_error("expected false");
            }

            token_string_ = "";
            return value(false);
        }

        if (token_string_[0] == 'n')
        {
            if (token_string_ != "null")
            {
                throw make_error("expected null");
            }

            token_string_ = "";
            return value::null();
        }

        throw make_error("expected one of: [true, false, null]");
    }

	std::string parse_string()
    {
        if (token_string_.front() != '"' || token_string_.back() != '"')
        {
            throw std::runtime_error("expected string surrounded in quotes, found " + token_string_);
        }

        std::string string(token_string_.substr(1, token_string_.size() - 2));
        token_string_ = "";

        return string;
    }

	bool scan(char c)
    {
        if (c == '\0')
        {
            return false;
        }

        column_++;

        if (c == '\n')
        {
            line_++;
            column_ = 0;
        }

        if (state_ == state::normal)
        {
            switch (classify(c))
            {
                case character_class::literal:
                    token_string_.append(1, c);
                    break;
                case character_class::whitespace:
                    break;
                case character_class::quote:
                    token_string_.append(1, '"');
                    state_ = state::string;
                    break;
                case character_class::structure:
                    handle_structural_token(c);
                    break;
                case character_class::invalid:
                    throw make_error("Invalid character");
            }
        }
        else if (state_ == state::string)
        {
            if (c == '\\' && state_ != state::string_escape)
            {
                state_ = state::string_escape;
            }
            else if (c == '"')
            {
                token_string_.append(1, '"');

                if (state_ == state::string_escape)
                {
                    state_ = state::string;
                }
                else
                {
                    state_ = state::normal;
                }
            }
            else
            {
                if (unicode_nibble_ > 0)
                {
                    if (unicode_nibble_ == 4)
                    {
                        if (surrogate_pair_.first <= 0x7F)
                        {
                            token_string_.append(1, surrogate_pair_.first & 0xFF);
                        }
                        else
                        {

                        }
                    }

                    surrogate_pair_ = { 0, 0 };
                    unicode_nibble_ = 0;
                }

                unsigned char u = c;

                if(u <= 0x7F)
                {
                    token_string_.append(1, c);
                }
                else
                {
                    token_string_.append(1, 0xC0 + (u >> 6));
                    token_string_.append(1, 0x80 + (u & 0x3F));
                }

                state_ = state::string;
            }
        }
        else if (state_ == state::string_escape)
        {
            if (c == 'u')
            {
                state_ = state::unicode;
            }
            else
            {
                if (c == 'n')
                {
                    token_string_.append(1, '\n');
                }
                else if (c == 'r')
                {
                    token_string_.append(1, '\r');
                }
                else if (c == '\\')
                {
                    token_string_.append(1, '\\');
                }
                else if (c == '"')
                {
                    token_string_.append(1, '"');
                }
                else if (c == '/')
                {
                    token_string_.append(1, '/');
                }
                else if (c == 'f')
                {
                    token_string_.append(1, '\f');
                }
                else if (c == 'b')
                {
                    token_string_.append(1, '\b');
                }
                else if (c == 't')
                {
                    token_string_.append(1, '\t');
                }
                else
                {
                    throw std::runtime_error("");
                }

                state_ = state::string;
            }
        }
        else if (state_ == state::unicode)
        {
            if (unicode_nibble_++ < 4)
            {
                surrogate_pair_.first <<= 8;
                surrogate_pair_.first += hex2int(c);
            }
            else
            {
                surrogate_pair_.second <<= 8;
                surrogate_pair_.second += hex2int(c);
            }

            if (unicode_nibble_ % 4 == 0)
            {
                state_ = state::string;
            }
        }

        if (stack_.empty())
        {
            return false;
        }

        return true;
    }

	parse_error make_error(const std::string &message)
    {
        return parse_error(line_, column_, buffer_, buffer_length_, message);
    }

	void push(value &&v)
    {
        if (stack_.empty())
        {
            throw make_error("stack empty");
        }

        if (!top().is_container())
        {
            throw make_error("not container");
        }

        value *inserted = nullptr;

        if (top().is_object())
        {
            if (!key_.first)
            {
                throw make_error("missing key for value");
            }

            auto inserted_iter = top().emplace_object(std::move(key_.second), std::move(v)).first;
            inserted = &inserted_iter->second;
            key_.first = false;
            key_.second.clear();
        }
        else if (top().is_array())
        {
            inserted = &*top().emplace_array(std::move(v));
        }
        else
        {
            throw std::runtime_error("top of stack not container");
        }

        if (inserted == nullptr)
        {
            throw "";
        }

        if (inserted->is_container())
        {
            stack_.push_back(inserted);
        }
    }

	value &top() { return *stack_.back(); }

	const char *buffer_;
	std::size_t buffer_length_;
	std::size_t buffer_position_;
	std::vector<value *> stack_;
	std::size_t line_;
	std::size_t column_;
	std::string token_string_;
	value *root_;
	std::pair<bool, std::string> key_;
	bool popped_container_;
	std::pair<uint32_t, uint32_t> surrogate_pair_;
	std::size_t unicode_nibble_;

	enum class state
	{
		normal,
		string,
		string_escape,
		unicode
	};

	state state_;
};

}
<|endoftext|>


<commit_before>#include "io.h"

#include "../../NULLC/nullc.h"

#if defined(_MSC_VER)
	#include <windows.h>
#endif

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

#if defined(_MSC_VER)
	#pragma warning(disable: 4996)
#endif

namespace NULLCIO
{
	void *contextFunc = NULL;
	unsigned (*writeFunc)(void *context, char *data, unsigned length) = NULL;
	unsigned (*readFunc)(void *context, char *target, unsigned length) = NULL;

	int	SafeSprintf(char* dst, size_t size, const char* src, ...)
	{
		if(size == 0)
			return 0;

		va_list args;
		va_start(args, src);

		int result = vsnprintf(dst, size, src, args);
		dst[size-1] = '\0';

		va_end(args);

		return (result == -1 || (size_t)result >= size) ? (int)size : result;
	}

	void DefaultInitialize()
	{
#if defined(_MSC_VER)
		static bool initialized = false;

		if(!initialized)
		{
			AllocConsole();

			freopen("CONOUT$", "w", stdout);
			freopen("CONIN$", "r", stdin);

			initialized = true;
		}
#endif
	}

	unsigned DefaultWrite(void *context, char *data, unsigned length)
	{
		(void)context;

		DefaultInitialize();

		return (unsigned)fwrite(data, 1, length, stdout);
	}

	unsigned DefaultRead(void *context, char *target, unsigned length)
	{
		(void)context;

		DefaultInitialize();

		return (unsigned)fread(target, 1, length, stdin);
	}

	void ReadString(char *buf, unsigned length)
	{
		if(!length)
			return;

		*buf = 0;

		char *pos = buf;
		char ch = 0;

		while(readFunc(contextFunc, &ch, 1))
		{
			if(ch == '\n')
				break;

			*pos++ = ch;

			if(pos + 1 == buf + length)
				break;
		}

		*pos = 0;
	}

	int abs(int x){ return x < 0 ? -x : x; }

	void WriteToConsole(NULLCArray data)
	{
		if(!writeFunc)
			return;

		// Empty arrays are silently ignored
		if(!data.ptr)
			return;

		for(unsigned i = 0; i < data.len; i++)
		{
			if(!data.ptr[i])
				return;

			writeFunc(contextFunc, data.ptr + i, 1);
		}
	}

	void WriteLongConsole(long long number, int base)
	{
		if(!(base > 1 && base <= 16))
		{
			nullcThrowError("ERROR: incorrect base %d", base);
			return;
		}

		if(!writeFunc)
			return;

		static char symb[] = "0123456789abcdef";
		bool sign = 0;
		char buf[128];
		char *curr = buf;
		if(number < 0)
			sign = 1;

		*curr++ = *(abs(number % base) + symb);
		while(number /= base)
			*curr++ = *(abs(number % base) + symb);
		if(sign)
			*curr++ = '-';
		*curr = 0;
		int size = int(curr - buf), halfsize = size >> 1;
		for(int i = 0; i < halfsize; i++)
		{
			char tmp = buf[i];
			buf[i] = buf[size-i-1];
			buf[size-i-1] = tmp;
		}

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void WriteIntConsole(int number, int base)
	{
		WriteLongConsole(number, base);
	}

	void WriteDoubleConsole(double num, int precision)
	{
		if(!writeFunc)
			return;

		char buf[128];
		SafeSprintf(buf, 128, "%.*f", precision, num);

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void WriteCharConsole(char ch)
	{
		if(!writeFunc)
			return;

		char buf[128];
		sprintf(buf, "%c", ch);

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void ReadIntFromConsole(int* val)
	{
		if(!val)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return;
		}

		if(!readFunc)
		{
			nullcThrowError("ERROR: read stream is not avaiable");
			return;
		}

		char buf[128];
		ReadString(buf, 128);

		sscanf(buf, "%d", val);
	}

	int ReadTextFromConsole(NULLCArray data)
	{
		if(!data.ptr)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return 0;
		}

		if(!readFunc)
			return 0;

		char buf[2048];
		ReadString(buf, 2048);

		if(!data.len)
			return 0;

		unsigned int len = (unsigned int)strlen(buf) + 1;
		char *target = data.ptr;
		for(unsigned int i = 0; i < (data.len < len ? data.len : len); i++)
			target[i] = buf[i];
		target[data.len - 1] = 0;

		return ((unsigned int)len < data.len - 1 ? len : data.len - 1);
	}

	void WriteToConsoleExact(NULLCArray data)
	{
		if(!data.ptr)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return;
		}

		if(!writeFunc)
			return;

		writeFunc(contextFunc, data.ptr, data.len);
	}

	void SetConsoleCursorPos(int x, int y)
	{
#if !defined(_MSC_VER)
		(void)x;
		(void)y;

		nullcThrowError("SetConsoleCursorPos: supported only under Windows");
#else
		if(x < 0 || y < 0)
		{
			nullcThrowError("SetConsoleCursorPos: Negative values are not allowed");
			return;
		}
		COORD coords;
		coords.X = (short)x;
		coords.Y = (short)y;
		HANDLE conStdOut = GetStdHandle(STD_OUTPUT_HANDLE);
		SetConsoleCursorPosition(conStdOut, coords);
#endif
	}

	void GetKeyboardState(NULLCArray arr)
	{
#if !defined(_MSC_VER)
		(void)arr;

		nullcThrowError("GetKeyboardState: supported only under Windows");
#else
		if(arr.len < 256)
			nullcThrowError("GetKeyboardState requires array with 256 or more elements");
		::GetKeyboardState((unsigned char*)arr.ptr);
#endif
	}

	void GetMouseState(int* x, int* y)
	{
#if !defined(_MSC_VER)
		(void)x;
		(void)y;

		nullcThrowError("GetMouseState: supported only under Windows");
#else
		if(!x)
		{
			nullcThrowError("ERROR: 'x' argument should not be a nullptr");
			return;
		}
		if(!y)
		{
			nullcThrowError("ERROR: 'y' argument should not be a nullptr");
			return;
		}

		POINT pos;
		GetCursorPos(&pos);
		*x = pos.x;
		*y = pos.y;
#endif
	}

	bool IsPressed(int key)
	{
#if !defined(_MSC_VER)
		(void)key;

		nullcThrowError("GetMouseState: supported only under Windows");
		return false;
#else
		unsigned char arr[256];
		::GetKeyboardState(arr);
		if(arr[key & 0xff] > 1)
			key = key;
		return !!(arr[key & 0xff] & 0x80);
#endif
	}
	bool IsToggled(int key)
	{
#if !defined(_MSC_VER)
		(void)key;

		nullcThrowError("GetMouseState: supported only under Windows");
		return false;
#else
		unsigned char arr[256];
		::GetKeyboardState(arr);
		return !!(arr[key & 0xff] & 0x1);
#endif
	}
}

#define REGISTER_FUNC(funcPtr, name, index) if(!nullcBindModuleFunction("std.io", (void(*)())NULLCIO::funcPtr, name, index)) return false;

bool nullcInitIOModule(void *context, unsigned (*writeFunc)(void *context, char *data, unsigned length), unsigned (*readFunc)(void *context, char *target, unsigned length))
{
	NULLCIO::contextFunc = context;
	NULLCIO::writeFunc = writeFunc;
	NULLCIO::readFunc = readFunc;

	REGISTER_FUNC(WriteToConsole, "Print", 0);
	REGISTER_FUNC(WriteIntConsole, "Print", 1);
	REGISTER_FUNC(WriteDoubleConsole, "Print", 2);
	REGISTER_FUNC(WriteLongConsole, "Print", 3);
	REGISTER_FUNC(WriteCharConsole, "Print", 4);
	REGISTER_FUNC(ReadTextFromConsole, "Input", 0);
	REGISTER_FUNC(ReadIntFromConsole, "Input", 1);
	REGISTER_FUNC(WriteToConsoleExact, "Write", 0);
	REGISTER_FUNC(SetConsoleCursorPos, "SetConsoleCursorPos", 0);

	REGISTER_FUNC(GetKeyboardState, "GetKeyboardState", 0);
	REGISTER_FUNC(GetMouseState, "GetMouseState", 0);
	REGISTER_FUNC(IsPressed, "IsPressed", 0);
	REGISTER_FUNC(IsToggled, "IsToggled", 0);
	REGISTER_FUNC(IsPressed, "IsPressed", 1);
	REGISTER_FUNC(IsToggled, "IsToggled", 1);

	return true;
}

bool nullcInitIOModule()
{
	return nullcInitIOModule(NULL, NULLCIO::DefaultWrite, NULLCIO::DefaultRead);
}
<commit_msg>Fixed function name in error report<commit_after>#include "io.h"

#include "../../NULLC/nullc.h"

#if defined(_MSC_VER)
	#include <windows.h>
#endif

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

#if defined(_MSC_VER)
	#pragma warning(disable: 4996)
#endif

namespace NULLCIO
{
	void *contextFunc = NULL;
	unsigned (*writeFunc)(void *context, char *data, unsigned length) = NULL;
	unsigned (*readFunc)(void *context, char *target, unsigned length) = NULL;

	int	SafeSprintf(char* dst, size_t size, const char* src, ...)
	{
		if(size == 0)
			return 0;

		va_list args;
		va_start(args, src);

		int result = vsnprintf(dst, size, src, args);
		dst[size-1] = '\0';

		va_end(args);

		return (result == -1 || (size_t)result >= size) ? (int)size : result;
	}

	void DefaultInitialize()
	{
#if defined(_MSC_VER)
		static bool initialized = false;

		if(!initialized)
		{
			AllocConsole();

			freopen("CONOUT$", "w", stdout);
			freopen("CONIN$", "r", stdin);

			initialized = true;
		}
#endif
	}

	unsigned DefaultWrite(void *context, char *data, unsigned length)
	{
		(void)context;

		DefaultInitialize();

		return (unsigned)fwrite(data, 1, length, stdout);
	}

	unsigned DefaultRead(void *context, char *target, unsigned length)
	{
		(void)context;

		DefaultInitialize();

		return (unsigned)fread(target, 1, length, stdin);
	}

	void ReadString(char *buf, unsigned length)
	{
		if(!length)
			return;

		*buf = 0;

		char *pos = buf;
		char ch = 0;

		while(readFunc(contextFunc, &ch, 1))
		{
			if(ch == '\n')
				break;

			*pos++ = ch;

			if(pos + 1 == buf + length)
				break;
		}

		*pos = 0;
	}

	int abs(int x){ return x < 0 ? -x : x; }

	void WriteToConsole(NULLCArray data)
	{
		if(!writeFunc)
			return;

		// Empty arrays are silently ignored
		if(!data.ptr)
			return;

		for(unsigned i = 0; i < data.len; i++)
		{
			if(!data.ptr[i])
				return;

			writeFunc(contextFunc, data.ptr + i, 1);
		}
	}

	void WriteLongConsole(long long number, int base)
	{
		if(!(base > 1 && base <= 16))
		{
			nullcThrowError("ERROR: incorrect base %d", base);
			return;
		}

		if(!writeFunc)
			return;

		static char symb[] = "0123456789abcdef";
		bool sign = 0;
		char buf[128];
		char *curr = buf;
		if(number < 0)
			sign = 1;

		*curr++ = *(abs(number % base) + symb);
		while(number /= base)
			*curr++ = *(abs(number % base) + symb);
		if(sign)
			*curr++ = '-';
		*curr = 0;
		int size = int(curr - buf), halfsize = size >> 1;
		for(int i = 0; i < halfsize; i++)
		{
			char tmp = buf[i];
			buf[i] = buf[size-i-1];
			buf[size-i-1] = tmp;
		}

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void WriteIntConsole(int number, int base)
	{
		WriteLongConsole(number, base);
	}

	void WriteDoubleConsole(double num, int precision)
	{
		if(!writeFunc)
			return;

		char buf[128];
		SafeSprintf(buf, 128, "%.*f", precision, num);

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void WriteCharConsole(char ch)
	{
		if(!writeFunc)
			return;

		char buf[128];
		sprintf(buf, "%c", ch);

		writeFunc(contextFunc, buf, (unsigned)strlen(buf));
	}

	void ReadIntFromConsole(int* val)
	{
		if(!val)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return;
		}

		if(!readFunc)
		{
			nullcThrowError("ERROR: read stream is not avaiable");
			return;
		}

		char buf[128];
		ReadString(buf, 128);

		sscanf(buf, "%d", val);
	}

	int ReadTextFromConsole(NULLCArray data)
	{
		if(!data.ptr)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return 0;
		}

		if(!readFunc)
			return 0;

		char buf[2048];
		ReadString(buf, 2048);

		if(!data.len)
			return 0;

		unsigned int len = (unsigned int)strlen(buf) + 1;
		char *target = data.ptr;
		for(unsigned int i = 0; i < (data.len < len ? data.len : len); i++)
			target[i] = buf[i];
		target[data.len - 1] = 0;

		return ((unsigned int)len < data.len - 1 ? len : data.len - 1);
	}

	void WriteToConsoleExact(NULLCArray data)
	{
		if(!data.ptr)
		{
			nullcThrowError("ERROR: argument should not be a nullptr");
			return;
		}

		if(!writeFunc)
			return;

		writeFunc(contextFunc, data.ptr, data.len);
	}

	void SetConsoleCursorPos(int x, int y)
	{
#if !defined(_MSC_VER)
		(void)x;
		(void)y;

		nullcThrowError("SetConsoleCursorPos: supported only under Windows");
#else
		if(x < 0 || y < 0)
		{
			nullcThrowError("SetConsoleCursorPos: Negative values are not allowed");
			return;
		}
		COORD coords;
		coords.X = (short)x;
		coords.Y = (short)y;
		HANDLE conStdOut = GetStdHandle(STD_OUTPUT_HANDLE);
		SetConsoleCursorPosition(conStdOut, coords);
#endif
	}

	void GetKeyboardState(NULLCArray arr)
	{
#if !defined(_MSC_VER)
		(void)arr;

		nullcThrowError("GetKeyboardState: supported only under Windows");
#else
		if(arr.len < 256)
			nullcThrowError("GetKeyboardState requires array with 256 or more elements");
		::GetKeyboardState((unsigned char*)arr.ptr);
#endif
	}

	void GetMouseState(int* x, int* y)
	{
#if !defined(_MSC_VER)
		(void)x;
		(void)y;

		nullcThrowError("GetMouseState: supported only under Windows");
#else
		if(!x)
		{
			nullcThrowError("ERROR: 'x' argument should not be a nullptr");
			return;
		}
		if(!y)
		{
			nullcThrowError("ERROR: 'y' argument should not be a nullptr");
			return;
		}

		POINT pos;
		GetCursorPos(&pos);
		*x = pos.x;
		*y = pos.y;
#endif
	}

	bool IsPressed(int key)
	{
#if !defined(_MSC_VER)
		(void)key;

		nullcThrowError("IsPressed: supported only under Windows");
		return false;
#else
		unsigned char arr[256];
		::GetKeyboardState(arr);
		if(arr[key & 0xff] > 1)
			key = key;
		return !!(arr[key & 0xff] & 0x80);
#endif
	}
	bool IsToggled(int key)
	{
#if !defined(_MSC_VER)
		(void)key;

		nullcThrowError("IsToggled: supported only under Windows");
		return false;
#else
		unsigned char arr[256];
		::GetKeyboardState(arr);
		return !!(arr[key & 0xff] & 0x1);
#endif
	}
}

#define REGISTER_FUNC(funcPtr, name, index) if(!nullcBindModuleFunction("std.io", (void(*)())NULLCIO::funcPtr, name, index)) return false;

bool nullcInitIOModule(void *context, unsigned (*writeFunc)(void *context, char *data, unsigned length), unsigned (*readFunc)(void *context, char *target, unsigned length))
{
	NULLCIO::contextFunc = context;
	NULLCIO::writeFunc = writeFunc;
	NULLCIO::readFunc = readFunc;

	REGISTER_FUNC(WriteToConsole, "Print", 0);
	REGISTER_FUNC(WriteIntConsole, "Print", 1);
	REGISTER_FUNC(WriteDoubleConsole, "Print", 2);
	REGISTER_FUNC(WriteLongConsole, "Print", 3);
	REGISTER_FUNC(WriteCharConsole, "Print", 4);
	REGISTER_FUNC(ReadTextFromConsole, "Input", 0);
	REGISTER_FUNC(ReadIntFromConsole, "Input", 1);
	REGISTER_FUNC(WriteToConsoleExact, "Write", 0);
	REGISTER_FUNC(SetConsoleCursorPos, "SetConsoleCursorPos", 0);

	REGISTER_FUNC(GetKeyboardState, "GetKeyboardState", 0);
	REGISTER_FUNC(GetMouseState, "GetMouseState", 0);
	REGISTER_FUNC(IsPressed, "IsPressed", 0);
	REGISTER_FUNC(IsToggled, "IsToggled", 0);
	REGISTER_FUNC(IsPressed, "IsPressed", 1);
	REGISTER_FUNC(IsToggled, "IsToggled", 1);

	return true;
}

bool nullcInitIOModule()
{
	return nullcInitIOModule(NULL, NULLCIO::DefaultWrite, NULLCIO::DefaultRead);
}
<|endoftext|>


<commit_before>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 440, high_left = 350, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// paint road area
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		//std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			// obtacle on the road: draw rectangle, adjust pwm for servo, blink led
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			//pwm_servo_right(high_right + servo_offset + road_offset + obstacle_position(obstacle.y + (obstacle.height / 2)));

			led_R.toggle();
		} else {
			led_R.low();
			pwm_servo_right(high_right + servo_offset + (road_offset << 1));
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
	   		// std::cout << obstacle.y + (obstacle.height / 2) << std::endl;
   			pwm_servo_left(high_left + servo_offset + road_offset + car_position(obstacle.y + (obstacle.height / 2)));
		} else {
			pwm_servo_left(high_left + servo_offset + (road_offset << 1));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 2){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 2){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return int ( 2 * y_position - 120);
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<commit_msg>magic function bug fixed<commit_after>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 440, high_left = 350, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// paint road area
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		//std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			// obtacle on the road: draw rectangle, adjust pwm for servo, blink led
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			//pwm_servo_right(high_right + servo_offset + road_offset + obstacle_position(obstacle.y + (obstacle.height / 2)));

			led_R.toggle();
		} else {
			led_R.low();
			pwm_servo_right(high_right + servo_offset + (road_offset << 1));
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));
	   		// std::cout << obstacle.y + (obstacle.height / 2) << std::endl;
   			pwm_servo_left(high_left + servo_offset + road_offset + car_position(obstacle.y + (obstacle.height / 2)));
		} else {
			pwm_servo_left(high_left + servo_offset + (road_offset << 1));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 2){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 2){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return int ( 120 - 2 * y_position);
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<|endoftext|>


<commit_before>#include "csv.h"
#include "utilities.h"

#include <iostream>
#include <string>

using namespace NCSV;

struct TMyInt {
    int Value;
};

int main() {
    try {
        TCSV csv("test.csv");
        while (csv.HasNext()) {
            int a;
            int b;
            std::string c;
            TMyInt d;
            auto dReader = [&d](const std::string& s) {
                d.Value = std::stoi(s);
            };
            csv.Next().To(a, b, ignore, c, dReader);
            std::cout << a << ", "
                      << b << ", "
                      << c << ", "
                      << d.Value << std::endl;
        }
    } catch (std::runtime_error& ex) {
        std::cerr << "error: " << ex.what() << std::endl;
    }
}

<commit_msg>rvalue support<commit_after>#include "csv.h"
#include "utilities.h"

#include <iostream>
#include <string>

using namespace NCSV;

struct TMyInt {
    int Value;
};

int main() {
    try {
        TCSV csv("test.csv");
        while (csv.HasNext()) {
            int a;
            int b;
            std::string c;
            TMyInt d;
            csv.Next().To(a, b, ignore, c,
                [&d](const std::string& s) {
                    d.Value = std::stoi(s);
                }
            );
            std::cout << a << ", "
                      << b << ", "
                      << c << ", "
                      << d.Value << std::endl;
        }
    } catch (std::runtime_error& ex) {
        std::cerr << "error: " << ex.what() << std::endl;
    }
}

<|endoftext|>


<commit_before>/*
 *
 *    Copyright (c) 2020 Project CHIP Authors
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include "AppConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>
#include <sys/param.h>

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

#include <inet/IPAddress.h>
#include <inet/InetError.h>
#include <inet/InetLayer.h>
#include <platform/CHIPDeviceLayer.h>
#include <support/CodeUtils.h>
#include <support/ErrorStr.h>
#include <system/SystemPacketBuffer.h>
#include <transport/SecureSessionMgr.h>
#include <transport/UDP.h>

#include "Server.h"

#include "attribute-storage.h"
#include "chip-zcl/chip-zcl-zpro-codec.h"
#include "gen/znet-bookkeeping.h"
#include "util.h"

using namespace ::chip;
using namespace ::chip::Inet;
using namespace ::chip::Transport;

// Transport Callbacks
namespace {

#ifndef EXAMPLE_SERVER_NODEID
// Use this hardcoded NODEID for the lock app example if none was provided
#define EXAMPLE_SERVER_NODEID 0x3546526e
#endif // EXAMPLE_SERVER_NODEID

class ServerCallback : public SecureSessionMgrCallback
{
public:
    virtual void OnMessageReceived(const MessageHeader & header, Transport::PeerConnectionState * state,
                                   System::PacketBuffer * buffer, SecureSessionMgrBase * mgr)
    {
        const size_t data_len = buffer->DataLength();
        char src_addr[PeerAddress::kMaxToStringSize];

        // as soon as a client connects, assume it is connected
        VerifyOrExit(buffer != NULL, EFR32_LOG("Received data but couldn't process it..."));
        VerifyOrExit(header.GetSourceNodeId().HasValue(), EFR32_LOG("Unknown source for received message"));

        VerifyOrExit(state->GetPeerNodeId() != kUndefinedNodeId, EFR32_LOG("Unknown source for received message"));

        state->GetPeerAddress().ToString(src_addr, sizeof(src_addr));

        EFR32_LOG("Packet received from %s: %zu bytes", src_addr, static_cast<size_t>(data_len));

        HandleDataModelMessage(header, buffer, mgr);
        buffer = NULL;

    exit:
        // SendTo calls Free on the buffer without an AddRef, if SendTo was not called, free the buffer.
        if (buffer != NULL)
        {
            System::PacketBuffer::Free(buffer);
        }
    }

    virtual void OnNewConnection(Transport::PeerConnectionState * state, SecureSessionMgrBase * mgr)
    {
        EFR32_LOG("Received a new connection.");
    }

private:
    /**
     * Handle a message that should be processed via our data model processing
     * codepath.
     *
     * @param [in] buffer The buffer holding the message.  This function guarantees
     *                    that it will free the buffer before returning.
     */
    void HandleDataModelMessage(const MessageHeader & header, System::PacketBuffer * buffer, SecureSessionMgrBase * mgr)
    {
        EmberApsFrame frame;
        bool ret = extractApsFrame(buffer->Start(), buffer->DataLength(), &frame);
        if (ret)
        {
            EFR32_LOG("APS framprocessing success!");
        }
        else
        {
            EFR32_LOG("APS processing failure!");
            System::PacketBuffer::Free(buffer);
            return;
        }
        ChipResponseDestination responseDest(header.GetSourceNodeId().Value(), mgr);
        uint8_t * message;
        uint16_t messageLen = extractMessage(buffer->Start(), buffer->DataLength(), &message);
        ret                 = emberAfProcessMessage(&frame,
                                    0, // type
                                    message, messageLen,
                                    &responseDest, // source identifier
                                    NULL);

        System::PacketBuffer::Free(buffer);

        if (ret)
        {
            EFR32_LOG("Data model processing success!");
        }
        else
        {
            EFR32_LOG("Data model processing failure!");
        }
    }
};

static ServerCallback gCallbacks;
static SecurePairingUsingTestSecret gTestPairing;

} // namespace

void InitDataModelHandler()
{
    emberAfEndpointConfigure();
    emAfInit();
}

// The echo server assumes the platform's networking has been setup already
void StartServer(DemoSessionManager * sessions)
{
    CHIP_ERROR err = CHIP_NO_ERROR;
    Optional<Transport::PeerAddress> peer(Transport::Type::kUndefined);

    err = sessions->Init(EXAMPLE_SERVER_NODEID, &DeviceLayer::SystemLayer,
                         UdpListenParameters(&DeviceLayer::InetLayer).SetAddressType(kIPAddressType_IPv6));
    SuccessOrExit(err);

    err = sessions->NewPairing(Optional<NodeId>::Value(kUndefinedNodeId), peer, 0, 0, &gTestPairing);
    SuccessOrExit(err);

    sessions->SetDelegate(&gCallbacks);

exit:
    if (err != CHIP_NO_ERROR)
    {
        EFR32_LOG("ERROR setting up transport: %s", ErrorStr(err));
    }
    else
    {
        EFR32_LOG("Lock Server Listening...");
    }
}
<commit_msg>Fix the EFR32 build (#2271)<commit_after>/*
 *
 *    Copyright (c) 2020 Project CHIP Authors
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include "AppConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>
#include <sys/param.h>

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

#include <app/chip-zcl-zpro-codec.h>
#include <inet/IPAddress.h>
#include <inet/InetError.h>
#include <inet/InetLayer.h>
#include <platform/CHIPDeviceLayer.h>
#include <support/CodeUtils.h>
#include <support/ErrorStr.h>
#include <system/SystemPacketBuffer.h>
#include <transport/SecureSessionMgr.h>
#include <transport/UDP.h>

#include "Server.h"

#include "attribute-storage.h"
#include "gen/znet-bookkeeping.h"
#include "util.h"

using namespace ::chip;
using namespace ::chip::Inet;
using namespace ::chip::Transport;

// Transport Callbacks
namespace {

#ifndef EXAMPLE_SERVER_NODEID
// Use this hardcoded NODEID for the lock app example if none was provided
#define EXAMPLE_SERVER_NODEID 0x3546526e
#endif // EXAMPLE_SERVER_NODEID

class ServerCallback : public SecureSessionMgrCallback
{
public:
    virtual void OnMessageReceived(const MessageHeader & header, Transport::PeerConnectionState * state,
                                   System::PacketBuffer * buffer, SecureSessionMgrBase * mgr)
    {
        const size_t data_len = buffer->DataLength();
        char src_addr[PeerAddress::kMaxToStringSize];

        // as soon as a client connects, assume it is connected
        VerifyOrExit(buffer != NULL, EFR32_LOG("Received data but couldn't process it..."));
        VerifyOrExit(header.GetSourceNodeId().HasValue(), EFR32_LOG("Unknown source for received message"));

        VerifyOrExit(state->GetPeerNodeId() != kUndefinedNodeId, EFR32_LOG("Unknown source for received message"));

        state->GetPeerAddress().ToString(src_addr, sizeof(src_addr));

        EFR32_LOG("Packet received from %s: %zu bytes", src_addr, static_cast<size_t>(data_len));

        HandleDataModelMessage(header, buffer, mgr);
        buffer = NULL;

    exit:
        // SendTo calls Free on the buffer without an AddRef, if SendTo was not called, free the buffer.
        if (buffer != NULL)
        {
            System::PacketBuffer::Free(buffer);
        }
    }

    virtual void OnNewConnection(Transport::PeerConnectionState * state, SecureSessionMgrBase * mgr)
    {
        EFR32_LOG("Received a new connection.");
    }

private:
    /**
     * Handle a message that should be processed via our data model processing
     * codepath.
     *
     * @param [in] buffer The buffer holding the message.  This function guarantees
     *                    that it will free the buffer before returning.
     */
    void HandleDataModelMessage(const MessageHeader & header, System::PacketBuffer * buffer, SecureSessionMgrBase * mgr)
    {
        EmberApsFrame frame;
        bool ret = extractApsFrame(buffer->Start(), buffer->DataLength(), &frame);
        if (ret)
        {
            EFR32_LOG("APS framprocessing success!");
        }
        else
        {
            EFR32_LOG("APS processing failure!");
            System::PacketBuffer::Free(buffer);
            return;
        }
        ChipResponseDestination responseDest(header.GetSourceNodeId().Value(), mgr);
        uint8_t * message;
        uint16_t messageLen = extractMessage(buffer->Start(), buffer->DataLength(), &message);
        ret                 = emberAfProcessMessage(&frame,
                                    0, // type
                                    message, messageLen,
                                    &responseDest, // source identifier
                                    NULL);

        System::PacketBuffer::Free(buffer);

        if (ret)
        {
            EFR32_LOG("Data model processing success!");
        }
        else
        {
            EFR32_LOG("Data model processing failure!");
        }
    }
};

static ServerCallback gCallbacks;
static SecurePairingUsingTestSecret gTestPairing;

} // namespace

void InitDataModelHandler()
{
    emberAfEndpointConfigure();
    emAfInit();
}

// The echo server assumes the platform's networking has been setup already
void StartServer(DemoSessionManager * sessions)
{
    CHIP_ERROR err = CHIP_NO_ERROR;
    Optional<Transport::PeerAddress> peer(Transport::Type::kUndefined);

    err = sessions->Init(EXAMPLE_SERVER_NODEID, &DeviceLayer::SystemLayer,
                         UdpListenParameters(&DeviceLayer::InetLayer).SetAddressType(kIPAddressType_IPv6));
    SuccessOrExit(err);

    err = sessions->NewPairing(Optional<NodeId>::Value(kUndefinedNodeId), peer, 0, 0, &gTestPairing);
    SuccessOrExit(err);

    sessions->SetDelegate(&gCallbacks);

exit:
    if (err != CHIP_NO_ERROR)
    {
        EFR32_LOG("ERROR setting up transport: %s", ErrorStr(err));
    }
    else
    {
        EFR32_LOG("Lock Server Listening...");
    }
}
<|endoftext|>


<commit_before>/* CsaClientTest.cpp
 *
 * Kubo Ryosuke
 */

#if !defined(NDEBUG)

#include "test/Test.h"
#include "../CsaClient.h"

TEST(CsaClient, test) {
	auto flagSets = sunfish::CsaClient::getFlagSets();

	auto probe = [flagSets](const char* str) {
		for (int i = 0; i < sunfish::CsaClient::RECV_NUM; i++) {
			if (flagSets[i].wildcard.match(str)) {
				return flagSets[i].flag;
			}
		}
		return sunfish::CsaClient::RECV_FLAG::RECV_NULL;
	};

	ASSERT_EQ(probe("LOGIN:sunfish OK"), sunfish::CsaClient::RECV_LOGIN_OK);
	ASSERT_EQ(probe("LOGIN:incorect"), sunfish::CsaClient::RECV_LOGIN_INC);
	ASSERT_EQ(probe("LOGOUT:completed"), sunfish::CsaClient::RECV_LOGOUT);
	ASSERT_EQ(probe("%TORYO"), sunfish::CsaClient::RECV_MOVE_EX);
	ASSERT_EQ(probe("+7776FU"), sunfish::CsaClient::RECV_MOVE_B);
	ASSERT_EQ(probe("+7776FU, foo"), sunfish::CsaClient::RECV_MOVE_B);
	ASSERT_EQ(probe("-3334FU, bar"), sunfish::CsaClient::RECV_MOVE_W);
	ASSERT_EQ(probe("BEGIN Game_Summary"), sunfish::CsaClient::RECV_SUMMARY);;
	ASSERT_EQ(probe("START:foo_bar"), sunfish::CsaClient::RECV_START);
	ASSERT_EQ(probe("REJECT:foo by bar"), sunfish::CsaClient::RECV_REJECT);
	ASSERT_EQ(probe("#WIN"), sunfish::CsaClient::RECV_WIN);
	ASSERT_EQ(probe("#LOSE"), sunfish::CsaClient::RECV_LOSE);
	ASSERT_EQ(probe("#WIN(LOSE)"), sunfish::CsaClient::RECV_WIN_LOSE);
	ASSERT_EQ(probe("#DRAW"), sunfish::CsaClient::RECV_DRAW);
	ASSERT_EQ(probe("#CHUDAN"), sunfish::CsaClient::RECV_INTERRUPT);
	ASSERT_EQ(probe("#SENNICHITE"), sunfish::CsaClient::RECV_REPEAT);
	ASSERT_EQ(probe("#OUTE_SENNICHITE"), sunfish::CsaClient::RECV_CHECK_REP);
	ASSERT_EQ(probe("#ILLEGAL_MOVE"), sunfish::CsaClient::RECV_ILLEGAL);
	ASSERT_EQ(probe("#TIME_UP"), sunfish::CsaClient::RECV_TIME_UP);
	ASSERT_EQ(probe("#RESIGN"), sunfish::CsaClient::RECV_RESIGN);
	ASSERT_EQ(probe("#JISHOGI"), sunfish::CsaClient::RECV_JISHOGI);
	ASSERT_EQ(probe("#MAX_MOVES"), sunfish::CsaClient::RECV_MAX_MOVE);
	ASSERT_EQ(probe("#CENSORED"), sunfish::CsaClient::RECV_CENSORED);

	ASSERT_EQ(probe("LOGIN:sunfish"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("LOGIN:sunfish "), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("LOGOUT:hoge "), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("START"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("REJECT"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("#HOGE"), sunfish::CsaClient::RECV_NULL);

}

#endif // !defined(NDEBUG)
<commit_msg>Fix test<commit_after>/* CsaClientTest.cpp
 *
 * Kubo Ryosuke
 */

#if !defined(NDEBUG)

#include "test/Test.h"
#include "../CsaClient.h"

TEST(CsaClient, test) {
	auto flagSets = sunfish::CsaClient::getFlagSets();

	auto probe = [flagSets](const char* str) {
		for (int i = 0; i < sunfish::CsaClient::RECV_NUM; i++) {
			if (flagSets[i].wildcard.match(str)) {
				return flagSets[i].flag;
			}
		}
		return sunfish::CsaClient::RECV_FLAG::RECV_NULL;
	};

	ASSERT_EQ(probe("LOGIN:sunfish OK"), sunfish::CsaClient::RECV_LOGIN_OK);
	ASSERT_EQ(probe("LOGIN:incorrect"), sunfish::CsaClient::RECV_LOGIN_INC);
	ASSERT_EQ(probe("LOGOUT:completed"), sunfish::CsaClient::RECV_LOGOUT);
	ASSERT_EQ(probe("%TORYO"), sunfish::CsaClient::RECV_MOVE_EX);
	ASSERT_EQ(probe("+7776FU"), sunfish::CsaClient::RECV_MOVE_B);
	ASSERT_EQ(probe("+7776FU, foo"), sunfish::CsaClient::RECV_MOVE_B);
	ASSERT_EQ(probe("-3334FU, bar"), sunfish::CsaClient::RECV_MOVE_W);
	ASSERT_EQ(probe("BEGIN Game_Summary"), sunfish::CsaClient::RECV_SUMMARY);;
	ASSERT_EQ(probe("START:foo_bar"), sunfish::CsaClient::RECV_START);
	ASSERT_EQ(probe("REJECT:foo by bar"), sunfish::CsaClient::RECV_REJECT);
	ASSERT_EQ(probe("#WIN"), sunfish::CsaClient::RECV_WIN);
	ASSERT_EQ(probe("#LOSE"), sunfish::CsaClient::RECV_LOSE);
	ASSERT_EQ(probe("#WIN(LOSE)"), sunfish::CsaClient::RECV_WIN_LOSE);
	ASSERT_EQ(probe("#DRAW"), sunfish::CsaClient::RECV_DRAW);
	ASSERT_EQ(probe("#CHUDAN"), sunfish::CsaClient::RECV_INTERRUPT);
	ASSERT_EQ(probe("#SENNICHITE"), sunfish::CsaClient::RECV_REPEAT);
	ASSERT_EQ(probe("#OUTE_SENNICHITE"), sunfish::CsaClient::RECV_CHECK_REP);
	ASSERT_EQ(probe("#ILLEGAL_MOVE"), sunfish::CsaClient::RECV_ILLEGAL);
	ASSERT_EQ(probe("#TIME_UP"), sunfish::CsaClient::RECV_TIME_UP);
	ASSERT_EQ(probe("#RESIGN"), sunfish::CsaClient::RECV_RESIGN);
	ASSERT_EQ(probe("#JISHOGI"), sunfish::CsaClient::RECV_JISHOGI);
	ASSERT_EQ(probe("#MAX_MOVES"), sunfish::CsaClient::RECV_MAX_MOVE);
	ASSERT_EQ(probe("#CENSORED"), sunfish::CsaClient::RECV_CENSORED);

	ASSERT_EQ(probe("LOGIN:sunfish"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("LOGIN:sunfish "), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("LOGOUT:hoge "), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("START"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("REJECT"), sunfish::CsaClient::RECV_NULL);
	ASSERT_EQ(probe("#HOGE"), sunfish::CsaClient::RECV_NULL);

}

#endif // !defined(NDEBUG)
<|endoftext|>


<commit_before>//Copyright (c) 2021 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include <gtest/gtest.h>

#include "../src/LayerPlan.h" //Code under test.

namespace cura
{

/*!
 * A fixture containing some sets of GCodePaths to test with.
 */
class ExtruderPlanTestPathCollection
{
public:
    /*!
     * One path with 5 vertices printing a 1000x1000 micron square starting from
     * 0,0.
     */
    std::vector<GCodePath> square;

    /*!
     * Three lines side by side, with two travel moves in between.
     */
    std::vector<GCodePath> lines;

    /*!
     * Configuration to print extruded paths with in the fixture.
     *
     * This config is referred to via pointer, so changing it will immediately
     * change it for all extruded paths.
     */
    GCodePathConfig extrusion_config;

    /*!
     * Configuration to print travel paths with in the fixture.
     *
     * This config is referred to via pointer, so changing it will immediately
     * change it for all travel paths.
     */
    GCodePathConfig travel_config;

    ExtruderPlanTestPathCollection() :
        extrusion_config(
            PrintFeatureType::OuterWall,
            /*line_width=*/400,
            /*layer_thickness=*/100,
            /*flow=*/1.0_r,
            GCodePathConfig::SpeedDerivatives(50, 1000, 10)
        ),
        travel_config(
            PrintFeatureType::MoveCombing,
            /*line_width=*/0,
            /*layer_thickness=*/100,
            /*flow=*/0.0_r,
            GCodePathConfig::SpeedDerivatives(120, 5000, 30)
        )
    {
        const std::string mesh_id = "test_mesh";
        constexpr Ratio flow_1 = 1.0_r;
        constexpr bool no_spiralize = false;
        constexpr Ratio speed_factor_1 = 1.0_r;
        square.assign({GCodePath(extrusion_config, mesh_id, SpaceFillType::PolyLines, flow_1, no_spiralize, speed_factor_1)});
        square.back().points = {
            Point(0, 0),
            Point(1000, 0),
            Point(1000, 1000),
            Point(0, 1000),
            Point(0, 0)
        };

        lines.assign({
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_factor_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_factor_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_factor_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_factor_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_factor_1)
        });
        lines[0].points = {Point(0, 0), Point(1000, 0)};
        lines[1].points = {Point(1000, 0), Point(1000, 400)};
        lines[2].points = {Point(1000, 400), Point(0, 400)};
        lines[3].points = {Point(0, 400), Point(0, 800)};
        lines[4].points = {Point(0, 800), Point(1000, 800)};
    }
};

/*!
 * Tests in this class get parameterized with a vector of GCodePaths to put in
 * the extruder plan, and an extruder plan to put it in.
 */
class ExtruderPlanPathsParameterizedTest : public testing::TestWithParam<std::vector<GCodePath>> {
public:
    /*!
     * An extruder plan that can be used as a victim for testing.
     */
    ExtruderPlan extruder_plan;

    ExtruderPlanPathsParameterizedTest() :
        extruder_plan(
            /*extruder=*/0,
            /*layer_nr=*/50,
            /*is_initial_layer=*/false,
            /*is_raft_layer=*/false,
            /*layer_thickness=*/100,
            FanSpeedLayerTimeSettings(),
            RetractionConfig()
        )
    {}
};

/*!
 * Tests that paths remain unmodified if applying back pressure compensation
 * with factor 0.
 */
TEST_P(ExtruderPlanPathsParameterizedTest, BackPressureCompensationZeroIsUncompensated)
{
    extruder_plan.paths = GetParam();
    std::vector<Ratio> original_flows;
    for(const GCodePath& path : extruder_plan.paths)
    {
        original_flows.push_back(path.flow);
    }

    extruder_plan.applyBackPressureCompensation(0.0_r);

    ASSERT_EQ(extruder_plan.paths.size(), original_flows.size()) << "Number of paths may not have changed.";
    for(size_t i = 0; i < extruder_plan.paths.size(); ++i)
    {
        EXPECT_EQ(original_flows[i], extruder_plan.paths[i].flow) << "The flow rate did not change, because the back pressure compensation ratio was 0.";
    }
}

INSTANTIATE_TEST_CASE_P(ExtruderPlanTestInstantiation, ExtruderPlanPathsParameterizedTest, testing::Values(
        ExtruderPlanTestPathCollection().square,
        ExtruderPlanTestPathCollection().lines
));

}
<commit_msg>Add more test paths to test with<commit_after>//Copyright (c) 2021 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include <gtest/gtest.h>

#include "../src/LayerPlan.h" //Code under test.

namespace cura
{

/*!
 * A fixture containing some sets of GCodePaths to test with.
 */
class ExtruderPlanTestPathCollection
{
public:
    /*!
     * One path with 5 vertices printing a 1000x1000 micron square starting from
     * 0,0.
     */
    std::vector<GCodePath> square;

    /*!
     * Three lines side by side, with two travel moves in between.
     */
    std::vector<GCodePath> lines;

    /*!
     * Three lines side by side with travel moves in between, but adjusted flow.
     *
     * The first line gets 120% flow.
     * The second line gets 80% flow.
     * The third line gets 40% flow.
     */
    std::vector<GCodePath> decreasing_flow;

    /*!
     * Three lines side by side with their speed factors adjusted.
     *
     * The first line gets 120% speed.
     * The second line gets 80% speed.
     * The third line gets 40% speed.
     */
    std::vector<GCodePath> decreasing_speed;

    /*!
     * Configuration to print extruded paths with in the fixture.
     *
     * This config is referred to via pointer, so changing it will immediately
     * change it for all extruded paths.
     */
    GCodePathConfig extrusion_config;

    /*!
     * Configuration to print travel paths with in the fixture.
     *
     * This config is referred to via pointer, so changing it will immediately
     * change it for all travel paths.
     */
    GCodePathConfig travel_config;

    ExtruderPlanTestPathCollection() :
        extrusion_config(
            PrintFeatureType::OuterWall,
            /*line_width=*/400,
            /*layer_thickness=*/100,
            /*flow=*/1.0_r,
            GCodePathConfig::SpeedDerivatives(50, 1000, 10)
        ),
        travel_config(
            PrintFeatureType::MoveCombing,
            /*line_width=*/0,
            /*layer_thickness=*/100,
            /*flow=*/0.0_r,
            GCodePathConfig::SpeedDerivatives(120, 5000, 30)
        )
    {
        const std::string mesh_id = "test_mesh";
        constexpr Ratio flow_1 = 1.0_r;
        constexpr bool no_spiralize = false;
        constexpr Ratio speed_1 = 1.0_r;
        square.assign({GCodePath(extrusion_config, mesh_id, SpaceFillType::PolyLines, flow_1, no_spiralize, speed_1)});
        square.back().points = {
            Point(0, 0),
            Point(1000, 0),
            Point(1000, 1000),
            Point(0, 1000),
            Point(0, 0)
        };

        lines.assign({
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1)
        });
        lines[0].points = {Point(0, 0), Point(1000, 0)};
        lines[1].points = {Point(1000, 0), Point(1000, 400)};
        lines[2].points = {Point(1000, 400), Point(0, 400)};
        lines[3].points = {Point(0, 400), Point(0, 800)};
        lines[4].points = {Point(0, 800), Point(1000, 800)};

        constexpr Ratio flow_12 = 1.2_r;
        constexpr Ratio flow_08 = 0.8_r;
        constexpr Ratio flow_04 = 0.4_r;
        decreasing_flow.assign({
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_12, no_spiralize, speed_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_08, no_spiralize, speed_1),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_04, no_spiralize, speed_1)
        });
        decreasing_flow[0].points = {Point(0, 0), Point(1000, 0)};
        decreasing_flow[1].points = {Point(1000, 0), Point(1000, 400)};
        decreasing_flow[2].points = {Point(1000, 400), Point(0, 400)};
        decreasing_flow[3].points = {Point(0, 400), Point(0, 800)};
        decreasing_flow[4].points = {Point(0, 800), Point(1000, 800)};

        constexpr Ratio speed_12 = 1.2_r;
        constexpr Ratio speed_08 = 0.8_r;
        constexpr Ratio speed_04 = 0.4_r;
        decreasing_speed.assign({
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_12),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_08),
            GCodePath(travel_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_1),
            GCodePath(extrusion_config, mesh_id, SpaceFillType::Lines, flow_1, no_spiralize, speed_04)
        });
        decreasing_speed[0].points = {Point(0, 0), Point(1000, 0)};
        decreasing_speed[1].points = {Point(1000, 0), Point(1000, 400)};
        decreasing_speed[2].points = {Point(1000, 400), Point(0, 400)};
        decreasing_speed[3].points = {Point(0, 400), Point(0, 800)};
        decreasing_speed[4].points = {Point(0, 800), Point(1000, 800)};
    }
};

/*!
 * Tests in this class get parameterized with a vector of GCodePaths to put in
 * the extruder plan, and an extruder plan to put it in.
 */
class ExtruderPlanPathsParameterizedTest : public testing::TestWithParam<std::vector<GCodePath>> {
public:
    /*!
     * An extruder plan that can be used as a victim for testing.
     */
    ExtruderPlan extruder_plan;

    ExtruderPlanPathsParameterizedTest() :
        extruder_plan(
            /*extruder=*/0,
            /*layer_nr=*/50,
            /*is_initial_layer=*/false,
            /*is_raft_layer=*/false,
            /*layer_thickness=*/100,
            FanSpeedLayerTimeSettings(),
            RetractionConfig()
        )
    {}
};

/*!
 * Tests that paths remain unmodified if applying back pressure compensation
 * with factor 0.
 */
TEST_P(ExtruderPlanPathsParameterizedTest, BackPressureCompensationZeroIsUncompensated)
{
    extruder_plan.paths = GetParam();
    std::vector<Ratio> original_flows;
    for(const GCodePath& path : extruder_plan.paths)
    {
        original_flows.push_back(path.flow);
    }

    extruder_plan.applyBackPressureCompensation(0.0_r);

    ASSERT_EQ(extruder_plan.paths.size(), original_flows.size()) << "Number of paths may not have changed.";
    for(size_t i = 0; i < extruder_plan.paths.size(); ++i)
    {
        EXPECT_EQ(original_flows[i], extruder_plan.paths[i].flow) << "The flow rate did not change, because the back pressure compensation ratio was 0.";
    }
}

INSTANTIATE_TEST_CASE_P(ExtruderPlanTestInstantiation, ExtruderPlanPathsParameterizedTest, testing::Values(
        ExtruderPlanTestPathCollection().square,
        ExtruderPlanTestPathCollection().lines,
        ExtruderPlanTestPathCollection().decreasing_flow,
        ExtruderPlanTestPathCollection().decreasing_speed
));

}
<|endoftext|>