<commit_before>#include <cstdint>
#include <chrono>
#include <thread>
#include <future>
#include <functional>
#include <random>
#include <iostream>
#include <vector>
#include <algorithm>

using microsec = std::chrono::microseconds;
using millisec = std::chrono::milliseconds;
using nanosec = std::chrono::nanoseconds;
using resolution = nanosec;
using my_clock = std::chrono::steady_clock;

#define INTERVAL_PERIOD         resolution(10000000)
#define JITTER_MIN              100000
#define JITTER_MAX              1000000
#define ITERATION_MAX           400
#define RUNTIME_LIMIT    resolution(4000000000)

template <int IntervalMin, int IntervalMax>
class PeriodicTimer {
private:
    volatile bool is_running_ = false;
    std::future<int> pending_;
    my_clock::time_point firstInterval_;
    my_clock::time_point lastInterval_;

    //! @brief call doIt until stop() is called, and return the number of
    // iterations for which doIt was called.
    int doItTimed(std::function<void(resolution)> doIt) {
        int result = 0;
        std::random_device seedGenerator;
        std::mt19937 gen(seedGenerator());
        std::uniform_int_distribution<> distribution(IntervalMin,
                                                     IntervalMax);
        resolution jitter = resolution(distribution(gen));
        firstInterval_ = my_clock::now();
        my_clock::time_point start{firstInterval_};
        my_clock::time_point startNext{firstInterval_};

        while (is_running_) {
            // Delay the function call for a small random amount of time.
            std::this_thread::sleep_for(jitter);
            doIt(jitter);
            ++result;

            // Calculate the next time we want to iterate
            startNext = start + INTERVAL_PERIOD;
            auto currentTime = my_clock::now();
            if (currentTime < startNext) {
                // Wait for the next period start time
                std::this_thread::sleep_until(startNext);
            } else {
                std::cout << "Warning: Missed interval " << result << std::endl;
            }

            // Get a new jitter and start time for the next iteration
            jitter = resolution(distribution(gen));
            start = startNext;
        }

        lastInterval_ = startNext;

        return result;
    }

public:
    //! @brief call doIt for repeat_count iterations.
    // A random delay (jitter) is calculated for each call to doIt. If doIt
    // runs for less than the delay, doItCount will wait for the remaining time
    // before the next interval. If doIt takes more time than the delay, the
    // next iteration takes place immediately. This way, doIt is called no more
    // often than
    void doItCount(std::function<void(resolution)> doIt,
                        uint32_t repeat_count) {
        std::random_device seedGenerator;
        std::mt19937 gen(seedGenerator());
        std::uniform_int_distribution<> distribution(IntervalMin,
                                                     IntervalMax);

        resolution jitterNext = resolution(distribution(gen));
        resolution jitter;
        firstInterval_ = my_clock::now();
        my_clock::time_point start{firstInterval_};
        my_clock::time_point startNext{start};

        uint32_t itr = 0;
        while (itr < repeat_count) {
            jitter = jitterNext;

            // Delay the function call for a small random amount of time.
            std::this_thread::sleep_for(jitter);
            doIt(jitter);
            ++itr;

            // Get a new jitter for the next iteration
            jitterNext = resolution(distribution(gen));

            // Calculate the next time we want to iterate
            startNext = start + INTERVAL_PERIOD;

            // Wait for the next period start time
            std::this_thread::sleep_until(startNext);
            start = startNext;
        }

        lastInterval_ = my_clock::now();
    }

    void start(std::function<void(resolution)> doIt) {
        is_running_ = true;
        // Run doItTimed on another thread, passing the "this" pointer and the
        // function doItTimed must run until stop() is executed.
        auto f = std::async(std::launch::async,
                            &PeriodicTimer::doItTimed,
                            this,
                            doIt);
        // Move the future to another variable so we don't wait for it here.
        pending_ = std::move(f);
    }

    int stop() {
        // Allow doItTimed to exit its while-loop
        is_running_ = false;

        // Return the number of iterations
        return pending_.get();
    }

    my_clock::duration runtime() const {
        return lastInterval_ - firstInterval_;
    }
};

class Jitters {
    std::vector<resolution> jitters_;
    resolution smallest_;
    resolution largest_;

public:
    Jitters()
        : jitters_(ITERATION_MAX)
        , smallest_(resolution::max())
        , largest_(resolution::min()) {
    }

    void
    insert(resolution j) {
        jitters_.push_back(j);

        if (j < smallest_) {
            smallest_ = j;
        }

        if (j > largest_) {
            largest_ = j;
        }
    }

    resolution
    average() {
        resolution result(0);
        for (auto j : jitters_) {
            result = result + j;
        }

        result /= jitters_.size();

        return result;
    }

    resolution
    largest() {
        return largest_;
    }

    resolution
    smallest() {
        return smallest_;
    }

    resolution
    median() {
        std::sort(jitters_.begin(), jitters_.end());
        return jitters_[jitters_.size() / 2];
    }
};

void main() {
    Jitters jitter1;
    const resolution jitterMin = resolution(JITTER_MIN);
    const resolution jitterMax = resolution(JITTER_MAX);
    const resolution repeatInterval = INTERVAL_PERIOD;
    PeriodicTimer<JITTER_MIN, JITTER_MAX> timer;

    std::cout << "The resolution of the high-resolution clock is: "
        << (static_cast<double>(std::chrono::high_resolution_clock::period::num)
            / std::chrono::high_resolution_clock::period::den) << " sec"
        << std::endl;
    std::cout << "The resolution of the steady clock is:          "
        << (static_cast<double>(std::chrono::steady_clock::period::num)
            / std::chrono::steady_clock::period::den)
        << " sec" << std::endl;
    std::cout << "The resolution of the system clock is:          "
        << (static_cast<double>(std::chrono::system_clock::period::num)
            / std::chrono::system_clock::period::den)
        << " sec" << std::endl << std::endl;

    std::cout << "Jitter Tests." << std::endl
        << "  Interval:   "
        << std::chrono::duration_cast<millisec>(repeatInterval).count()
        << " ms" << std::endl
        << "  Min Jitter: "
        << std::chrono::duration_cast<microsec>(jitterMin).count() << " us"
        << std::endl
        << "  Max Jitter: "
        << std::chrono::duration_cast<microsec>(jitterMax).count() << " us"
        << std::endl << std::endl;

    /*** Iteration Test ***/
    std::cout << "Jitter test 1. Iterations: " << ITERATION_MAX << std::endl;
    timer.doItCount(std::bind(&Jitters::insert, &jitter1, std::placeholders::_1), ITERATION_MAX);
    auto runtime = timer.runtime();

    std::cout << "Iterations            " << ITERATION_MAX << std::endl;
    std::cout << "Expected elapsed time "
        << (ITERATION_MAX * std::chrono::duration_cast<millisec>(repeatInterval).count())
        << " ms" << std::endl;
    std::cout << "Actual elapsed time   "
        << std::chrono::duration_cast<millisec>(runtime).count()
        << " ms" << std::endl;
    std::cout << "Smallest jitter is    "
        << std::chrono::duration_cast<microsec>(jitter1.smallest()).count()
        << " us" << std::endl;
    std::cout << "Largest jitter is     "
        << std::chrono::duration_cast<microsec>(jitter1.largest()).count()
        << " us" << std::endl;
    std::cout << "Average jitter is     "
        << std::chrono::duration_cast<microsec>(jitter1.average()).count()
        << " us" << std::endl;
    std::cout << "Median jitter is      "
        << std::chrono::duration_cast<microsec>(jitter1.median()).count()
        << " us" << std::endl;

    std::cout << std::endl;

    /*** Timed Test ***/
    Jitters jitter2;
    const resolution iterationTimeLimit = RUNTIME_LIMIT;
    std::cout << "Jitter test 2. Timed : "
        << std::chrono::duration_cast<millisec>(iterationTimeLimit).count() << " ms" << std::endl;

    timer.start(std::bind(&Jitters::insert, &jitter2, std::placeholders::_1));
    std::this_thread::sleep_for(iterationTimeLimit);
    int iterations = timer.stop();
    runtime = timer.runtime();

    std::cout << "Expected iterations " << runtime / repeatInterval
        << std::endl;
    std::cout << "Actual iterations   " << iterations << std::endl;
    std::cout << "Elapsed time        "
        << std::chrono::duration_cast<millisec>(runtime).count()
        << " ms" << std::endl;
    std::cout << "Smallest jitter is  "
        << std::chrono::duration_cast<microsec>(jitter2.smallest()).count()
        << " us" << std::endl;
    std::cout << "Largest jitter is   "
        << std::chrono::duration_cast<microsec>(jitter2.largest()).count()
        << " us" << std::endl;
    std::cout << "Average jitter is   "
        << std::chrono::duration_cast<microsec>(jitter2.average()).count()
        << " us" << std::endl;
    std::cout << "Median jitter is:   "
        << std::chrono::duration_cast<microsec>(jitter2.median()).count()
        << " us" << std::endl;
}

<commit_msg>Added diagnostics to show when the actual interval exceeds the desired limit<commit_after>#include <cstdint>
#include <chrono>
#include <thread>
#include <future>
#include <functional>
#include <random>
#include <iostream>
#include <vector>
#include <algorithm>

using microsec = std::chrono::microseconds;
using millisec = std::chrono::milliseconds;
using nanosec = std::chrono::nanoseconds;
using resolution = nanosec;
using my_clock = std::chrono::steady_clock;
using duration = my_clock::duration;

#define INTERVAL_PERIOD         resolution(10000000)
#define JITTER_MIN              100000
#define JITTER_MAX              1000000
#define ITERATION_MAX           400
#define RUNTIME_LIMIT    resolution(4000000000)

class TimeDurations {
    std::vector<duration> jitters_;
    duration smallest_;
    duration largest_;

public:
    TimeDurations()
        : jitters_(ITERATION_MAX)
        , smallest_(resolution::max())
        , largest_(resolution::min()) {
    }

    void
        insert(duration j) {
        jitters_.push_back(j);

        if (j < smallest_) {
            smallest_ = j;
        }

        if (j > largest_) {
            largest_ = j;
        }
    }

    duration average() {
        duration result(0);
        for (auto j : jitters_) {
            result = result + j;
        }

        result /= jitters_.size();

        return result;
    }

    duration
        largest() {
        return largest_;
    }

    duration
        smallest() {
        return smallest_;
    }

    duration
        median() {
        std::sort(jitters_.begin(), jitters_.end());
        return jitters_[jitters_.size() / 2];
    }
};

template <int IntervalMin, int IntervalMax>
class PeriodicTimer {
private:
    volatile bool is_running_ = false;
    std::future<int> pending_;
    my_clock::time_point firstInterval_;
    my_clock::time_point lastInterval_;

    //! @brief call doIt until stop() is called, and return the number of
    // iterations for which doIt was called.
    int doItTimed(std::function<void(duration)> doIt) {
        TimeDurations durations;
        int result = 0;
        std::random_device seedGenerator;
        std::mt19937 gen(seedGenerator());
        std::uniform_int_distribution<> distribution(IntervalMin,
                                                     IntervalMax);
        resolution jitter = resolution(distribution(gen));
        firstInterval_ = my_clock::now();
        my_clock::time_point start{firstInterval_};
        my_clock::time_point startNext{firstInterval_};

        while (is_running_) {
            // Delay the function call for a small random amount of time.
            std::this_thread::sleep_for(jitter);
            doIt(jitter);
            ++result;

            // Calculate the next time we want to iterate
            startNext = start + INTERVAL_PERIOD;
            auto currentTime = my_clock::now();
            durations.insert(currentTime - start);
            std::chrono::steady_clock::time_point jitter_time = start + jitter;
            std::chrono::duration<double> time_span_jitter
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    jitter_time - start);
            std::chrono::duration<double> time_span_actual
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    currentTime - start);
            std::chrono::duration<double> time_span_expected
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    startNext - start);

            /* Some days it takes just over 10 ms to run doIt(), so it helps
            to see what's happening. As it is, doIt() regularly takes over 3 ms
            on my old 2.4 GHz core i5 desktop, running Windows 10. That seems
            excessive.
            */
            if (currentTime < startNext) {
                // Wait for the next period start time
                std::this_thread::sleep_until(startNext);
            } else {
                std::cout << "Warning: Missed interval " << result
                    << ". Jitter: " << jitter.count() << ". Jitter duration: "
                    << time_span_jitter.count() << ". Duration: "
                    << time_span_actual.count() << ". Interval Limit: "
                    << time_span_expected.count() << std::endl;
            }

            // Get a new jitter and start time for the next iteration
            jitter = resolution(distribution(gen));
            start = startNext;
        }

        lastInterval_ = startNext;
        std::cout << "Shortest interval is  "
            << std::chrono::duration_cast<microsec>(durations.smallest()).count()
            << " us" << std::endl;
        std::cout << "Longest interval is   "
            << std::chrono::duration_cast<microsec>(durations.largest()).count()
            << " us" << std::endl;
        std::cout << "Average interval is   "
            << std::chrono::duration_cast<microsec>(durations.average()).count()
            << " us" << std::endl;
        std::cout << "Median interval is:   "
            << std::chrono::duration_cast<microsec>(durations.median()).count()
            << " us" << std::endl << std::endl;

        return result;
    }

public:
    //! @brief call doIt for repeat_count iterations.
    // A random delay (jitter) is calculated for each call to doIt. If doIt
    // runs for less than the delay, doItCount will wait for the remaining time
    // before the next interval. If doIt takes more time than the delay, the
    // next iteration takes place immediately. This way, doIt is called no more
    // often than
    void doItCount(std::function<void(resolution)> doIt,
                        uint32_t repeat_count) {
        TimeDurations durations;
        std::random_device seedGenerator;
        std::mt19937 gen(seedGenerator());
        std::uniform_int_distribution<> distribution(IntervalMin,
                                                     IntervalMax);

        resolution jitter = resolution(distribution(gen));
        firstInterval_ = my_clock::now();
        my_clock::time_point start{firstInterval_};
        my_clock::time_point startNext{firstInterval_};

        uint32_t itr = 0;
        while (itr < repeat_count) {
            // Delay the function call for a small random amount of time.
            std::this_thread::sleep_for(jitter);
            doIt(jitter);
            ++itr;

            // Collect some stats
            startNext = start + INTERVAL_PERIOD;
            auto currentTime = my_clock::now();
            durations.insert(currentTime - start);
            std::chrono::steady_clock::time_point jitter_time = start + jitter;
            std::chrono::duration<double> time_span_jitter
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    jitter_time - start);
            std::chrono::duration<double> time_span_actual
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    currentTime - start);
            std::chrono::duration<double> time_span_expected
                = std::chrono::duration_cast<std::chrono::duration<double>>(
                    startNext - start);

            if (currentTime < startNext) {
                // Wait for the next period start time
                std::this_thread::sleep_until(startNext);
            } else {
                std::cout << "Warning: Missed interval " << itr
                    << ". Jitter: " << jitter.count() << ". Jitter duration: "
                    << time_span_jitter.count() << ". Duration: "
                    << time_span_actual.count() << ". Interval Limit: "
                    << time_span_expected.count() << std::endl;
            }

            // Get a new jitter for the next iteration
            jitter = resolution(distribution(gen));
            start = startNext;
        }

        lastInterval_ = my_clock::now();
        std::cout << "Shortest interval is  "
            << std::chrono::duration_cast<microsec>(durations.smallest()).count()
            << " us" << std::endl;
        std::cout << "Longest interval is   "
            << std::chrono::duration_cast<microsec>(durations.largest()).count()
            << " us" << std::endl;
        std::cout << "Average interval is   "
            << std::chrono::duration_cast<microsec>(durations.average()).count()
            << " us" << std::endl;
        std::cout << "Median interval is:   "
            << std::chrono::duration_cast<microsec>(durations.median()).count()
            << " us" << std::endl << std::endl;
    }

    void start(std::function<void(resolution)> doIt) {
        is_running_ = true;
        // Run doItTimed on another thread, passing the "this" pointer and the
        // function doItTimed must run until stop() is executed.
        auto f = std::async(std::launch::async,
                            &PeriodicTimer::doItTimed,
                            this,
                            doIt);
        // Move the future to another variable so we don't wait for it here.
        pending_ = std::move(f);
    }

    int stop() {
        // Allow doItTimed to exit its while-loop
        is_running_ = false;

        // Return the number of iterations
        return pending_.get();
    }

    my_clock::duration runtime() const {
        return lastInterval_ - firstInterval_;
    }
};

void main() {
    TimeDurations jitter1;
    const resolution jitterMin = resolution(JITTER_MIN);
    const resolution jitterMax = resolution(JITTER_MAX);
    const resolution repeatInterval = INTERVAL_PERIOD;
    PeriodicTimer<JITTER_MIN, JITTER_MAX> timer;

    std::cout << "The resolution of the high-resolution clock is: "
        << (static_cast<double>(std::chrono::high_resolution_clock::period::num)
            / std::chrono::high_resolution_clock::period::den) << " sec"
        << std::endl;
    std::cout << "The resolution of the steady clock is:          "
        << (static_cast<double>(std::chrono::steady_clock::period::num)
            / std::chrono::steady_clock::period::den)
        << " sec" << std::endl;
    std::cout << "The resolution of the system clock is:          "
        << (static_cast<double>(std::chrono::system_clock::period::num)
            / std::chrono::system_clock::period::den)
        << " sec" << std::endl << std::endl;

    std::cout << "Jitter Tests." << std::endl
        << "  Interval:   "
        << std::chrono::duration_cast<millisec>(repeatInterval).count()
        << " ms" << std::endl
        << "  Min Jitter: "
        << std::chrono::duration_cast<microsec>(jitterMin).count() << " us"
        << std::endl
        << "  Max Jitter: "
        << std::chrono::duration_cast<microsec>(jitterMax).count() << " us"
        << std::endl << std::endl;

    /*** Iteration Test ***/
    std::cout << "Jitter test 1. Iterations: " << ITERATION_MAX << std::endl;
    timer.doItCount(std::bind(&TimeDurations::insert, &jitter1, std::placeholders::_1), ITERATION_MAX);
    auto runtime = timer.runtime();

    std::cout << "Iterations            " << ITERATION_MAX << std::endl;
    std::cout << "Expected elapsed time "
        << (ITERATION_MAX * std::chrono::duration_cast<millisec>(repeatInterval).count())
        << " ms" << std::endl;
    std::cout << "Actual elapsed time   "
        << std::chrono::duration_cast<millisec>(runtime).count()
        << " ms" << std::endl;
    std::cout << "Smallest jitter is    "
        << std::chrono::duration_cast<microsec>(jitter1.smallest()).count()
        << " us" << std::endl;
    std::cout << "Largest jitter is     "
        << std::chrono::duration_cast<microsec>(jitter1.largest()).count()
        << " us" << std::endl;
    std::cout << "Average jitter is     "
        << std::chrono::duration_cast<microsec>(jitter1.average()).count()
        << " us" << std::endl;
    std::cout << "Median jitter is      "
        << std::chrono::duration_cast<microsec>(jitter1.median()).count()
        << " us" << std::endl;

    std::cout << std::endl;

    /*** Timed Test ***/
    TimeDurations jitter2;
    const resolution iterationTimeLimit = RUNTIME_LIMIT;
    std::cout << "Jitter test 2. Timed : "
        << std::chrono::duration_cast<millisec>(iterationTimeLimit).count() << " ms" << std::endl;

    timer.start(std::bind(&TimeDurations::insert, &jitter2, std::placeholders::_1));
    std::this_thread::sleep_for(iterationTimeLimit);
    int iterations = timer.stop();
    runtime = timer.runtime();

    std::cout << "Expected iterations " << runtime / repeatInterval
        << std::endl;
    std::cout << "Actual iterations   " << iterations << std::endl;
    std::cout << "Elapsed time        "
        << std::chrono::duration_cast<millisec>(runtime).count()
        << " ms" << std::endl;
    std::cout << "Smallest jitter is  "
        << std::chrono::duration_cast<microsec>(jitter2.smallest()).count()
        << " us" << std::endl;
    std::cout << "Largest jitter is   "
        << std::chrono::duration_cast<microsec>(jitter2.largest()).count()
        << " us" << std::endl;
    std::cout << "Average jitter is   "
        << std::chrono::duration_cast<microsec>(jitter2.average()).count()
        << " us" << std::endl;
    std::cout << "Median jitter is:   "
        << std::chrono::duration_cast<microsec>(jitter2.median()).count()
        << " us" << std::endl;
}

<|endoftext|>


<commit_before>#include <string.h>
#include <iostream>
#include <unistd.h>
#include <string>
#include <sys/types.h>
#include <sys/wait.h>
#include <stdio.h>
#include <errno.h>
#include <vector>

using namespace std;

int check_connect(const string& str, int& pos, int start){
	unsigned found = 0;
	found = str.find_first_of(";&|#",start);
	cout << "FLAG: entering check function" << endl;
	if (found >= str.size()){
		pos = -1;
		return -1;
	}
	else{
		cout << "FLAG: checking connectors" << endl;
		if (str.at(found) == ';'){
			pos = found;
			return 1;
		}
		else if (str.at(found) == '&'){
			//FIX check for next &
			cout << "FLAG: found &" << endl;
			if (str.at(found+1) == '&'){
				pos = found;
				return 2;
			}
			else{
				cout << "flag: about to recursively call" << endl;
				return check_connect(str,pos,found+1);
			}
		}

		else if (str.at(found) == '|'){
			//FIX check for next |
			if (str.at(found+1) == '|'){
				pos = found;
				return 3;
			}
			else{
				cout << "flag: about to recursively call 2" << endl;
				return check_connect(str,pos,found+1);
			}
		}
		else if (str.at(found) == '#'){
			pos = found;
			return 4;
		}
	}
	return -1;
}

int main()
{
	int start = 0;
	int c_stat;
	int pos = -1;
	vector<string> str;
	str.push_back("The first string, no connectors");
	str.push_back("Second string; Semicolon included");
	str.push_back("&Third: && got that ampersand");
	str.push_back("We has | in i||t");
	str.push_back("This has a #comment");
	cout << "entering for loop" << endl;
	for (int i = 0; i < 5; ++i){
		c_stat = check_connect(str.at(i), pos, start);
		cout << "string " << i << ": " << c_stat << " at: " << pos << endl;
	}
	return 0;
}

<commit_msg>clean up connector function before merge<commit_after>#include <string.h>
#include <iostream>
#include <unistd.h>
#include <string>
#include <sys/types.h>
#include <sys/wait.h>
#include <stdio.h>
#include <errno.h>
#include <vector>

using namespace std;

int check_connect(const string& str, int& pos, int start){
	unsigned found = 0;
	found = str.find_first_of(";&|#",start);
	if (found >= str.size()){
		pos = -1;
		return -1;
	}
	else{
		if (str.at(found) == ';'){
			pos = found;
			return 1;
		}
		else if (str.at(found) == '&'){
			if (str.at(found+1) == '&'){
				pos = found;
				return 2;
			}
			else{
				return check_connect(str,pos,found+1);
			}
		}

		else if (str.at(found) == '|'){
			if (str.at(found+1) == '|'){
				pos = found;
				return 3;
			}
			else{
				return check_connect(str,pos,found+1);
			}
		}
		else if (str.at(found) == '#'){
			pos = found;
			return 4;
		}
	}
	return -1;
}

int main()
{
	int start = 0;
	int c_stat;
	int pos = -1;
	vector<string> str;
	str.push_back("The first string, no connectors");
	str.push_back("Second string; Semicolon included");
	str.push_back("&Third: && got that ampersand");
	str.push_back("We has | in i||t");
	str.push_back("This has a #comment");
	cout << "entering for loop" << endl;
	for (int i = 0; i < 5; ++i){
		c_stat = check_connect(str.at(i), pos, start);
		cout << "string " << i << ": " << c_stat << " at: " << pos << endl;
	}
	return 0;
}

<|endoftext|>


<commit_before>#include <Bounce2.h>
#include "FastLED.h"
#include <EEPROM.h>

// Digital PINs
#define LED_PIN_CH1 4
#define LED_PIN_CH2 2
#define MODE_BUTTON_PIN 6
#define BRIGHTNESS_BUTTON_PIN 8
#define BEAT_BUTTON_PIN 10

// Analog PINs
#define ACCELX_PIN 0
#define ACCELY_PIN 2
#define ACCELZ_PIN 4

// Other constants
#define BAUD_RATE 9600
#define NUM_LEDS_CH1 120
#define NUM_LEDS_CH2 29
#define FRAMES_PER_SECOND 30

// ui
int mode = 1;
int modeCount = 4;
Bounce modeButton;
Bounce brightnessButton;
Bounce beatButton;

// config
int currentBrightnessIndex = 1; // start with medium brightness
byte brightnesses[] = {30,60,90};
int staticBpm = 60; // in case no pulse is found
int pulseBpm = 0; // actually detected value
int maxBpm = 180; // avoids the system going crazy
byte beat[]  = {2,2,2,2,3,4,3,2,1,0,10,2,2,3,4,6,8,5,3,3,3,3}; // From http://ecg.utah.edu/img/items/Normal%2012_Lead%20ECG.jpg
byte beatLength = 22;
int beatMaxIntensity = 10; // max value in beat array
int movesPerBeat = 4;
int minMagnitude = 0;
int maxMagnitude = 50; // max difference between two magnitude measurements from accelerometer at which we show maxHue
int minBrightnessDivisor= 150; // lower = brighter
int maxBrightnessDivisor = 300; // higher = dimmer
int minHue = 160; // hue value under low motion, as defined through minMagnitude (from 0-255)
int maxHue = 255; // hue value under high motion, as defined through maxMagnitude (from 0-255)
int gainRatePerBeat = 6; // change towards new target magnitude
int decayRatePerBeat = 2; // move back towards minMagnitude
int sampleSize = 8; // smooth out accelerometer readings
int samplePerBeat = 1;

// state
int bpm = staticBpm;
byte bufr[NUM_LEDS_CH1];
byte offset = 0;
int currentMagnitude;
int targetMagnitude = maxMagnitude;
int adjustedMagnitude = maxMagnitude;

// pulse state
volatile boolean pulseFound = false; // "True" when User's live heartbeat is detected. "False" when not a "live beat".
volatile boolean pulseBeatFound = false; // becomes true when Arduoino finds a beat.
volatile boolean hasPulse = false; //

CRGB leds[2][NUM_LEDS_CH1];

void setup() {
  FastLED.addLeds<NEOPIXEL, LED_PIN_CH1>(leds[0], NUM_LEDS_CH1);
  FastLED.addLeds<NEOPIXEL, LED_PIN_CH2>(leds[1], NUM_LEDS_CH2);

  Serial.begin(BAUD_RATE);

  pinMode(MODE_BUTTON_PIN, INPUT_PULLUP);
  modeButton.attach(MODE_BUTTON_PIN);
  modeButton.interval(50);

  pinMode(BRIGHTNESS_BUTTON_PIN, INPUT_PULLUP);
  brightnessButton.attach(BRIGHTNESS_BUTTON_PIN);
  brightnessButton.interval(50);

  pinMode(BEAT_BUTTON_PIN, INPUT_PULLUP);
  beatButton.attach(BEAT_BUTTON_PIN);
  beatButton.interval(50);

  updateModeFromEEPROM();
}

// other pattern config
uint8_t gHue = 0; // rotating "base color" used by many of the patterns

// Cycle mode in persistent memory on every on switch.
// More resilient against hardware failures than a button.
void updateModeFromEEPROM() {
  mode = EEPROM.read(0);
  if(!mode) {
    mode = 1;
  }
  mode = (mode % modeCount) + 1;

  EEPROM.write(0, mode);
}

void updateModeFromButton() {
  modeButton.update();
  int current = modeButton.read();
  if(modeButton.fell()) {
    mode = (mode % modeCount) + 1;
    Serial.print("mode: ");
    Serial.println(mode);
  }
}

void updateBrightnessFromButton() {
  brightnessButton.update();
  int current = brightnessButton.read();
  if(brightnessButton.fell()) {
    currentBrightnessIndex = ((currentBrightnessIndex + 1) % 3);
    Serial.print("currentBrightnessIndex: ");
    Serial.println(currentBrightnessIndex);
  }
}

void moveBuffer() {
  int i;
  byte c = (int) beat[offset] * 255 / beatMaxIntensity;
  int midPoint = NUM_LEDS_CH1/2;

  // Move current beat intensity from middle to beginning of strip (backward in buffer)
  bufr[midPoint - 1] = c;
  for (i=0;i<midPoint - 1;i++){
    bufr[i] = bufr[i+1];
  }

  // Move current beat intensity from middle to end of strip (forward in buffer)
  bufr[midPoint] = c;
  for (i=NUM_LEDS_CH1 - 1;i>midPoint;i--){
    bufr[i] = bufr[i-1];
  }
}

void draw() {
  int i;

  // Go from a cool color on low to a warm color on high activity
  int hue = map(
    adjustedMagnitude,
    minMagnitude,
    maxMagnitude,
    minHue,
    maxHue
  );

  // Lower magnitude means more brightness reduction
  int brightnessFactor = map(
    adjustedMagnitude,
    minMagnitude,
    maxMagnitude,
    maxBrightnessDivisor,
    minBrightnessDivisor
  );

  // Channel 1
  for (i=0;i<NUM_LEDS_CH1;i++){
      leds[0][i] = CHSV(hue, bufr[i], bufr[i]/(brightnessFactor/100));
  }

  // Channel 2
  int halfPoint = (int)sizeof(bufr)/2;
  for (i=0;i<NUM_LEDS_CH2;i++){
      leds[1][i] = CHSV(hue, bufr[halfPoint], bufr[halfPoint]/(brightnessFactor/100));
  }

  FastLED.show();
}

int calcAdjustedMagnitude() {
  int newMagnitude = getMagnitude();
  int magnitudeDiff = abs(currentMagnitude - newMagnitude);

  // Get new target (smoothed out over a couple of readings)
  targetMagnitude = max(
    constrain(magnitudeDiff, minMagnitude, maxMagnitude),
    targetMagnitude
  );

  // Slowly work towards new target (either increasing or decreasing)
  if(adjustedMagnitude <= targetMagnitude) {
    adjustedMagnitude = constrain(
      constrain(
        targetMagnitude + gainRatePerBeat,
        minMagnitude,
        maxMagnitude
      ),
      minMagnitude,
      maxMagnitude
    );
  } else {
    adjustedMagnitude = constrain(
      constrain(
        targetMagnitude - gainRatePerBeat,
        minMagnitude,
        maxMagnitude
      ),
      minMagnitude,
      maxMagnitude
    );
  }

  // Slowly decay max target
  targetMagnitude = targetMagnitude - decayRatePerBeat;
  targetMagnitude = constrain(
    targetMagnitude,
    minMagnitude,
    maxMagnitude
  );

//  Serial.print(magnitudeDiff);
//  Serial.print("\t");
//  Serial.print(targetMagnitude);
//  Serial.print("\t");
//  Serial.print(adjustedMagnitude);
//  Serial.println();

  currentMagnitude = newMagnitude;
}

// Get a magnitude across all vectors.
// Smooth out result through a rolling average
int getMagnitude() {
  float avgMag = 0;
  for(int x = 0 ; x < sampleSize ; x++) {
    float aX = analogRead(ACCELX_PIN);
    float aY = analogRead(ACCELY_PIN);
    float aZ = analogRead(ACCELZ_PIN);

    float magnitude = sqrt((aX * aX) + (aY * aY) + (aZ * aZ));
    avgMag += magnitude;
  }
  avgMag /= sampleSize;

  return avgMag;
}

void loopHeartRate() {
  offset = (offset + 1) % beatLength;

  if (pulseFound == true && pulseBpm <= maxBpm) {
    bpm = pulseBpm;
  } else {
    bpm = staticBpm;
  }

  // TODO Fix BPM noise
  bpm = staticBpm;

  if (pulseBeatFound == true){
    pulseBeatFound = false;
  }

  int i;
  for (i=0;i<movesPerBeat;i++){
    // Delay in milliseconds. BPM are measured by minute, so divide accordingly.
    delay((float) 1 / (bpm * 60 * 1000) / beatLength / movesPerBeat);
    moveBuffer();
    draw();
  }

  // Measure only a few times per beat to avoid slowdowns
  if(offset % round(beatLength/samplePerBeat) == 0) {
    calcAdjustedMagnitude();
  }
}

void rainbow(int ledIndex, int num)
{
  // FastLED's built-in rainbow generator
  fill_rainbow( leds[ledIndex], num, gHue, 7);
}

void rainbowWithGlitter(int ledIndex, int num)
{
  // built-in FastLED rainbow, plus some random sparkly glitter
  rainbow(ledIndex, num);
  addGlitter(80, ledIndex, num);
}

void addGlitter( fract8 chanceOfGlitter, int ledIndex, int num)
{
  if( random8() < chanceOfGlitter) {
    leds[ledIndex][ random16(num) ] += CRGB::White;
  }
}

void confetti(int ledIndex, int num)
{
  // random colored speckles that blink in and fade smoothly
  fadeToBlackBy( leds[ledIndex], num, 10);
  int pos = random16(num);
  leds[ledIndex][pos] += CHSV( gHue + random8(64), 200, 255);
}

void sinelon(int ledIndex, int num)
{
  // a colored dot sweeping back and forth, with fading trails
  fadeToBlackBy( leds[ledIndex], num, 20);
  int pos = beatsin16(13,0,num);
  leds[ledIndex][pos] += CHSV( gHue, 255, 192);
}

void juggle(int ledIndex, int num) {
  // eight colored dots, weaving in and out of sync with each other
  fadeToBlackBy( leds[ledIndex], num, 20);
  byte dothue = 0;
  for( int i = 0; i < 8; i++) {
    leds[ledIndex][beatsin16(i+7,0,num)] |= CHSV(dothue, 200, 255);
    dothue += 32;
  }
}

void loop() {
  updateModeFromButton();
  updateBrightnessFromButton();

  int brightness = brightnesses[currentBrightnessIndex];

  // Activate effect based on mode
  if(mode == 1) {
    // Bright pulse meter
    minBrightnessDivisor = 200;
    maxBrightnessDivisor = 400;
    loopHeartRate();
  } else if (mode == 2) {
    FastLED.setBrightness(brightness);
    confetti(0, NUM_LEDS_CH1);
    confetti(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  } else if (mode == 3) {
    FastLED.setBrightness(brightness);
    sinelon(0, NUM_LEDS_CH1);
    sinelon(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  } else if (mode == 4) {
    FastLED.setBrightness(brightness);
    juggle(0, NUM_LEDS_CH1);
    juggle(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  }

}
<commit_msg>Moved function declarations around to shut up linter<commit_after>#include <Bounce2.h>
#include "FastLED.h"
#include <EEPROM.h>

// Digital PINs
#define LED_PIN_CH1 4
#define LED_PIN_CH2 2
#define MODE_BUTTON_PIN 6
#define BRIGHTNESS_BUTTON_PIN 8
#define BEAT_BUTTON_PIN 10

// Analog PINs
#define ACCELX_PIN 0
#define ACCELY_PIN 2
#define ACCELZ_PIN 4

// Other constants
#define BAUD_RATE 9600
#define NUM_LEDS_CH1 120
#define NUM_LEDS_CH2 29
#define FRAMES_PER_SECOND 30

// ui
int mode = 1;
int modeCount = 4;
Bounce modeButton;
Bounce brightnessButton;
Bounce beatButton;

// config
int currentBrightnessIndex = 1; // start with medium brightness
byte brightnesses[] = {30,60,90};
int staticBpm = 60; // in case no pulse is found
int pulseBpm = 0; // actually detected value
int maxBpm = 180; // avoids the system going crazy
byte beat[]  = {2,2,2,2,3,4,3,2,1,0,10,2,2,3,4,6,8,5,3,3,3,3}; // From http://ecg.utah.edu/img/items/Normal%2012_Lead%20ECG.jpg
byte beatLength = 22;
int beatMaxIntensity = 10; // max value in beat array
int movesPerBeat = 4;
int minMagnitude = 0;
int maxMagnitude = 50; // max difference between two magnitude measurements from accelerometer at which we show maxHue
int minBrightnessDivisor= 150; // lower = brighter
int maxBrightnessDivisor = 300; // higher = dimmer
int minHue = 160; // hue value under low motion, as defined through minMagnitude (from 0-255)
int maxHue = 255; // hue value under high motion, as defined through maxMagnitude (from 0-255)
int gainRatePerBeat = 6; // change towards new target magnitude
int decayRatePerBeat = 2; // move back towards minMagnitude
int sampleSize = 8; // smooth out accelerometer readings
int samplePerBeat = 1;

// state
int bpm = staticBpm;
byte bufr[NUM_LEDS_CH1];
byte offset = 0;
int currentMagnitude;
int targetMagnitude = maxMagnitude;
int adjustedMagnitude = maxMagnitude;

// pulse state
volatile boolean pulseFound = false; // "True" when User's live heartbeat is detected. "False" when not a "live beat".
volatile boolean pulseBeatFound = false; // becomes true when Arduoino finds a beat.
volatile boolean hasPulse = false; //

CRGB leds[2][NUM_LEDS_CH1];

// other pattern config
uint8_t gHue = 0; // rotating "base color" used by many of the patterns

// Cycle mode in persistent memory on every on switch.
// More resilient against hardware failures than a button.
void updateModeFromEEPROM() {
  mode = EEPROM.read(0);
  if(!mode) {
    mode = 1;
  }
  mode = (mode % modeCount) + 1;

  EEPROM.write(0, mode);
}

void updateModeFromButton() {
  modeButton.update();
  if(modeButton.fell()) {
    mode = (mode % modeCount) + 1;
    Serial.print("mode: ");
    Serial.println(mode);
  }
}

void updateBrightnessFromButton() {
  brightnessButton.update();
  if(brightnessButton.fell()) {
    currentBrightnessIndex = ((currentBrightnessIndex + 1) % 3);
    Serial.print("currentBrightnessIndex: ");
    Serial.println(currentBrightnessIndex);
  }
}

void moveBuffer() {
  int i;
  byte c = (int) beat[offset] * 255 / beatMaxIntensity;
  int midPoint = NUM_LEDS_CH1/2;

  // Move current beat intensity from middle to beginning of strip (backward in buffer)
  bufr[midPoint - 1] = c;
  for (i=0;i<midPoint - 1;i++){
    bufr[i] = bufr[i+1];
  }

  // Move current beat intensity from middle to end of strip (forward in buffer)
  bufr[midPoint] = c;
  for (i=NUM_LEDS_CH1 - 1;i>midPoint;i--){
    bufr[i] = bufr[i-1];
  }
}

void draw() {
  int i;

  // Go from a cool color on low to a warm color on high activity
  int hue = map(
    adjustedMagnitude,
    minMagnitude,
    maxMagnitude,
    minHue,
    maxHue
  );

  // Lower magnitude means more brightness reduction
  int brightnessFactor = map(
    adjustedMagnitude,
    minMagnitude,
    maxMagnitude,
    maxBrightnessDivisor,
    minBrightnessDivisor
  );

  // Channel 1
  for (i=0;i<NUM_LEDS_CH1;i++){
      leds[0][i] = CHSV(hue, bufr[i], bufr[i]/(brightnessFactor/100));
  }

  // Channel 2
  int halfPoint = (int)sizeof(bufr)/2;
  for (i=0;i<NUM_LEDS_CH2;i++){
      leds[1][i] = CHSV(hue, bufr[halfPoint], bufr[halfPoint]/(brightnessFactor/100));
  }

  FastLED.show();
}

// Get a magnitude across all vectors.
// Smooth out result through a rolling average
int getMagnitude() {
  float avgMag = 0;
  for(int x = 0 ; x < sampleSize ; x++) {
    float aX = analogRead(ACCELX_PIN);
    float aY = analogRead(ACCELY_PIN);
    float aZ = analogRead(ACCELZ_PIN);

    float magnitude = sqrt((aX * aX) + (aY * aY) + (aZ * aZ));
    avgMag += magnitude;
  }
  avgMag /= sampleSize;

  return avgMag;
}

void calcAdjustedMagnitude() {
  int newMagnitude = getMagnitude();
  int magnitudeDiff = abs(currentMagnitude - newMagnitude);

  // Get new target (smoothed out over a couple of readings)
  targetMagnitude = max(
    constrain(magnitudeDiff, minMagnitude, maxMagnitude),
    targetMagnitude
  );

  // Slowly work towards new target (either increasing or decreasing)
  if(adjustedMagnitude <= targetMagnitude) {
    adjustedMagnitude = constrain(
      constrain(
        targetMagnitude + gainRatePerBeat,
        minMagnitude,
        maxMagnitude
      ),
      minMagnitude,
      maxMagnitude
    );
  } else {
    adjustedMagnitude = constrain(
      constrain(
        targetMagnitude - gainRatePerBeat,
        minMagnitude,
        maxMagnitude
      ),
      minMagnitude,
      maxMagnitude
    );
  }

  // Slowly decay max target
  targetMagnitude = targetMagnitude - decayRatePerBeat;
  targetMagnitude = constrain(
    targetMagnitude,
    minMagnitude,
    maxMagnitude
  );

//  Serial.print(magnitudeDiff);
//  Serial.print("\t");
//  Serial.print(targetMagnitude);
//  Serial.print("\t");
//  Serial.print(adjustedMagnitude);
//  Serial.println();

  currentMagnitude = newMagnitude;
}

void loopHeartRate() {
  offset = (offset + 1) % beatLength;

  if (pulseFound == true && pulseBpm <= maxBpm) {
    bpm = pulseBpm;
  } else {
    bpm = staticBpm;
  }

  // TODO Fix BPM noise
  bpm = staticBpm;

  if (pulseBeatFound == true){
    pulseBeatFound = false;
  }

  int i;
  for (i=0;i<movesPerBeat;i++){
    // Delay in milliseconds. BPM are measured by minute, so divide accordingly.
    delay((float) 1 / (bpm * 60 * 1000) / beatLength / movesPerBeat);
    moveBuffer();
    draw();
  }

  // Measure only a few times per beat to avoid slowdowns
  if(offset % round(beatLength/samplePerBeat) == 0) {
    calcAdjustedMagnitude();
  }
}

void rainbow(int ledIndex, int num)
{
  // FastLED's built-in rainbow generator
  fill_rainbow( leds[ledIndex], num, gHue, 7);
}

void addGlitter( fract8 chanceOfGlitter, int ledIndex, int num)
{
  if( random8() < chanceOfGlitter) {
    leds[ledIndex][ random16(num) ] += CRGB::White;
  }
}

void rainbowWithGlitter(int ledIndex, int num)
{
  // built-in FastLED rainbow, plus some random sparkly glitter
  rainbow(ledIndex, num);
  addGlitter(80, ledIndex, num);
}

void confetti(int ledIndex, int num)
{
  // random colored speckles that blink in and fade smoothly
  fadeToBlackBy( leds[ledIndex], num, 10);
  int pos = random16(num);
  leds[ledIndex][pos] += CHSV( gHue + random8(64), 200, 255);
}

void sinelon(int ledIndex, int num)
{
  // a colored dot sweeping back and forth, with fading trails
  fadeToBlackBy( leds[ledIndex], num, 20);
  int pos = beatsin16(13,0,num);
  leds[ledIndex][pos] += CHSV( gHue, 255, 192);
}

void juggle(int ledIndex, int num) {
  // eight colored dots, weaving in and out of sync with each other
  fadeToBlackBy( leds[ledIndex], num, 20);
  byte dothue = 0;
  for( int i = 0; i < 8; i++) {
    leds[ledIndex][beatsin16(i+7,0,num)] |= CHSV(dothue, 200, 255);
    dothue += 32;
  }
}

void setup() {
  FastLED.addLeds<NEOPIXEL, LED_PIN_CH1>(leds[0], NUM_LEDS_CH1);
  FastLED.addLeds<NEOPIXEL, LED_PIN_CH2>(leds[1], NUM_LEDS_CH2);

  Serial.begin(BAUD_RATE);

  pinMode(MODE_BUTTON_PIN, INPUT_PULLUP);
  modeButton.attach(MODE_BUTTON_PIN);
  modeButton.interval(50);

  pinMode(BRIGHTNESS_BUTTON_PIN, INPUT_PULLUP);
  brightnessButton.attach(BRIGHTNESS_BUTTON_PIN);
  brightnessButton.interval(50);

  pinMode(BEAT_BUTTON_PIN, INPUT_PULLUP);
  beatButton.attach(BEAT_BUTTON_PIN);
  beatButton.interval(50);

  updateModeFromEEPROM();
}

void loop() {
  updateModeFromButton();
  updateBrightnessFromButton();

  int brightness = brightnesses[currentBrightnessIndex];

  // Activate effect based on mode
  if(mode == 1) {
    // Bright pulse meter
    minBrightnessDivisor = 200;
    maxBrightnessDivisor = 400;
    loopHeartRate();
  } else if (mode == 2) {
    FastLED.setBrightness(brightness);
    confetti(0, NUM_LEDS_CH1);
    confetti(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  } else if (mode == 3) {
    FastLED.setBrightness(brightness);
    sinelon(0, NUM_LEDS_CH1);
    sinelon(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  } else if (mode == 4) {
    FastLED.setBrightness(brightness);
    juggle(0, NUM_LEDS_CH1);
    juggle(1, NUM_LEDS_CH2);
    FastLED.show();
    FastLED.delay(1000/FRAMES_PER_SECOND);
    EVERY_N_MILLISECONDS( 20 ) { gHue++; }
  }

}
<|endoftext|>


<commit_before>#define DISTANCECMCRITICAL 20
#define DISTANCECMWARNING 30
#define PUBLSIHINTERVAL 1000
#define TRYFOLLOWLINEINTERVAL 3000
#define WAITINTERVAL 5000
#define MOVESPEED 100

#include <Arduino.h>

#include <Streaming.h>
#include <ArduinoJson.h>
#include <MeMCore.h>

enum direction { FORWARD, BACKWARD, LEFT, RIGHT, STOP } moveDirection = STOP;

MeDCMotor motorL(M1);
MeDCMotor motorR(M2);
MeLineFollower lineFollower(PORT_2);
MeTemperature temperature(PORT_1, SLOT2);
MeUltrasonicSensor ultrasonicSensor(PORT_3);
MeLightSensor lightSensor(PORT_6);
MeIR ir;
MeRGBLed rgbLed(PORT_7, 2);
MeBuzzer buzzer;

double distanceCm = 0.0;
int moveSpeed = 100;
int lineFollowFlag = 0;
byte readSensors = 0;
bool start = false;
bool moveBot = false;
bool makeNoise = false;
bool obstacle = false;

unsigned long publishTimer = millis();
bool wait = false;
unsigned long waitTimer = millis();
bool tryFollowLine = true;
unsigned long tryFollowLineTimer = millis();

DynamicJsonBuffer jsonBuffer;
JsonObject& root = jsonBuffer.createObject();

void forward() {
  moveBot = true;
  motorL.run(-MOVESPEED);
  motorR.run(MOVESPEED);
}

void backward() {
  moveBot = true;
  motorL.run(MOVESPEED);
  motorR.run(-MOVESPEED);
}

void turnLeft() {
  moveBot = true;
  motorL.run(-MOVESPEED / 10);
  motorR.run(MOVESPEED);
}

void turnRight() {
  moveBot = true;
  motorL.run(-MOVESPEED);
  motorR.run(MOVESPEED / 10);
}

void stop() {
  moveBot = false;
  motorL.run(0);
  motorR.run(0);
}

bool distanceCheck(double distanceCmLimit) {
  if (distanceCm < distanceCmLimit) {
    return false;
  } else {
    return true;
  }
}

void move() {
  if (distanceCheck(DISTANCECMCRITICAL)) {
    switch (moveDirection) {
      case FORWARD:
      forward();
      break;
      case BACKWARD:
      backward();
      break;
      case LEFT:
      turnLeft();
      break;
      case RIGHT:
      turnRight();
      break;
      case STOP:
      stop();
      break;
    }
  } else {
    switch (moveDirection) {
      case BACKWARD:
      backward();
      break;
      default:
      moveDirection = STOP;
      stop();
      break;
    }
  }
}

void toggleStart() {
  start = !start;
  if (start) {
    rgbLed.setColor(1, 0, 10, 0);
    rgbLed.show();
  } else {
    rgbLed.setColor(1, 0, 0, 0);
    rgbLed.show();
    moveDirection = STOP;
  }
  delay(250);
}

void bottomCheck() {
  if (analogRead(A7) == 0) {
    toggleStart();
  }
}

void irCheck() {
  unsigned long value;

  if (ir.decode()) {
    value = ir.value;
    switch (value >> 16 & 0xff) {
      case IR_BUTTON_LEFT:
      moveDirection = LEFT;
      break;
      case IR_BUTTON_RIGHT:
      moveDirection = RIGHT;
      break;
      case IR_BUTTON_DOWN:
      moveDirection = BACKWARD;
      break;
      case IR_BUTTON_UP:
      moveDirection = FORWARD;
      break;
      case IR_BUTTON_SETTING:
      moveDirection = STOP;
      break;
      case IR_BUTTON_A:
      toggleStart();
      break;
    }
  }
}

void noiseCheck() {
  if (!moveBot and start) {
    if (wait == false) {
      wait = true;
      waitTimer = millis();
    }
    if (wait and millis() - waitTimer > WAITINTERVAL) {
      makeNoise = true;
      //buzzer.tone(262, 100);
      rgbLed.setColor(2, 10, 0, 0);
      rgbLed.show();
    } else {
      rgbLed.setColor(2, 0, 0, 10);
      rgbLed.show();
    }
  } else {
    wait = false;
    makeNoise = false;
    buzzer.noTone();
    rgbLed.setColor(2, 0, 0, 0);
    rgbLed.show();
  }
}

void autonomous() {
  byte randNumber;

  randomSeed(analogRead(6));
  randNumber = random(2);
  if (distanceCheck(DISTANCECMCRITICAL) and !obstacle) {
    moveDirection = FORWARD;
  } else {
    obstacle = true;
  }

  if (obstacle) {
    if (!distanceCheck(DISTANCECMWARNING)) {
      moveDirection = BACKWARD;
    } else {
      switch (randNumber) {
        case 0:
        moveDirection = LEFT;
        turnLeft();
        delay(200);
        break;
        case 1:
        moveDirection = RIGHT;
        turnRight();
        delay(200);
        break;
      }
      obstacle = false;
    }
  }
}

void readData() {
  readSensors = lineFollower.readSensors();
  distanceCm = ultrasonicSensor.distanceCm();
}

void sendData() {
  if (millis() - publishTimer > PUBLSIHINTERVAL) {
    publishTimer = millis();
    root["moveBot"] = moveBot;
    root["makeNoise"] = makeNoise;
    root["distanceCm"] = distanceCm;
    root["temperature"] = temperature.temperature();
    root["lightSensor"] = lightSensor.read();
    Serial << endl;
    root.prettyPrintTo(Serial);
  }
}

void setup() {
  Serial.begin(115200);
  Serial << endl << endl;
  pinMode(A7, INPUT);
  ir.begin();
  rgbLed.setColor(0, 0, 0, 0);
  rgbLed.show();
}

void loop() {
  bottomCheck();
  irCheck();
  readData();
  sendData();
  noiseCheck();
  move();
  if (start) {
    switch (readSensors) {
      case S1_IN_S2_IN:
      tryFollowLine = true;
      moveDirection = FORWARD;
      lineFollowFlag = 10;
      break;
      case S1_IN_S2_OUT:
      tryFollowLine = true;
      moveDirection = FORWARD;
      if (lineFollowFlag > 1) lineFollowFlag--;
      break;
      case S1_OUT_S2_IN:
      tryFollowLine = true;
      moveDirection = FORWARD;
      if (lineFollowFlag < 20) lineFollowFlag++;
      break;
      case S1_OUT_S2_OUT:
      if (tryFollowLine) {
        tryFollowLine = !tryFollowLine;
        tryFollowLineTimer = millis();
      }
      if (millis() - tryFollowLineTimer < TRYFOLLOWLINEINTERVAL) {
        if (lineFollowFlag == 10) moveDirection = BACKWARD;
        if (lineFollowFlag < 10) moveDirection = LEFT;
        if (lineFollowFlag > 10) moveDirection = RIGHT;
      } else {
        autonomous();
      }
      break;
    }
  }
}
<commit_msg>cleanup, bugfix, readability<commit_after>#define DISTANCECMCRITICAL 20
#define DISTANCECMWARNING 30
#define PUBLSIHINTERVAL 1000
#define TRYFOLLOWLINEINTERVAL 3000
#define WAITINTERVAL 5000
#define MOVESPEED 100

#include <Arduino.h>

#include <Streaming.h>
#include <ArduinoJson.h>
#include <MeMCore.h>

enum direction { STOP, FORWARD, BACKWARD, LEFT, RIGHT } moveDirection = STOP;

MeDCMotor motorL(M1);
MeDCMotor motorR(M2);
MeLineFollower lineFollower(PORT_2);
MeTemperature temperature(PORT_4, SLOT2);
MeUltrasonicSensor ultrasonicSensor(PORT_3);
MeLightSensor lightSensor(PORT_6);
MeIR ir;
MeRGBLed rgbLed(PORT_7, 2);
MeBuzzer buzzer;

int moveSpeed = 100;
int lineFollowFlag = 0;
bool start = false;
bool makeNoise = false;
bool obstacle = false;

unsigned long publishTimer = millis();
bool wait = false;
unsigned long waitTimer = millis();
bool tryFollowLine = true;
unsigned long tryFollowLineTimer = millis();

DynamicJsonBuffer jsonBuffer;
JsonObject& root = jsonBuffer.createObject();

void forward() {
  motorL.run(-MOVESPEED);
  motorR.run(MOVESPEED);
}

void backward() {
  motorL.run(MOVESPEED);
  motorR.run(-MOVESPEED);
}

void turnLeft() {
  motorL.run(-MOVESPEED / 10);
  motorR.run(MOVESPEED);
}

void turnRight() {
  motorL.run(-MOVESPEED);
  motorR.run(MOVESPEED / 10);
}

void stop() {
  moveDirection = STOP;
  motorL.run(0);
  motorR.run(0);
}

bool distanceWarning(double distanceCmLimit) {
  if (ultrasonicSensor.distanceCm() < distanceCmLimit) {
    return true;
  } else {
    return false;
  }
}

void move() {
  if (distanceWarning(DISTANCECMCRITICAL)) {
    if (moveDirection == BACKWARD) {
      backward();
    } else {
      stop();
    }
  } else {
    switch (moveDirection) {
      case FORWARD:
      forward();
      break;
      case BACKWARD:
      backward();
      break;
      case LEFT:
      turnLeft();
      break;
      case RIGHT:
      turnRight();
      break;
      case STOP:
      stop();
      break;
    }
  }
}

void toggleStart() {
  start = !start;
  if (start) {
    rgbLed.setColor(1, 0, 10, 0);
    rgbLed.show();
  } else {
    rgbLed.setColor(1, 0, 0, 0);
    rgbLed.show();
    moveDirection = STOP;
  }
  delay(250);
}

void bottomCheck() {
  if (analogRead(A7) == 0) {
    toggleStart();
  }
}

void irCheck() {
  unsigned long value;

  if (ir.decode()) {
    value = ir.value;
    switch (value >> 16 & 0xff) {
      case IR_BUTTON_LEFT:
      moveDirection = LEFT;
      break;
      case IR_BUTTON_RIGHT:
      moveDirection = RIGHT;
      break;
      case IR_BUTTON_DOWN:
      moveDirection = BACKWARD;
      break;
      case IR_BUTTON_UP:
      moveDirection = FORWARD;
      break;
      case IR_BUTTON_SETTING:
      moveDirection = STOP;
      break;
      case IR_BUTTON_A:
      toggleStart();
      break;
    }
  }
}

void silent() {
  buzzer.noTone();
  rgbLed.setColor(2, 0, 0, 0);
  rgbLed.show();
}

void warning() {
  rgbLed.setColor(2, 0, 0, 10);
  rgbLed.show();
}

void alarm() {
  //buzzer.tone(262, 100);
  rgbLed.setColor(2, 10, 0, 0);
  rgbLed.show();
}

void noiseCheck() {
  if (start and distanceWarning(DISTANCECMCRITICAL)) {
    if (wait == false) {
      wait = true;
      waitTimer = millis();
    }
    if (wait and millis() - waitTimer > WAITINTERVAL) {
      makeNoise = true;
      alarm();
    } else {
      warning();
    }
  } else {
    wait = false;
    makeNoise = false;
    silent();
  }
}

void autonomous() {
  byte randNumber;

  randomSeed(analogRead(6));
  randNumber = random(2);
  if (!distanceWarning(DISTANCECMCRITICAL) and !obstacle) {
    moveDirection = FORWARD;
  } else {
    obstacle = true;
  }
  if (obstacle) {
    if (distanceWarning(DISTANCECMWARNING)) {
      moveDirection = BACKWARD;
    } else {
      switch (randNumber) {
        case 0:
        moveDirection = LEFT;
        turnLeft();
        delay(400);
        break;
        case 1:
        moveDirection = RIGHT;
        turnRight();
        delay(400);
        break;
      }
      obstacle = false;
    }
  }
}

void sendData() {
  if (millis() - publishTimer > PUBLSIHINTERVAL) {
    publishTimer = millis();
    root["start"] = start;
    root["moveDirection"] = moveDirection;
    root["wait"] = wait;
    root["makeNoise"] = makeNoise;
    root["distanceCm"] = ultrasonicSensor.distanceCm();
    root["lightSensor"] = lightSensor.read();
    root["temperature"] = temperature.temperature();
    root.prettyPrintTo(Serial);
    Serial << endl;
  }
}

void setup() {
  Serial.begin(115200);
  Serial << endl << endl;
  pinMode(A7, INPUT);
  ir.begin();
  rgbLed.setColor(0, 0, 0, 0);
  rgbLed.show();
}

void loop() {
  bottomCheck();
  irCheck();
  noiseCheck();
  sendData();
  move();
  if (start) {
    switch (lineFollower.readSensors()) {
      case S1_IN_S2_IN:
      tryFollowLine = true;
      moveDirection = FORWARD;
      lineFollowFlag = 10;
      break;
      case S1_IN_S2_OUT:
      tryFollowLine = true;
      moveDirection = FORWARD;
      if (lineFollowFlag > 1) lineFollowFlag--;
      break;
      case S1_OUT_S2_IN:
      tryFollowLine = true;
      moveDirection = FORWARD;
      if (lineFollowFlag < 20) lineFollowFlag++;
      break;
      case S1_OUT_S2_OUT:
      if (tryFollowLine) {
        tryFollowLine = !tryFollowLine;
        tryFollowLineTimer = millis();
      }
      if (millis() - tryFollowLineTimer < TRYFOLLOWLINEINTERVAL) {
        if (lineFollowFlag == 10) moveDirection = BACKWARD;
        if (lineFollowFlag < 10) moveDirection = LEFT;
        if (lineFollowFlag > 10) moveDirection = RIGHT;
      } else {
        autonomous();
      }
      break;
    }
  }
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <iostream>
#include <cstdlib>

using namespace std;

const int MAX_ARGS = 256;

int runCommands(char **argv) {

	int status;
	pid_t pid;

	pid = fork();

	if(pid < 0) {
		perror("Error: pid < 0!");
		return 0;
	}
	else if(pid == 0) {
		if(execvp(*argv, argv) < 0) {
			perror("execvp error!");
			return 0;
		}
	}
	while(wait(&status) != pid)
		;
	return 1;
}

void verificationConnector(int value, char **parsedTokens, char *statements) {

	if(strcmp(statements, "&&") == 0) {
		if(value){
            verificationConnector(1, parsedTokens, strtok(0, " "));
		}
	} else if(strcmp(statements, "||") == 0) {
		if(!value){
            verificationConnector(1, parsedTokens, strtok(0, " "));
		}
	} else if(statements == 0 || *statements == '#') {
		return;
	} else if(strcmp(statements, "exit") == 0) {
		exit(0);
	} else {
		int i;
		char *next = strtok(0, " ");
		*parsedTokens = statements;
		for(i = 1; next && *next != '#' && strcmp(next, "&&") && strcmp(next, "||"); i++) {
			parsedTokens[i] = next;
			next = strtok(0, " ");
		}

		verificationConnector(runCommands(parsedTokens), parsedTokens + i + 1, next);
	}
}

void handleCommands(char *commands) {
	char *statements[MAX_ARGS] = {0};
	char *parsedTokens[MAX_ARGS] = {0};
	int i = 0;

	for(char *tok = strtok(commands, ";"); tok; tok = strtok(0, ";")) {
		statements[i] = tok;
		i++;
	}

	for(int i = 0; statements[i]; i++) {
		verificationConnector(1, parsedTokens, strtok(statements[i], " "));
		for(int j = 0; parsedTokens[j]; j++) {
			parsedTokens[j] = 0;
		}
	}
}

void changeBuffer(string &buffer) {
	for(size_t i = 2; i < buffer.length(); i++) {
		if(buffer[i] == '&' && buffer[i - 1] == '&' && buffer[i - 2] != ' ') {
			buffer = buffer.substr(0, i - 1) + " && " + buffer.substr(i + 1);
			i+=2;
		} else if(buffer[i] == '|' && buffer[i - 1] == '|' && buffer[i - 2] != ' ') {
			buffer = buffer.substr(0, i - 1) + " || " + buffer.substr(i + 1);
			i+=2;
		} else if(buffer[i] == '#' && buffer[i - 1] != ' ') {
			buffer = buffer.substr(0, i - 1) + + " #" + buffer.substr(i + 1);
			i+=2;
		}
	}
}

int main() {
	string buffer;

	while(true) {
		cout << "$ ";
		getline(cin,buffer);
		changeBuffer(buffer);
		char *temp = (char *)buffer.c_str();
		handleCommands(temp);
	}

	return 0;
}

<commit_msg>updated the main file: have the DealCommand class<commit_after>#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <iostream>
#include <cstdlib>
#include "DealCommand.h"

using namespace std;

int main() {
	string buffer;
	DealCommand* deal = new DealCommand;

	while(true) {
		cout << "$ ";

		getline(cin,buffer);
		deal->changeBuffer(buffer);
		char *temp = (char *)buffer.c_str();
		deal->handleCommands(temp);
	}

	return 0;
}

<|endoftext|>


<commit_before>#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <e_line_judge/user_calibration.h>
#include <e_line_judge/ball_detection.h>
#include <e_line_judge/ball_tracking.h>
#include <e_line_judge/line_decision.h>
#include <e_line_judge/pre_processing.h>
#include <e_line_judge/surface_contact_detection.h>
#include <iostream>

cv::Point2i ball_center;

void mouseCallback(int event, int x, int y, int flags, void *param)
{
    if (event == CV_EVENT_LBUTTONDOWN)
    {
        ball_center.x = x;
        ball_center.y = y;
    }
}

int main(int argc, char** argv)
{
    UserCalibration uc;
    cv::Mat image;
    int frame_calibration_number;
    bool slow_motion = false;
    bool live_video = false;
    bool rotate_image = true;
    bool hardcoded_calibration = true;

    //reading example image, uncomment for debugging
    //image = cv::imread("../images/big_yellow_ball.jpg");

    cv::VideoCapture vidCap;

    if(argc == 1)
    {
        std::cout << "Using ball falling on the left video" << std::endl;
        vidCap = cv::VideoCapture("../data/video/left.MOV");
    }

    if(argc > 1)
    {
        std::cout << "argv = " << argv[1] << std::endl;

        if(!strcmp(argv[1], "left"))
        {
            std::cout << "Using ball falling on the left video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/left.MOV");
        }
        else if(!strcmp(argv[1], "right"))
        {
            std::cout << "Using ball falling on the right video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/right.MOV");
        }
        else if(!strcmp(argv[1], "center"))
        {
            std::cout << "Using ball falling on the center video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/center.MOV");
        }
        else if (!strcmp(argv[1], "live"))
        {
            std::cout << "Using live video" << std::endl;
            vidCap = cv::VideoCapture(1);
            live_video = true;
            rotate_image = false;
        }
        else if(!strcmp(argv[1], "file"))
        {
            if (argc > 2)
            {
                vidCap = cv::VideoCapture(argv[2]);
                if (!vidCap.isOpened())
                {
                    std::cout << "Cannot open file " << argv[2] << std::endl;
                    exit(0);
                }
            }
            else
            {
                std::cout << "Specify a filename " << std::endl;
                exit(0);
            }
        }
        if(argc > 3)
        {
            slow_motion = true;
        }
    }

    //rotating the image
    PreProcessing pp;

    if (!live_video)
    {
        frame_calibration_number = uc.getBallFrameEstimate();

        for(int i = 0; i < frame_calibration_number; i++)
        {
            vidCap >> image;
        }
        vidCap.set(CV_CAP_PROP_POS_AVI_RATIO, 0);
    }
    else
    {
        vidCap >> image;
    }

    std::cout << "Video size: " << image.cols << "x" << image.rows << std::endl;
    if (rotate_image)
    {
        std::cout << "Rotating and resizing video to: 360x640" << std::endl;
        pp.rotateImage(image, 360, 640);
    }

    std::cout << "Showing calibration frame" << std::endl;

    //get HSV range for the ball to calibrate
    cv::Scalar lower_range;
    cv::Scalar upper_range;
    double radius_estimate;
    if (!hardcoded_calibration)
    {
        uc.getBallHSVRange(image, lower_range, upper_range, radius_estimate, false);
    }
    else
    {
        //lower_range = cv::Scalar(12, 119, 200, 0);
        //upper_range = cv::Scalar(18, 249, 255, 0);
		lower_range = cv::Scalar(11, 118, 225, 0);
		upper_range = cv::Scalar(18, 249, 255, 0);
        radius_estimate = 10.0;
    }

    //get line coordinates
    std::vector<cv::Point2i> lines;
    uc.getLineLimits(image, lines);

    //Displaying the input lines
    /*
    cv::Mat img_copy;
    image.copyTo(img_copy);
    cv::line(img_copy, lines[0], lines[1], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[1], lines[2], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[2], lines[3], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[3], lines[0], cv::Scalar(0,0,255));
    cv::imshow("Lines", img_copy);
    */

    uc.setDisplay("Starting Electronic Line Judge...");

    //tracking the ball
    //restarting the video capture to frame 0

    //creating instance of tracking class
    BallDetection bd(lower_range, upper_range, radius_estimate);
    cv::Point2i ball_center;
    LineDecision ld(lines);
    int decision_result = 0;
    double ball_radius;

	std::queue<cv::Mat> last_frames;
	int frames_back = 1; //taking one frame back for the decision after the change in direction has been detected
    SurfaceContactDetection scd;
    cv::Point2i previous_ball_center(0.0, 0.0);
    int ball_detection_iteration = 0;

    bool made_decision = false;

    while (true)
    {
        // Read a video frame
        vidCap >> image;
		cv::Mat image_copy;

        if(!image.empty())
        {
            //rotating frame
            if (rotate_image)
            {
                pp.rotateImage(image, 360, 640);
            }

            //avoiding pushing a pointer (gives runtime error) and pushing a copy instead
            image.copyTo(image_copy);
            last_frames.push(image_copy);

			if(last_frames.size() > frames_back + 1)
			{
				last_frames.pop();
			}

            //do stuff
            if(bd.detect_ball(image, ball_center, ball_radius))
            {
                //after detection of the ball, print its value on y
                //std::cout << "ball y coordinate : " << ball_center.y << std::endl;

                //drawing detected circles
                // circle center
                cv::circle(image, ball_center, 3, cv::Scalar(0,255,0), -1, 8, 0);
                // circle outline
                cv::circle(image, ball_center, ball_radius, cv::Scalar(0,0,255), 3, 8, 0);

                std::cout << "prev: " << previous_ball_center.y << ", curr: " << ball_center.y << std::endl;
                if(!made_decision && ball_detection_iteration != 0 && scd.hasTrayectoryChanged(previous_ball_center.y, ball_center.y))
                {
					//taking two frames back
					cv::Mat decision_frame;
					last_frames.front().copyTo(decision_frame);

					//result image will store lines and marked ball with the decision
					cv::Mat result_image;

					//detect ball on decision frame
					if(bd.detect_ball(decision_frame, ball_center, ball_radius))
					{
						//testing the line decision, uncomment for debugging
						cv::Point2i projected_ball_center = ball_center;
						projected_ball_center.y += ball_radius;
						cv::circle(decision_frame, projected_ball_center, 3, cv::Scalar(255,0,0), -1, 8, 0);
						decision_result = ld.getDecision(decision_frame, projected_ball_center, ball_radius);

						// result image
						decision_frame.copyTo(result_image);
						cv::line(result_image, lines[0], lines[1], cv::Scalar(0,0,255));
						cv::line(result_image, lines[1], lines[2], cv::Scalar(0,0,255));
						cv::line(result_image, lines[2], lines[3], cv::Scalar(0,0,255));
						cv::line(result_image, lines[3], lines[0], cv::Scalar(0,0,255));
					}
					else
					{
						std::cout << "Error : ball does not exist in decision frame" << std::endl;
					}

                    if(decision_result == -1)
                    {
                        uc.setDisplay("Decision: LEFT");
                        std::cout << "Left" << std::endl;
                        cv::putText(result_image, "LEFT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    else if(decision_result == 0)
                    {
                        if (live_video)
                        {
                            uc.setDisplay("Decision: LEFT");
                            cv::putText(result_image, "LEFT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                            std::cout << "Left" << std::endl;
                        }
                        else
                        {
                            uc.setDisplay("Decision: ON LINE");
                            cv::putText(result_image, "ON LINE", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                            std::cout << "On Line" << std::endl;
                        }
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    else
                    {
                        uc.setDisplay("Decision: RIGHT");
                        cv::putText(result_image, "RIGHT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                        std::cout << "Right" << std::endl;
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    cv::imshow("Result", result_image);
                }
                ball_detection_iteration++;
                previous_ball_center = ball_center;
            }

            cv::line(image, lines[0], lines[1], cv::Scalar(0,0,255));
            cv::line(image, lines[1], lines[2], cv::Scalar(0,0,255));
            cv::line(image, lines[2], lines[3], cv::Scalar(0,0,255));
            cv::line(image, lines[3], lines[0], cv::Scalar(0,0,255));
            cv::imshow("Ball detection", image);

            // Quit the loop when a key is pressed, also give a delay on the video
            int wait_time = 5;
            if (slow_motion)
            {
                wait_time = 500;
            }
            int keyPressed = cv::waitKey(wait_time);
            if (keyPressed != -1) break;
        }
        else
        {
            vidCap.set(CV_CAP_PROP_POS_AVI_RATIO , 0);
            std::cout << "Reached end of video, playing again" << std::endl;
            uc.setDisplay("Starting Electronic Line Judge...");
            made_decision = false;
            previous_ball_center.x = 0.0;
            previous_ball_center.y = 0.0;
            ball_detection_iteration = 0;
            cv::destroyWindow("Result");
        }
    }

    return 0;
}
<commit_msg>using current frame if previous frame contains no ball<commit_after>#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <e_line_judge/user_calibration.h>
#include <e_line_judge/ball_detection.h>
#include <e_line_judge/ball_tracking.h>
#include <e_line_judge/line_decision.h>
#include <e_line_judge/pre_processing.h>
#include <e_line_judge/surface_contact_detection.h>
#include <iostream>

cv::Point2i ball_center;

void mouseCallback(int event, int x, int y, int flags, void *param)
{
    if (event == CV_EVENT_LBUTTONDOWN)
    {
        ball_center.x = x;
        ball_center.y = y;
    }
}

int main(int argc, char** argv)
{
    UserCalibration uc;
    cv::Mat image;
    int frame_calibration_number;
    bool slow_motion = false;
    bool live_video = false;
    bool rotate_image = true;
    bool hardcoded_calibration = true;

    //reading example image, uncomment for debugging
    //image = cv::imread("../images/big_yellow_ball.jpg");

    cv::VideoCapture vidCap;

    if(argc == 1)
    {
        std::cout << "Using ball falling on the left video" << std::endl;
        vidCap = cv::VideoCapture("../data/video/devel_videos/left.MOV");
    }

    if(argc > 1)
    {
        std::cout << "argv = " << argv[1] << std::endl;

        if(!strcmp(argv[1], "left"))
        {
            std::cout << "Using ball falling on the left video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/devel_videos/left.MOV");
        }
        else if(!strcmp(argv[1], "right"))
        {
            std::cout << "Using ball falling on the right video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/devel_videos/right.MOV");
        }
        else if(!strcmp(argv[1], "center"))
        {
            std::cout << "Using ball falling on the center video" << std::endl;
            vidCap = cv::VideoCapture("../data/video/devel_videos/center.MOV");
        }
        else if (!strcmp(argv[1], "live"))
        {
            std::cout << "Using live video" << std::endl;
            vidCap = cv::VideoCapture(1);
            live_video = true;
            rotate_image = false;
        }
        else if(!strcmp(argv[1], "file"))
        {
            if (argc > 2)
            {
                vidCap = cv::VideoCapture(argv[2]);
                if (!vidCap.isOpened())
                {
                    std::cout << "Cannot open file " << argv[2] << std::endl;
                    exit(0);
                }
            }
            else
            {
                std::cout << "Specify a filename " << std::endl;
                exit(0);
            }
        }
        if(argc > 3)
        {
            slow_motion = true;
        }
    }

    //rotating the image
    PreProcessing pp;

    if (!live_video)
    {
        frame_calibration_number = uc.getBallFrameEstimate();

        for(int i = 0; i < frame_calibration_number; i++)
        {
            vidCap >> image;
        }
        vidCap.set(CV_CAP_PROP_POS_AVI_RATIO, 0);
    }
    else
    {
        vidCap >> image;
    }

    std::cout << "Video size: " << image.cols << "x" << image.rows << std::endl;
    if (rotate_image)
    {
        std::cout << "Rotating and resizing video to: 360x640" << std::endl;
        pp.rotateImage(image, 360, 640);
    }

    std::cout << "Showing calibration frame" << std::endl;

    //get HSV range for the ball to calibrate
    cv::Scalar lower_range;
    cv::Scalar upper_range;
    double radius_estimate;
    if (!hardcoded_calibration)
    {
        uc.getBallHSVRange(image, lower_range, upper_range, radius_estimate, false);
    }
    else
    {
        //lower_range = cv::Scalar(12, 119, 200, 0);
        //upper_range = cv::Scalar(18, 249, 255, 0);
		lower_range = cv::Scalar(11, 118, 225, 0);
		upper_range = cv::Scalar(18, 249, 255, 0);
        radius_estimate = 10.0;
    }

    //get line coordinates
    std::vector<cv::Point2i> lines;
    uc.getLineLimits(image, lines);

    //Displaying the input lines
    /*
    cv::Mat img_copy;
    image.copyTo(img_copy);
    cv::line(img_copy, lines[0], lines[1], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[1], lines[2], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[2], lines[3], cv::Scalar(0,0,255));
    cv::line(img_copy, lines[3], lines[0], cv::Scalar(0,0,255));
    cv::imshow("Lines", img_copy);
    */

    uc.setDisplay("Starting Electronic Line Judge...");

    //tracking the ball
    //restarting the video capture to frame 0

    //creating instance of tracking class
    BallDetection bd(lower_range, upper_range, radius_estimate);
    cv::Point2i ball_center;
    LineDecision ld(lines);
    int decision_result = 0;
    double ball_radius;

	std::queue<cv::Mat> last_frames;
	int frames_back = 1; //taking one frame back for the decision after the change in direction has been detected
    SurfaceContactDetection scd;
    cv::Point2i previous_ball_center(0.0, 0.0);
    int ball_detection_iteration = 0;

    bool made_decision = false;

    while (true)
    {
        // Read a video frame
        vidCap >> image;
		cv::Mat image_copy;

        if(!image.empty())
        {
            //rotating frame
            if (rotate_image)
            {
                pp.rotateImage(image, 360, 640);
            }

            //avoiding pushing a pointer (gives runtime error) and pushing a copy instead
            image.copyTo(image_copy);
            last_frames.push(image_copy);

			if(last_frames.size() > frames_back + 1)
			{
				last_frames.pop();
			}

            //do stuff
            if(bd.detect_ball(image, ball_center, ball_radius))
            {
                //after detection of the ball, print its value on y
                //std::cout << "ball y coordinate : " << ball_center.y << std::endl;

                std::cout << "prev: " << previous_ball_center.y << ", curr: " << ball_center.y << std::endl;
                if(!made_decision && ball_detection_iteration != 0 && scd.hasTrayectoryChanged(previous_ball_center.y, ball_center.y))
                {
					//taking two frames back
					cv::Mat decision_frame;
					last_frames.front().copyTo(decision_frame);

					//result image will store lines and marked ball with the decision
					cv::Mat result_image;

					//detect ball on decision frame
					if(bd.detect_ball(decision_frame, ball_center, ball_radius))
					{
						//testing the line decision, uncomment for debugging
						cv::Point2i projected_ball_center = ball_center;
						projected_ball_center.y += ball_radius;
						cv::circle(decision_frame, projected_ball_center, 3, cv::Scalar(255,0,0), -1, 8, 0);
						decision_result = ld.getDecision(decision_frame, projected_ball_center, ball_radius);

						// result image
						decision_frame.copyTo(result_image);
						cv::line(result_image, lines[0], lines[1], cv::Scalar(0,0,255));
						cv::line(result_image, lines[1], lines[2], cv::Scalar(0,0,255));
						cv::line(result_image, lines[2], lines[3], cv::Scalar(0,0,255));
						cv::line(result_image, lines[3], lines[0], cv::Scalar(0,0,255));

						//drawing detected circles
						// circle center
						cv::circle(result_image, ball_center, 3, cv::Scalar(0,255,0), -1, 8, 0);
						// circle outline
						cv::circle(result_image, ball_center, ball_radius, cv::Scalar(0,0,255), 3, 8, 0);
					}
					else
					{
						std::cout << "Error : ball does not exist in decision frame" << std::endl;

						//if ball does not exist in the decision frame, then we use the current frame
						std::cout << "Using current frame" << std::endl;

						if(bd.detect_ball(image, ball_center, ball_radius))
						{
							//testing the line decision, uncomment for debugging
							cv::Point2i projected_ball_center = ball_center;
							projected_ball_center.y += ball_radius;
							cv::circle(image, projected_ball_center, 3, cv::Scalar(255,0,0), -1, 8, 0);
							decision_result = ld.getDecision(image, projected_ball_center, ball_radius);

							// result image
							image.copyTo(result_image);
							cv::line(result_image, lines[0], lines[1], cv::Scalar(0,0,255));
							cv::line(result_image, lines[1], lines[2], cv::Scalar(0,0,255));
							cv::line(result_image, lines[2], lines[3], cv::Scalar(0,0,255));
							cv::line(result_image, lines[3], lines[0], cv::Scalar(0,0,255));

							//drawing detected circles
							// circle center
							cv::circle(result_image, ball_center, 3, cv::Scalar(0,255,0), -1, 8, 0);
							// circle outline
							cv::circle(result_image, ball_center, ball_radius, cv::Scalar(0,0,255), 3, 8, 0);
						}
						else
						{
							std::cout << "Error : ball does not exist in current frame" << std::endl;
						}
					}

                    if(decision_result == -1)
                    {
                        uc.setDisplay("Decision: LEFT");
                        std::cout << "Left" << std::endl;
                        cv::putText(result_image, "LEFT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    else if(decision_result == 0)
                    {
                        if (live_video)
                        {
                            uc.setDisplay("Decision: LEFT");
                            cv::putText(result_image, "LEFT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                            std::cout << "Left" << std::endl;
                        }
                        else
                        {
                            uc.setDisplay("Decision: ON LINE");
                            cv::putText(result_image, "ON LINE", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                            std::cout << "On Line" << std::endl;
                        }
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    else
                    {
                        uc.setDisplay("Decision: RIGHT");
                        cv::putText(result_image, "RIGHT", cv::Point(5,result_image.rows - 20),
                                    CV_FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0,0,255),1,8,false);
                        std::cout << "Right" << std::endl;
                        if (!live_video)
                        {
                            made_decision = true;
                        }
                    }
                    cv::imshow("Result", result_image);
                }
                ball_detection_iteration++;
                previous_ball_center = ball_center;
            }

            cv::line(image, lines[0], lines[1], cv::Scalar(0,0,255));
            cv::line(image, lines[1], lines[2], cv::Scalar(0,0,255));
            cv::line(image, lines[2], lines[3], cv::Scalar(0,0,255));
            cv::line(image, lines[3], lines[0], cv::Scalar(0,0,255));
            cv::imshow("Ball detection", image);

            // Quit the loop when a key is pressed, also give a delay on the video
            int wait_time = 5;
            if (slow_motion)
            {
                wait_time = 500;
            }
            int keyPressed = cv::waitKey(wait_time);
            if (keyPressed != -1) break;
        }
        else
        {
            vidCap.set(CV_CAP_PROP_POS_AVI_RATIO , 0);
            std::cout << "Reached end of video, playing again" << std::endl;
            uc.setDisplay("Starting Electronic Line Judge...");
            made_decision = false;
            previous_ball_center.x = 0.0;
            previous_ball_center.y = 0.0;
            ball_detection_iteration = 0;
            cv::destroyWindow("Result");
        }
    }

    return 0;
}
<|endoftext|>


<commit_before>#pragma GCC diagnostic ignored "-Wswitch"

#define GLEW_STATIC
#define MICRO_SECONDS_PER_FRAME 16667

#include <GL/glew.h>
#include <SFML/Window.hpp>
#include <chrono>
#include <unistd.h>

std::chrono::steady_clock::time_point start;
std::chrono::steady_clock::time_point end;

//-----------------------------------------------------
// SFML setup
//-----------------------------------------------------
sf::Window window(sf::VideoMode(1280, 720), "OpenGL", sf::Style::Close);

bool running = true;

void process_input(){

    sf::Event windowEvent;

    while (window.pollEvent(windowEvent)) {
        switch (windowEvent.type) {
            case sf::Event::Closed:
                running = false;
                break;
        }
    }

}

void update(){

}

void render(){
    window.display();
}

int main() {

    //-----------------------------------------------------
    // Glew setup
    //-----------------------------------------------------
    glewExperimental = GL_TRUE;
    glewInit();

    //-----------------------------------------------------
    // Main game loop
    //-----------------------------------------------------
    while (running) {

        //Get the time at the beginning of the frame
        start = std::chrono::steady_clock::now();

        process_input();
        update();
        render();

        //Calculate how long the frame took to process
        end = std::chrono::steady_clock::now();
        long long microseconds = std::chrono::duration_cast<std::chrono::microseconds>(end-start).count();

        //Sleep if the processing took less than what is necessary to achieve 60 FPS
        if(( MICRO_SECONDS_PER_FRAME - microseconds ) > 0){
            usleep((MICRO_SECONDS_PER_FRAME - microseconds));
        }
    }

    return 0;
}
<commit_msg>More complete setup for SFML. Must test on desktop<commit_after>#pragma GCC diagnostic ignored "-Wswitch"

#define GLEW_STATIC
#define MICRO_SECONDS_PER_FRAME 16667

#include <GL/glew.h>
#include <SFML/Window.hpp>
#include <chrono>
#include <unistd.h>

//Global variables
std::chrono::steady_clock::time_point start;
std::chrono::steady_clock::time_point end;
sf::Window *window;
bool running = true;

void setup_SFML(){
    sf::ContextSettings settings;

    settings.antialiasingLevel = 4;
    settings.majorVersion = 3;
    settings.minorVersion = 3;

    window = new sf::Window(sf::VideoMode(1280, 720), "OpenGL", sf::Style::Close,settings);
}

void process_input(){

    sf::Event windowEvent;

    while (window->pollEvent(windowEvent)) {
        switch (windowEvent.type) {
            case sf::Event::Closed:
                running = false;
                break;
        }
    }

}

void update(){

}

void render(){
    window->display();
}

int main() {

    setup_SFML();

    //-----------------------------------------------------
    // Glew setup
    //-----------------------------------------------------
    glewExperimental = GL_TRUE;
    glewInit();

    //-----------------------------------------------------
    // Main game loop
    //-----------------------------------------------------
    while (running) {

        //Get the time at the beginning of the frame
        start = std::chrono::steady_clock::now();

        process_input();
        update();
        render();

        //Calculate how long the frame took to process
        end = std::chrono::steady_clock::now();
        long long microseconds = std::chrono::duration_cast<std::chrono::microseconds>(end-start).count();

        //Sleep if the processing took less than what is necessary to achieve 60 FPS
        if(( MICRO_SECONDS_PER_FRAME - microseconds ) > 0){
            usleep((MICRO_SECONDS_PER_FRAME - microseconds));
        }
    }

    delete window;

    return 0;
}
<|endoftext|>


<commit_before>#include "userinterface.hpp"

int main(int argc, char *argv[])
{
    std::vector<std::string> arg;
    for(int i = 0; i < argc; i++)
    {
        arg.push_back(argv[i]);
    }
    UserInterface ui(arg);
}
<commit_msg>add top level exception handling<commit_after>#include "userinterface.hpp"

int main(int argc, char * argv[])
{
    try
    {
        std::vector<std::string> arg;
        for (int i = 0; i < argc; i++)
        {
            arg.push_back(argv[i]);
        }
        UserInterface ui(arg);
    }
    catch (...)
    {
        std::cout << "Unknown error!" << std::endl;
    }
}
<|endoftext|>


<commit_before>#include <dmc/dmc.hpp>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <chrono>

template <class Scalar>
struct test_object : dmc::dual_object<Scalar, test_object<Scalar>>
{
public:
	typedef dmc::object<Scalar> base_type;

	using typename base_type::scalar_type;
	using typename base_type::vector_type;

	explicit test_object(scalar_type radius)
		: radius_(radius)
	{
	}

	template <class T>
	T templated_value(const Eigen::Matrix<T, 3, 1>& p) const
	{
		auto cube1 = 1.0 - p.cwiseAbs().maxCoeff();
		auto cube2 = 1.0 - (p - Eigen::Matrix<T, 3, 1>(0.5, 0.5, 0.5)).cwiseAbs().maxCoeff();

		return std::min(cube1, -cube2);
	}

private:
	scalar_type radius_;
};

int main(int /*argc*/, char* /*argv*/ [])
{
	dmc::tree_config<double> config;
	config.grid_width = 0.1;
	config.tolerance = 0.001;

	auto start_time = std::chrono::high_resolution_clock::now();

	dmc::tree<double> t({-3.0, -3.0, -3.0}, {3.0, 3.0, 3.0}, config);

	double last_progress = 0.0;

	t.generate(test_object<double>(1.5f), [&](double progress) {
		if (progress > last_progress + 0.01)
		{
			std::cout << std::fixed << std::setprecision(3) << progress << std::endl;
			last_progress = progress;
		}
	});

	auto generated_time = std::chrono::high_resolution_clock::now();

	auto triangles = t.enumerate();

	auto enumerated_time = std::chrono::high_resolution_clock::now();

	std::cout << "Generation: " << std::fixed << std::setprecision(2) << std::chrono::duration<double>(generated_time - start_time).count() << "s" << std::endl;
	std::cout << "Enumeration: " << std::fixed << std::setprecision(2) << std::chrono::duration<double>(enumerated_time - generated_time).count() << "s" << std::endl;

	std::ofstream os("a.stl", std::ios::binary);
	write_stl(os, triangles);
}
<commit_msg>Refactoring<commit_after>#include <dmc/dmc.hpp>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <chrono>

template <class Scalar>
struct test_object : dmc::dual_object<Scalar, test_object<Scalar>>
{
private:
	typedef dmc::object<Scalar> base_type;

public:
	using typename base_type::scalar_type;
	using typename base_type::vector_type;

	template <class T>
	T templated_value(const Eigen::Matrix<T, 3, 1>& p) const
	{
		auto cube1 = 1.0 - p.cwiseAbs().maxCoeff();
		auto cube2 = 1.0 - (p - Eigen::Matrix<T, 3, 1>(0.5, 0.5, 0.5)).cwiseAbs().maxCoeff();

		return std::min(cube1, -cube2);
	}
};

int main(int /*argc*/, char* /*argv*/ [])
{
	dmc::tree_config<double> config;
	config.grid_width = 0.1;
	config.tolerance = 0.001;

	auto start_time = std::chrono::high_resolution_clock::now();

	dmc::tree<double> t({-3.0, -3.0, -3.0}, {3.0, 3.0, 3.0}, config);

	double last_progress = 0.0;

	test_object<double> obj;

	t.generate(obj, [&](double progress) {
		if (progress > last_progress + 0.01)
		{
			std::cout << std::fixed << std::setprecision(3) << progress << std::endl;
			last_progress = progress;
		}
	});

	auto generated_time = std::chrono::high_resolution_clock::now();

	auto triangles = t.enumerate();

	auto enumerated_time = std::chrono::high_resolution_clock::now();

	std::cout << "Generation: " << std::fixed << std::setprecision(2) << std::chrono::duration<double>(generated_time - start_time).count() << "s" << std::endl;
	std::cout << "Enumeration: " << std::fixed << std::setprecision(2) << std::chrono::duration<double>(enumerated_time - generated_time).count() << "s" << std::endl;

	std::ofstream os("a.stl", std::ios::binary);
	write_stl(os, triangles);
}
<|endoftext|>


<commit_before>#include "bcm2835/rpiemulator.h"

#include <getopt.h>

/**
 * Prints the command line options
 */
static void
cmdline_usage(int argc, char **argv)
{
  printf("%s [options] image\n", argc > 0 ? argv[0] : "emulate");
  printf("Options:\n");
  printf("  --quiet         Does not dump CPU state\n");
  printf("  --graphics      Emulate framebuffer\n");
  printf("  --memory=size   Specify memory size in bytes\n");
  printf("  --addr=addr     Specify kernel start address\n");
  printf("  --help          Print this message\n");
}

/**
 * Parses command line arguments using getopt
 * @param opt   Reference to the options struct
 * @param argc  Number of command line arguments
 * @param argv  Argument values
 * @param usage Pointer to usage value
 * @return Nonzero if arguments are valid
 */
static int
cmdline_parse(remu::EmulatorOptions &opt, int argc, char **argv, int *usage)
{
  struct option options[] =
  {
    { "help",      no_argument,        usage,             1 },
    { "quiet",     no_argument,        &opt.quiet,        1 },
    { "nes",       no_argument,        &opt.nes_enabled,  1 },
    { "memory",    required_argument, 0,                 'm' },
    { "addr",      required_argument, 0,                 'a' },
    { "gpio-test", required_argument, 0,                 'i' },
    { 0, 0, 0, 0 }
  };

  int c, index;

  /* Default args */
  opt.mem_size = 256 * 1024 * 1024;
  opt.start_addr = 0x10000;

  /* Read all arguments */
  while ((c = getopt_long(argc, argv, "vghsm:a:", options, &index)) != -1)
  {
    switch (c)
    {
      case 'q': opt.quiet = 1; break;
      case 'h': *usage = 1;    break;
      case 'm':
      {
        /* Handle prefixes */
        int length = strlen(optarg);
        switch (optarg[length - 1])
        {
          case 'm': case 'M':
          {
            optarg[length - 1] = '\0';
            sscanf(optarg, "%zu", &opt.mem_size);
            opt.mem_size *= 1024 * 1024;
            break;
          }
          case 'k': case 'K':
          {
            optarg[length - 1] = '\0';
            sscanf(optarg, "%zu", &opt.mem_size);
            opt.mem_size *= 1024;
            break;
          }
          default:
          {
            sscanf(optarg, "%zu", &opt.mem_size);
          }
        }
        break;
      }
      case 'a':
      {
        sscanf(optarg, "%u", &opt.start_addr);
        break;
      }
      case 'i':
      {
        sscanf(optarg, "%u", &opt.gpio_test_offset);
        break;
      }
      case 0:
      {
        /* Flag set */
        break;
      }
      case '?':
      {
        /* Error */
        return 0;
      }
    }
  }

  /* Read image source */
  opt.image = optind >= argc ? NULL : argv[optind];
  return 1;
}

/**
 * Checks the validity of the arguments
 * @param opt  Reference to the options struct
 * @param argc Number of command line arguments
 * @param argv Argument values
 * @return Nonzero if arguments are valid
 */
static int
cmdline_check(remu::EmulatorOptions &opt)
{
  /* Image source */
  if (!opt.image)
  {
    fprintf(stderr, "No kernel image specified.\n");
    return 0;
  }

  /* Memory size at least 64kb */
  if (opt.mem_size < 0x10000)
  {
    fprintf(stderr, "Must specify a minimum of 64kb of memory.\n");
    return 0;
  }

  return 1;
}

/**
 * Entry point of the application
 * @param argc Number of command line arguments
 * @param argv Argument values
 * @return EXIT_SUCCESS if everything goes fine
 */
int
main(int argc, char **argv)
{
  /* Parse command line arguments */
  remu::EmulatorOptions opt;
  int usage = 0;
  if (!cmdline_parse(opt, argc, argv, &usage))
  {
    return EXIT_FAILURE;
  }

  if (usage)
  {
    cmdline_usage(argc, argv);
    return EXIT_SUCCESS;
  }

  if (!cmdline_check(opt))
  {
    return EXIT_FAILURE;
  }

  try {
    remu::bcm2835::RPiEmulator emu(opt);

    /* Run the emulator */
    emu.load();
    emu.execute();
  } catch(const std::exception &exc) {
    fprintf(stderr, "Fatal Error:  %s\n", exc.what());

    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

<commit_msg>Convert cmdline_check to return bool<commit_after>#include "bcm2835/rpiemulator.h"

#include <getopt.h>

/**
 * Prints the command line options
 */
static void
cmdline_usage(int argc, char **argv)
{
  printf("%s [options] image\n", argc > 0 ? argv[0] : "emulate");
  printf("Options:\n");
  printf("  --quiet         Does not dump CPU state\n");
  printf("  --graphics      Emulate framebuffer\n");
  printf("  --memory=size   Specify memory size in bytes\n");
  printf("  --addr=addr     Specify kernel start address\n");
  printf("  --help          Print this message\n");
}

/**
 * Parses command line arguments using getopt
 * @param opt   Reference to the options struct
 * @param argc  Number of command line arguments
 * @param argv  Argument values
 * @param usage Pointer to usage value
 * @return Nonzero if arguments are valid
 */
static int
cmdline_parse(remu::EmulatorOptions &opt, int argc, char **argv, int *usage)
{
  struct option options[] =
  {
    { "help",      no_argument,        usage,             1 },
    { "quiet",     no_argument,        &opt.quiet,        1 },
    { "nes",       no_argument,        &opt.nes_enabled,  1 },
    { "memory",    required_argument, 0,                 'm' },
    { "addr",      required_argument, 0,                 'a' },
    { "gpio-test", required_argument, 0,                 'i' },
    { 0, 0, 0, 0 }
  };

  int c, index;

  /* Default args */
  opt.mem_size = 256 * 1024 * 1024;
  opt.start_addr = 0x10000;

  /* Read all arguments */
  while ((c = getopt_long(argc, argv, "vghsm:a:", options, &index)) != -1)
  {
    switch (c)
    {
      case 'q': opt.quiet = 1; break;
      case 'h': *usage = 1;    break;
      case 'm':
      {
        /* Handle prefixes */
        int length = strlen(optarg);
        switch (optarg[length - 1])
        {
          case 'm': case 'M':
          {
            optarg[length - 1] = '\0';
            sscanf(optarg, "%zu", &opt.mem_size);
            opt.mem_size *= 1024 * 1024;
            break;
          }
          case 'k': case 'K':
          {
            optarg[length - 1] = '\0';
            sscanf(optarg, "%zu", &opt.mem_size);
            opt.mem_size *= 1024;
            break;
          }
          default:
          {
            sscanf(optarg, "%zu", &opt.mem_size);
          }
        }
        break;
      }
      case 'a':
      {
        sscanf(optarg, "%u", &opt.start_addr);
        break;
      }
      case 'i':
      {
        sscanf(optarg, "%u", &opt.gpio_test_offset);
        break;
      }
      case 0:
      {
        /* Flag set */
        break;
      }
      case '?':
      {
        /* Error */
        return 0;
      }
    }
  }

  /* Read image source */
  opt.image = optind >= argc ? NULL : argv[optind];
  return 1;
}

/**
 * Checks the validity of the arguments
 * @param opt  Reference to the options struct
 * @param argc Number of command line arguments
 * @param argv Argument values
 * @return Nonzero if arguments are valid
 */
static bool
cmdline_check(remu::EmulatorOptions &opt)
{
  /* Image source */
  if (!opt.image)
  {
    fprintf(stderr, "No kernel image specified.\n");
    return false;
  }

  /* Memory size at least 64kb */
  if (opt.mem_size < 0x10000)
  {
    fprintf(stderr, "Must specify a minimum of 64kb of memory.\n");
    return false;
  }

  return true;
}

/**
 * Entry point of the application
 * @param argc Number of command line arguments
 * @param argv Argument values
 * @return EXIT_SUCCESS if everything goes fine
 */
int
main(int argc, char **argv)
{
  /* Parse command line arguments */
  remu::EmulatorOptions opt;
  int usage = 0;
  if (!cmdline_parse(opt, argc, argv, &usage))
  {
    return EXIT_FAILURE;
  }

  if (usage)
  {
    cmdline_usage(argc, argv);
    return EXIT_SUCCESS;
  }

  if (!cmdline_check(opt))
  {
    return EXIT_FAILURE;
  }

  try {
    remu::bcm2835::RPiEmulator emu(opt);

    /* Run the emulator */
    emu.load();
    emu.execute();
  } catch(const std::exception &exc) {
    fprintf(stderr, "Fatal Error:  %s\n", exc.what());

    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

<|endoftext|>