<commit_before>/*
 * rational.cpp
 */
#include "rational.h"

namespace c8 {
    rational::rational(double v) {
        /*
         * Split our double into a significand and an exponent.
         */
        uint64_t *vbits_ptr = reinterpret_cast<uint64_t *>(&v);
        uint64_t vbits = *vbits_ptr;
        bool neg = (vbits & 0x8000000000000000ULL) ? true : false;
        vbits &= 0x7fffffffffffffffULL;
        int64_t exp = static_cast<int64_t>(vbits >> 52) - 1023;
        int64_t sig = static_cast<int64_t>(vbits & 0x000fffffffffffffULL);

        /*
         * Is this an infinity or a NaN?
         */
        if (exp == 1024) {
            throw c8::not_a_number();
        }

        /*
         * Do we have a normalized value (as opposed to a denormalized one)?  Most
         * floating point values are normalized.
         */
        if (exp != -1022) {
            /*
             * For normal numbers we have an implied 52nd bit set.  Set that
             * explicitly now!  When we do that though we're also shifting our
             * exponent by 52 places too.
             */
            sig |= 0x0010000000000000ULL;
            exp -= 52;
        }

        if (neg) {
            sig = -sig;
        }

        integer i = integer(sig);

        if (exp < 0) {
            num_ = i;
            denom_ = natural(1) << static_cast<unsigned int>(-exp);
        } else {
            num_ = i << static_cast<unsigned int>(exp);
            denom_ = natural(1);
        }

        normalize();
    }

    rational::rational(const std::string &v) {
        /*
         * Do we have a '/' character separating a numerator and denominator?
         */
        std::size_t pos = v.find(std::string("/"));
        if (pos == std::string::npos) {
            num_ = integer(v);
        } else {
            num_ = integer(v.substr(0, pos));
            denom_ = natural(v.substr(pos + 1));
        }

        normalize();
    }

    /*
     * Add another rational to this one.
     */
    auto rational::add(const rational &v) const -> rational {
        rational res;

        res.num_ = (num_ * integer(v.denom_)) + (integer(denom_) * v.num_);
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Subtract another rational from this one.
     *
     * Note that there is a subtle difference between the rational subtract operation
     * and the natural number version: We don't have to worry about throwing
     * exceptions for negative results.
     */
    auto rational::subtract(const rational &v) const -> rational {
        rational res;

        res.num_ = (num_ * integer(v.denom_)) - (integer(denom_) * v.num_);
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Multiply another rational with this one.
     */
    auto rational::multiply(const rational &v) const -> rational {
        rational res;

        res.num_ = num_ * v.num_;
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Divide this rational by another one.
     */
    auto rational::divide(const rational &v) const -> rational {
        rational res;

        /*
         * Are we attempting to divide by zero?  If we are then throw an exception.
         */
        if (is_zero(abs(v.num_))) {
            throw c8::divide_by_zero();
        }

        res.num_ = num_ * integer(v.denom_);
        integer d = integer(denom_) * v.num_;
        if (isnegative(d)) {
            res.num_ = -res.num_;
        }

        res.denom_ = abs(d);

        res.normalize();

        return res;
    }

    /*
     * Compare a rational with this one.
     */
    auto rational::compare(const rational &v) const -> rational_comparison {
        integer lhs = num_ * integer(v.denom_);
        integer rhs = v.num_ * integer(denom_);

        auto ures = lhs.compare(rhs);
        switch (ures) {
        case integer_comparison::lt:
            return rational_comparison::lt;

        case integer_comparison::eq:
            return rational_comparison::eq;

        case integer_comparison::gt:
            return rational_comparison::gt;
        }
    }

    /*
     * Normalize the data.
     */
    auto rational::normalize() -> void {
        /*
         * Find the GCD of the numerator and denominator.
         */
        natural g = gcd(abs(num_), denom_);
        num_ /= g;
        denom_ /= g;
    }

    /*
     * Convert this rational to a double.
     */
    auto rational::to_double() const -> double {
        /*
         * Is our value zero?  If it is then handle this as a special case.
         */
        natural n = abs(num_);
        if (is_zero(n)) {
            return 0.0;
        }

        /*
         * We need to get a dividend that is sufficiently large that when it's
         * divided by our denominator that we get 52 bits of result.  We don't
         * want our division operation to be too expensive either, so one of the
         * other things we can do is scale our denominator down.
         */
        natural d = denom_;
        int eshift = 0;
        unsigned int dbits = d.count_bits();
        if (dbits > 52) {
            unsigned int s = dbits - 52;
            eshift -= static_cast<int>(s);
            dbits = 52;
            d >>= s;
        }

        unsigned int nbits = n.count_bits();
        if (nbits > dbits + 53) {
            unsigned int s = nbits - (dbits + 53);
            n >>= s;
            eshift += static_cast<int>(s);
        } else {
            unsigned int s = dbits + 53 - nbits;
            n <<= s;
            eshift -= static_cast<int>(s);
        }

        /*
         * Compute the signifcand.
         */
        natural sig = n / d;

        /*
         * Convert the significand to an uin64_t.  It will either have 53 or 54
         * bits.  If it's 54 bits then shift right by one.  Once we have 53 bits
         * we can strip bit 52.
         */
        uint64_t res = static_cast<uint64_t>(to_unsigned_long_long(sig));
        while (res > 0x0020000000000000ULL) {
            res >>= 1;
            eshift++;
        }

        res &= 0x000fffffffffffffULL;

        eshift += 52;

        /*
         * Will this number fit in a double?  If not then throw an exception.
         */
        if (eshift > 1024) {
            throw overflow_error();
        }

        if (eshift < -1022) {
            throw underflow_error();
        }

        res |= (static_cast<uint64_t>(eshift + 1023) << 52);

        /*
         * Is our numerator negative?
         */
        if (isnegative(num_)) {
            res |= 0x8000000000000000ULL;
        }

        double *dbits_ptr = reinterpret_cast<double *>(&res);
        return *dbits_ptr;
    }

    /*
     * << operator to print a rational.
     */
    auto operator<<(std::ostream &outstr, const rational &v) -> std::ostream & {
        outstr << v.num_ << '/' << v.denom_;

        return outstr;
    }
}

<commit_msg>Improve comments<commit_after>/*
 * rational.cpp
 */
#include "rational.h"

namespace c8 {
    /*
     * Construct a rational from an IEEE754 double precision floating point value.
     */
    rational::rational(double v) {
        /*
         * Convert our double into a bit representation.
         *
         * Note: As currently written the code only works for a little-endian CPU.
         */
        uint64_t *vbits_ptr = reinterpret_cast<uint64_t *>(&v);
        uint64_t vbits = *vbits_ptr;

        /*
         * With a double precisions IEEE754 floating point value we have:
         *
         * bits 0-51:   Significand
         * bits 52-62:  Exponent
         * bit 63:      Sign
         */
        bool neg = (vbits & 0x8000000000000000ULL) ? true : false;
        vbits &= 0x7fffffffffffffffULL;
        int64_t exp = static_cast<int64_t>(vbits >> 52) - 1023;
        int64_t sig = static_cast<int64_t>(vbits & 0x000fffffffffffffULL);

        /*
         * Is this an infinity or a NaN?
         */
        if (exp == 1024) {
            throw c8::not_a_number();
        }

        /*
         * Do we have a normalized value (as opposed to a denormalized one)?  Most
         * floating point values are normalized.
         */
        if (exp != -1022) {
            /*
             * For normal numbers we have an implied 52nd bit set.  Set that
             * explicitly now!  When we do that though we're also shifting our
             * exponent by 52 places too.
             */
            sig |= 0x0010000000000000ULL;
            exp -= 52;
        }

        if (neg) {
            sig = -sig;
        }

        /*
         * Create our numerator and denominator.  If our exponent is negative then
         * we scale our denominator up, but if the exponent is positive then we
         * scan our numerator instead.
         */
        integer i = integer(sig);

        if (exp < 0) {
            num_ = i;
            denom_ = natural(1) << static_cast<unsigned int>(-exp);
        } else {
            num_ = i << static_cast<unsigned int>(exp);
            denom_ = natural(1);
        }

        normalize();
    }

    /*
     * Construct a rational using a string.
     *
     * The string can have an optional '-' sign to indicate that it's negative,
     * and then the usual C++-like hex, octal, * or decimal representations.
     * The numerator and denominator are separated by a '/', and there are no
     * optional spaces.
     */
    rational::rational(const std::string &v) {
        /*
         * Do we have a '/' character separating a numerator and denominator?
         */
        std::size_t pos = v.find(std::string("/"));
        if (pos == std::string::npos) {
            num_ = integer(v);
        } else {
            num_ = integer(v.substr(0, pos));
            denom_ = natural(v.substr(pos + 1));
        }

        normalize();
    }

    /*
     * Add another rational to this one.
     */
    auto rational::add(const rational &v) const -> rational {
        rational res;

        res.num_ = (num_ * integer(v.denom_)) + (integer(denom_) * v.num_);
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Subtract another rational from this one.
     */
    auto rational::subtract(const rational &v) const -> rational {
        rational res;

        res.num_ = (num_ * integer(v.denom_)) - (integer(denom_) * v.num_);
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Multiply another rational with this one.
     */
    auto rational::multiply(const rational &v) const -> rational {
        rational res;

        res.num_ = num_ * v.num_;
        res.denom_ = denom_ * v.denom_;

        res.normalize();

        return res;
    }

    /*
     * Divide this rational by another one.
     */
    auto rational::divide(const rational &v) const -> rational {
        rational res;

        /*
         * Are we attempting to divide by zero?  If we are then throw an exception.
         */
        if (is_zero(abs(v.num_))) {
            throw c8::divide_by_zero();
        }

        res.num_ = num_ * integer(v.denom_);
        integer d = integer(denom_) * v.num_;
        if (isnegative(d)) {
            res.num_ = -res.num_;
        }

        res.denom_ = abs(d);

        res.normalize();

        return res;
    }

    /*
     * Compare a rational with this one.
     */
    auto rational::compare(const rational &v) const -> rational_comparison {
        integer lhs = num_ * integer(v.denom_);
        integer rhs = v.num_ * integer(denom_);

        auto ures = lhs.compare(rhs);
        switch (ures) {
        case integer_comparison::lt:
            return rational_comparison::lt;

        case integer_comparison::eq:
            return rational_comparison::eq;

        case integer_comparison::gt:
            return rational_comparison::gt;
        }
    }

    /*
     * Normalize the data.
     */
    auto rational::normalize() -> void {
        /*
         * Find the GCD of the numerator and denominator.
         */
        natural g = gcd(abs(num_), denom_);
        num_ /= g;
        denom_ /= g;
    }

    /*
     * Convert this rational to a double.
     */
    auto rational::to_double() const -> double {
        /*
         * Is our value zero?  If it is then handle this as a special case.
         */
        natural n = abs(num_);
        if (is_zero(n)) {
            return 0.0;
        }

        /*
         * We need to get a dividend that is sufficiently large that when it's
         * divided by our denominator that we get 52 bits of result.  We don't
         * want our division operation to be too expensive either, so one of the
         * other things we can do is scale our denominator down.
         */
        natural d = denom_;
        int eshift = 0;
        unsigned int dbits = d.count_bits();
        if (dbits > 52) {
            unsigned int s = dbits - 52;
            eshift -= static_cast<int>(s);
            dbits = 52;
            d >>= s;
        }

        unsigned int nbits = n.count_bits();
        if (nbits > dbits + 53) {
            unsigned int s = nbits - (dbits + 53);
            n >>= s;
            eshift += static_cast<int>(s);
        } else {
            unsigned int s = dbits + 53 - nbits;
            n <<= s;
            eshift -= static_cast<int>(s);
        }

        /*
         * Compute the signifcand.
         */
        natural sig = n / d;

        /*
         * Convert the significand to an uin64_t.  It will either have 53 or 54
         * bits.  If it's 54 bits then shift right by one.  Once we have 53 bits
         * we can strip bit 52.
         */
        uint64_t res = static_cast<uint64_t>(to_unsigned_long_long(sig));
        while (res > 0x0020000000000000ULL) {
            res >>= 1;
            eshift++;
        }

        res &= 0x000fffffffffffffULL;

        eshift += 52;

        /*
         * Will this number fit in a double?  If not then throw an exception.
         */
        if (eshift > 1024) {
            throw overflow_error();
        }

        if (eshift < -1022) {
            throw underflow_error();
        }

        res |= (static_cast<uint64_t>(eshift + 1023) << 52);

        /*
         * Is our numerator negative?
         */
        if (isnegative(num_)) {
            res |= 0x8000000000000000ULL;
        }

        /*
         * Convert our result bitmap into a double representation.
         *
         * Note: As currently written the code only works for a little-endian CPU.
         */
        double *dbits_ptr = reinterpret_cast<double *>(&res);
        return *dbits_ptr;
    }

    /*
     * << operator to print a rational.
     */
    auto operator<<(std::ostream &outstr, const rational &v) -> std::ostream & {
        outstr << v.num_ << '/' << v.denom_;

        return outstr;
    }
}

<|endoftext|>


<commit_before>
#include <planet/common.hpp>
#include <planet/utils.hpp>
#include <planet/fs_core.hpp>
#include <fuse.h>

namespace planet {

    shared_ptr<file_entry> file_cast(shared_ptr<fs_entry> entry)
    {
        if (!entry || entry->type() != file_type::regular_file)
            throw std::runtime_error(get_errmsg(detail::pe_file_cast));
        return std::static_pointer_cast<file_entry>(entry);
    }

    shared_ptr<dentry> directory_cast(shared_ptr<fs_entry> entry)
    {
        if (!entry || entry->type() != file_type::directory)
            throw std::runtime_error(get_errmsg(detail::pe_dir_cast));
        return std::static_pointer_cast<dentry>(entry);
    }

    shared_ptr<file_entry> search_file_entry(core_file_system const& root, path_type const& path)
    {
        auto entry = root.get_entry_of(path);
        return (entry && entry->type() == file_type::regular_file ?
            file_cast(entry) : detail::shared_null_ptr);
    }

    shared_ptr<dentry> search_dir_entry(core_file_system const& root, path_type const& path)
    {
        auto entry = root.get_entry_of(path);
        return (entry && entry->type() == file_type::directory ?
            directory_cast(entry) : detail::shared_null_ptr);
    }

    void fill_st_inode(st_inode& inode)
    {
        inode.uid = ::fuse_get_context()->uid;
        inode.gid = ::fuse_get_context()->gid;
    }

    //
    // raii_wrapper
    //
    raii_wrapper::raii_wrapper(raii_wrapper&& r)
    {
        finalizer_ = std::move(r.finalizer_);
    }

    raii_wrapper& raii_wrapper::operator=(raii_wrapper&& r)
    {
        finalizer_ = std::move(r.finalizer_);
        return *this;
    }

    raii_wrapper::~raii_wrapper()
    {
        try {
            finalize();
        } catch (...) {
            // dtor must not throw any exceptions
        }
    }

    raii_wrapper::functor_t const& raii_wrapper::get_finalizer() const
    {
        return finalizer_;
    }

    void raii_wrapper::finalize() const
    {
        if (finalizer_)
            finalizer_();
    }

}   // namespace planet
<commit_msg>fill_st_inode: temporary way to fix a weird bug<commit_after>
#include <planet/common.hpp>
#include <planet/utils.hpp>
#include <planet/fs_core.hpp>
#include <fuse.h>

namespace planet {

    shared_ptr<file_entry> file_cast(shared_ptr<fs_entry> entry)
    {
        if (!entry || entry->type() != file_type::regular_file)
            throw std::runtime_error(get_errmsg(detail::pe_file_cast));
        return std::static_pointer_cast<file_entry>(entry);
    }

    shared_ptr<dentry> directory_cast(shared_ptr<fs_entry> entry)
    {
        if (!entry || entry->type() != file_type::directory)
            throw std::runtime_error(get_errmsg(detail::pe_dir_cast));
        return std::static_pointer_cast<dentry>(entry);
    }

    shared_ptr<file_entry> search_file_entry(core_file_system const& root, path_type const& path)
    {
        auto entry = root.get_entry_of(path);
        return (entry && entry->type() == file_type::regular_file ?
            file_cast(entry) : detail::shared_null_ptr);
    }

    shared_ptr<dentry> search_dir_entry(core_file_system const& root, path_type const& path)
    {
        auto entry = root.get_entry_of(path);
        return (entry && entry->type() == file_type::directory ?
            directory_cast(entry) : detail::shared_null_ptr);
    }

    void fill_st_inode(st_inode& inode)
    {
        //inode.uid = ::fuse_get_context()->uid;
        //inode.gid = ::fuse_get_context()->gid;
        inode.uid = ::getuid();
        inode.gid = ::getgid();
    }

    //
    // raii_wrapper
    //
    raii_wrapper::raii_wrapper(raii_wrapper&& r)
    {
        finalizer_ = std::move(r.finalizer_);
    }

    raii_wrapper& raii_wrapper::operator=(raii_wrapper&& r)
    {
        finalizer_ = std::move(r.finalizer_);
        return *this;
    }

    raii_wrapper::~raii_wrapper()
    {
        try {
            finalize();
        } catch (...) {
            // dtor must not throw any exceptions
        }
    }

    raii_wrapper::functor_t const& raii_wrapper::get_finalizer() const
    {
        return finalizer_;
    }

    void raii_wrapper::finalize() const
    {
        if (finalizer_)
            finalizer_();
    }

}   // namespace planet
<|endoftext|>


<commit_before>//----------------------------------------------------------------------------
// registry.cpp
//
// registry manipulation stuff
//
// Copyright (C) 2011 Neil Butterworth
//----------------------------------------------------------------------------

#include <iostream>
#include <algorithm>
#include "registry.h"
#include "error.h"
using std::string;

RegPath :: RegPath( HKEY root ) : mRoot( root ), mPathKey( 0 ) {
	long res = 0;
	if ( mRoot == HKEY_CURRENT_USER ) {
		res = RegOpenKeyEx( mRoot, "Environment", 0, KEY_ALL_ACCESS, & mPathKey);
	}
	else {
		throw Error( "not yet implemented ") ;
	}
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not get registry key - " + LastWinError() );
	}

	const int BUFSIZE = 2048;
	BYTE buffer[ BUFSIZE + 1 ];
	DWORD bufflen = BUFSIZE;
	DWORD type = 0;
	res = RegQueryValueEx( mPathKey, "PATH", NULL, & type, buffer, & bufflen   );
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not get registry value - " + LastWinError() );
	}
	buffer[ bufflen ] = 0;
	SplitPath( (const char *) buffer );
	std::cout << "[" << buffer << "]" << std::endl;
}

RegPath :: ~RegPath() {
	if ( mPathKey ) {
		RegCloseKey( mPathKey );
	}
}

unsigned int RegPath :: Count() const {
	return mPath.size();
}

string RegPath :: At( unsigned int  i) const {
	return mPath.at( i );
}

void RegPath :: SplitPath( const std::string & path ) {
	string::size_type pos = 0;

	while( pos != string::npos ) {
		string::size_type fpos = path.find( ";", pos );
		string s;
		if ( fpos == string::npos ) {
			s = path.substr( pos );
			pos = string::npos;
		}
		else {
			s = path.substr( pos, fpos - pos );
			pos = fpos + 1;
		}
		if ( s.find_first_not_of( " \t" ) != string::npos ) {
			mPath.push_back( s );
		}
	}
}

bool RegPath :: Find( const string & apath ) const {

	return std::find( mPath.begin(), mPath.end(), apath ) != mPath.end();
}

bool RegPath :: Add( const string & apath ) {
	mPath.push_back( apath );
	UpdateReg();
	return true;
}

void RegPath :: UpdateReg() {
	string newpath;
	for ( unsigned int i = 0; i < mPath.size(); i++ ) {
		newpath += mPath[i] + ";";
	}

	long res = RegSetValueEx( mPathKey, "PATH", 0, REG_EXPAND_SZ,
								(BYTE *)newpath.c_str(), newpath.size() + 1 );
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not add update path in registry - " + LastWinError() );
	}
}

bool RegPath :: Remove( const string & apath ) {
	VecType::iterator it = std::find( mPath.begin(), mPath.end(), apath );
	if ( it != mPath.end() ) {
		mPath.erase( it );
	}
	UpdateReg();
	return true;
}

<commit_msg>commants and minor name changes<commit_after>//----------------------------------------------------------------------------
// registry.cpp
//
// registry manipulation stuff
//
// Copyright (C) 2011 Neil Butterworth
//----------------------------------------------------------------------------

#include <iostream>
#include <algorithm>
#include "registry.h"
#include "error.h"
using std::string;

//----------------------------------------------------------------------------
// Create a RegPath object from a registry key, which must be either
// HKEY_CURRENT_USER or HKEY_LOCAL_MACHINE.
//----------------------------------------------------------------------------

RegPath :: RegPath( HKEY root ) : mRoot( root ), mPathKey( 0 ) {
	long res = 0;
	if ( mRoot == HKEY_CURRENT_USER ) {
		res = RegOpenKeyEx( mRoot, "Environment", 0, KEY_ALL_ACCESS, & mPathKey);
	}
	else {
		throw Error( "not yet implemented ") ;
	}
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not get registry key - " + LastWinError() );
	}

	const int BUFSIZE = 2048;
	BYTE buffer[ BUFSIZE + 1 ];
	DWORD bufflen = BUFSIZE;
	DWORD type = 0;
	res = RegQueryValueEx( mPathKey, "PATH", NULL, & type, buffer, & bufflen   );
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not get registry value - " + LastWinError() );
	}
	buffer[ bufflen ] = 0;
	SplitPath( (const char *) buffer );
	std::cout << "[" << buffer << "]" << std::endl;
}

//----------------------------------------------------------------------------
// Close the path key opened in ctor.
//----------------------------------------------------------------------------

RegPath :: ~RegPath() {
	if ( mPathKey ) {
		RegCloseKey( mPathKey );
	}
}

//----------------------------------------------------------------------------
// How many directories on path?
//----------------------------------------------------------------------------

unsigned int RegPath :: Count() const {
	return mPath.size();
}

//----------------------------------------------------------------------------
// Get zero-based directory
//----------------------------------------------------------------------------

string RegPath :: At( unsigned int  i) const {
	return mPath.at( i );
}

//----------------------------------------------------------------------------
// Helper to split path at ';' character
//----------------------------------------------------------------------------

void RegPath :: SplitPath( const std::string & path ) {
	string::size_type pos = 0;

	while( pos != string::npos ) {
		string::size_type fpos = path.find( ";", pos );
		string s;
		if ( fpos == string::npos ) {
			s = path.substr( pos );
			pos = string::npos;
		}
		else {
			s = path.substr( pos, fpos - pos );
			pos = fpos + 1;
		}
		if ( s.find_first_not_of( " \t" ) != string::npos ) {
			mPath.push_back( s );
		}
	}
}

//----------------------------------------------------------------------------
// See if path contains adir
//----------------------------------------------------------------------------

bool RegPath :: Find( const string & adir ) const {

	return std::find( mPath.begin(), mPath.end(), adir ) != mPath.end();
}

//----------------------------------------------------------------------------
// Add directory to path - no check for multiple adds of same directory
//----------------------------------------------------------------------------

bool RegPath :: Add( const string & adir ) {
	mPath.push_back( adir );
	UpdateReg();
	return true;
}

//----------------------------------------------------------------------------
// Helper to update registry with current path.
//----------------------------------------------------------------------------

void RegPath :: UpdateReg() {
	string newpath;
	for ( unsigned int i = 0; i < mPath.size(); i++ ) {
		newpath += mPath[i] + ";";
	}

	long res = RegSetValueEx( mPathKey, "PATH", 0, REG_EXPAND_SZ,
								(BYTE *)newpath.c_str(), newpath.size() + 1 );
	if ( res != ERROR_SUCCESS ) {
		throw Error( "Could not add update path in registry - " + LastWinError() );
	}
}

//----------------------------------------------------------------------------
// Remove single instance of adir from path, updating registry.
//----------------------------------------------------------------------------

bool RegPath :: Remove( const string & adir ) {
	VecType::iterator it = std::find( mPath.begin(), mPath.end(), adir );
	if ( it != mPath.end() ) {
		mPath.erase( it );
	}
	UpdateReg();
	return true;
}

<|endoftext|>


<commit_before>#include <algorithm>
#include <iostream>
#include <cassert>
#include "reinhard.h"

using std::transform;

/*
 * Compute the luminance vector Lw, as well as the mean log luminance, as
 * specified in Eq. 1.
 */
void reinhard_tonemap_aux(const vector<spectrum>& input, int n,
                          vector<scalar>& luminances, scalar& mean_log_luminance)
{
  scalar delta = 0.001;
  luminances.resize(n);

  transform(input.begin(), input.end(), luminances.begin(),
            [](const spectrum& s) { return s.luminance(); });

  scalar total_log = accumulate(luminances.begin(), luminances.end(), scalar(0),
                                [=](scalar s, scalar v) { return s + log(delta + v); });

  mean_log_luminance = exp(total_log / n);
}

void ReinhardGlobal::tonemap(const vector<spectrum>& input, vector<spectrum>& output,
                             uint w, uint h) const
{
  vector<scalar> luminances(w*h);
  scalar mean_log_luminance;
  reinhard_tonemap_aux(input, w*h, luminances, mean_log_luminance);

  scalar l_mult = key_value / mean_log_luminance;
  scalar max_luminance = *max_element(luminances.begin(), luminances.end());
  scalar ml2 = max_luminance * max_luminance;

  // Instead of compute Eq. 2 and Eq. 4 separately, we merge them into the same
  // transform calcuation.
  transform(luminances.begin(), luminances.end(), luminances.begin(),
            [=](scalar s) { return s * (l_mult + s / ml2) / (1 + l_mult * s); });

  transform(input.begin(), input.end(), luminances.begin(),
            output.begin(),
            [](const spectrum& sp, scalar s) { return sp.rescale_luminance(s).clamp(); });
}

ReinhardLocal::ReinhardLocal(const Options& options) : opt(options)
{
}

ReinhardLocal::Options::Options()
  : key_value(0.18), scale_selection_threshhold(0.05), sharpening_factor(8),
    scale_pixel_factor(sqrt(0.125)), cs_ratio(1.6), num_scales(8)
{
}

void ReinhardLocal::tonemap(const vector<spectrum>& input, vector<spectrum>& output,
                            uint w, uint h) const
{
  vector<scalar> luminances(w*h);
  scalar mean_log_luminance;
  reinhard_tonemap_aux(input, w*h, luminances, mean_log_luminance);
  scalar l_mult = opt.key_value / mean_log_luminance;

  // Explicitly compute L(x, y) as specified in equation 2.
  transform(luminances.begin(), luminances.end(), luminances.begin(),
            [=](scalar L) { return L * l_mult; });

  auto center_surround = center_surround_functions(w, h, luminances, opt);

  vector<scalar> display_lum(input.size());
  for (auto i = 0u; i < luminances.size(); ++i)
  {
    int scale = 0;
    for ( ; scale < opt.num_scales - 1; ++scale)
    {
      if (fabs(center_surround[scale][i].first) < opt.scale_selection_threshhold)
        break;
    }
    display_lum[i] = luminances[i] / (1 + center_surround[scale][i].second);
  }

  output.resize(input.size());
  transform(display_lum.begin(), display_lum.end(), input.begin(), output.begin(),
            [](scalar lum, const spectrum& s) { return s.rescale_luminance(std::min<scalar>(lum, 1.0)).clamp(); });
}

vector<vector<pair<scalar, scalar>>> ReinhardLocal::center_surround_functions(
  uint w, uint h, const vector<scalar>& luminances, const Options& opt)
{
  vector<vector<pair<scalar, scalar>>> cs(opt.num_scales);

  vector<scalar> v1, v2, filter;
  scalar current_scale = 1;

  // compute the first center scale
  {
    int fs = gaussian_filter(opt.scale_pixel_factor * current_scale, filter);
    convolve(w, h, luminances, fs, fs, filter, v1);
  }

  scalar normalizing_mult = pow(2, opt.sharpening_factor) * opt.key_value;

  /*
   * At each iteration, compute the surround function, followed by the
   * center-surround function.
   */
  for (int i = 0; i < opt.num_scales; ++i)
  {
    int fs = gaussian_filter(opt.scale_pixel_factor * current_scale * opt.cs_ratio, filter);
    const scalar normalization = normalizing_mult / (current_scale * current_scale);
    convolve(w, h, luminances, fs, fs, filter, v2);

    cs[i].resize(v1.size());
    transform(v1.begin(), v1.end(), v2.begin(), cs[i].begin(),
              [=](scalar r1, scalar r2)
              {
                return std::make_pair((r1 - r2) / (normalization + r1), r1);
              });

    // We increase the scale by the same factor as the center-surround ratio, so
    // that we can reuse the surround from the previous scale as the center for
    // the current scale.
    current_scale *= opt.cs_ratio;
    std::swap(v1, v2);
  }

  return cs;
}

/*
 * Returns the gaussian filter with sigma scale parameter.
 *
 * The input vector is resized to fit the filter, and the size of the filter (on
 * a side) is returned.
 */
int ReinhardLocal::gaussian_filter(scalar scale, vector<scalar>& filter)
{
  int hf_size = ceil(scale);
  int filter_size = hf_size * 2 + 1;

  filter.resize(0);
  filter.reserve(filter_size * filter_size);

  const scalar s2 = scale * scale;
  for (int y = -hf_size; y <= hf_size; ++y)
  {
    for (int x = -hf_size; x <= hf_size; ++x)
    {
      filter.push_back( exp( -(x*x + y*y) / s2) / (PI * s2) );
    }
  }

  return filter_size;
}

/**
 * Integer square root algorithm, taken from:
 * http://www.codecodex.com/wiki/Calculate_an_integer_square_root
 */
unsigned int isqrt(unsigned long n)
{
  unsigned int c = 0x8000;
  unsigned int g = 0x8000;

  while (true) {
    if (g * g > n)
      g ^= c;
    c >>= 1;
    if (c == 0)
      return g;
    g |= c;
  }
}
<commit_msg>Changes post tone-mapper from linear to cutoff<commit_after>#include <algorithm>
#include <iostream>
#include <cassert>
#include "reinhard.h"

using std::transform;

/*
 * Compute the luminance vector Lw, as well as the mean log luminance, as
 * specified in Eq. 1.
 */
void reinhard_tonemap_aux(const vector<spectrum>& input, int n,
                          vector<scalar>& luminances, scalar& mean_log_luminance)
{
  scalar delta = 0.001;
  luminances.resize(n);

  transform(input.begin(), input.end(), luminances.begin(),
            [](const spectrum& s) { return s.luminance(); });

  scalar total_log = accumulate(luminances.begin(), luminances.end(), scalar(0),
                                [=](scalar s, scalar v) { return s + log(delta + v); });

  mean_log_luminance = exp(total_log / n);
}

void ReinhardGlobal::tonemap(const vector<spectrum>& input, vector<spectrum>& output,
                             uint w, uint h) const
{
  vector<scalar> luminances(w*h);
  scalar mean_log_luminance;
  reinhard_tonemap_aux(input, w*h, luminances, mean_log_luminance);

  scalar l_mult = key_value / mean_log_luminance;
  scalar max_luminance = *max_element(luminances.begin(), luminances.end());
  scalar ml2 = max_luminance * max_luminance;

  // Instead of compute Eq. 2 and Eq. 4 separately, we merge them into the same
  // transform calcuation.
  transform(luminances.begin(), luminances.end(), luminances.begin(),
            [=](scalar s) { return s * (l_mult + s / ml2) / (1 + l_mult * s); });

  transform(input.begin(), input.end(), luminances.begin(),
            output.begin(),
            [](const spectrum& sp, scalar s) { return sp.rescale_luminance(s).clamp(); });
}

ReinhardLocal::ReinhardLocal(const Options& options) : opt(options)
{
}

ReinhardLocal::Options::Options()
  : key_value(0.18), scale_selection_threshhold(0.05), sharpening_factor(8),
    scale_pixel_factor(sqrt(0.125)), cs_ratio(1.6), num_scales(8)
{
}

void ReinhardLocal::tonemap(const vector<spectrum>& input, vector<spectrum>& output,
                            uint w, uint h) const
{
  vector<scalar> luminances(w*h);
  scalar mean_log_luminance;
  reinhard_tonemap_aux(input, w*h, luminances, mean_log_luminance);
  scalar l_mult = opt.key_value / mean_log_luminance;

  // Explicitly compute L(x, y) as specified in equation 2.
  transform(luminances.begin(), luminances.end(), luminances.begin(),
            [=](scalar L) { return L * l_mult; });

  auto center_surround = center_surround_functions(w, h, luminances, opt);

  vector<scalar> display_lum(input.size());
  for (auto i = 0u; i < luminances.size(); ++i)
  {
    int scale = 0;
    for ( ; scale < opt.num_scales - 1; ++scale)
    {
      if (fabs(center_surround[scale][i].first) < opt.scale_selection_threshhold)
        break;
    }
    display_lum[i] = luminances[i] / (1 + center_surround[scale][i].second);
    //display_lum[i] = center_surround[5][i].first;
  }

  output.resize(input.size());
  transform(display_lum.begin(), display_lum.end(), input.begin(), output.begin(),
            [](scalar lum, const spectrum& s) { return s.rescale_luminance(std::min<scalar>(lum, 1.0)).clamp(); });
  // transform(display_lum.begin(), display_lum.end(), output.begin(),
  //           [](scalar lum)
  //           {
  //             return spectrum{lum};
  //           });

  CutoffToneMapper().tonemap(output, output, w, h);
}

vector<vector<pair<scalar, scalar>>> ReinhardLocal::center_surround_functions(
  uint w, uint h, const vector<scalar>& luminances, const Options& opt)
{
  vector<vector<pair<scalar, scalar>>> cs(opt.num_scales);

  vector<scalar> v1, v2, filter;
  scalar current_scale = 1;

  // compute the first center scale
  {
    scalar v1_gf_sigma = opt.scale_pixel_factor * current_scale;
    int fs = gaussian_filter(v1_gf_sigma, filter);
    convolve(w, h, luminances, fs, fs, filter, v1);
  }

  scalar normalizing_mult = pow(2, opt.sharpening_factor) * opt.key_value;

  /*
   * At each iteration, compute the surround function, followed by the
   * center-surround function.
   */
  for (int i = 0; i < opt.num_scales; ++i)
  {
    scalar v2_gf_sigma = opt.scale_pixel_factor * current_scale * opt.cs_ratio;
    int fs = gaussian_filter(v2_gf_sigma, filter);
    const scalar normalization = normalizing_mult / (current_scale * current_scale);
    convolve(w, h, luminances, fs, fs, filter, v2);

    cs[i].resize(v1.size());
    transform(v1.begin(), v1.end(), v2.begin(), cs[i].begin(),
              [=](scalar r1, scalar r2)
              {
                return std::make_pair((r1 - r2) / (normalization + r1), r1);
              });

    // We increase the scale by the same factor as the center-surround ratio, so
    // that we can reuse the surround from the previous scale as the center for
    // the current scale.
    current_scale *= opt.cs_ratio;
    std::swap(v1, v2);
  }

  return cs;
}

/*
 * Returns the gaussian filter with sigma scale parameter.
 *
 * The input vector is resized to fit the filter, and the size of the filter (on
 * a side) is returned.
 */
int ReinhardLocal::gaussian_filter(scalar scale, vector<scalar>& filter)
{
  int hf_size = ceil(scale);
  int filter_size = hf_size * 2 + 1;

  filter.resize(0);
  filter.reserve(filter_size * filter_size);

  const scalar s2 = scale * scale;
  for (int y = -hf_size; y <= hf_size; ++y)
  {
    for (int x = -hf_size; x <= hf_size; ++x)
    {
      filter.push_back( exp( -(x*x + y*y) / s2) / (PI * s2) );
    }
  }

  return filter_size;
}

/**
 * Integer square root algorithm, taken from:
 * http://www.codecodex.com/wiki/Calculate_an_integer_square_root
 */
unsigned int isqrt(unsigned long n)
{
  unsigned int c = 0x8000;
  unsigned int g = 0x8000;

  while (true) {
    if (g * g > n)
      g ^= c;
    c >>= 1;
    if (c == 0)
      return g;
    g |= c;
  }
}
<|endoftext|>


<commit_before>/*
   Copyright 2015 Daniel Jay Haskin

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

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

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#ifndef UDR_RESOLVER_HPP
#define UDR_RESOLVER_HPP 1
#include <list>
#include <algorithm>
#include <string>
#include "types.hpp"
#include <boost/expected.hpp>

/// HAVING A CONSTRAINT IS NOT GOOD.
// I NEED A VECTOR OF CONSTRAINTS
// SO THAT I CAN ALSO EASILY HAVE NO
// CONSTRAINTS AT ALL
// EITHER THAT OR GET IT TO WHERE
// CONSTRAINTS DEFAULT TO "true"
//
/* TODO:
 *   - Expected value use for repository
 *   - Start implementing stuff
 *   - circular dependencies
 *   - conjunctive normal form orders
 *   - DNF is easier to process, but CNF is more natural to specify.
 *   - (a>1&a<2|b>1&b<2)&(b>3&b<4|c>3&c<8)
 *   - (b>1&b<2&b>3&b<4)|(a>1&a<2&c>3&c<8)
     *   cnf terms: {name -> (relation -> version)}
     *   dnf terms: {name -> (relation -> version)}
     *   convert from cnf to dnf, or at least specify in dnf to start with
 *   - memoize what candidates have already been found a&b|(a&c);(in the case of (a|b@1)&(b@|c)&d&b@3 -> b@1,2,3|a&c, previously memoize that we've already found b (or so? i don't like first found).
 *       - Get list of which names are in multiple terms and how many terms they are in. Resolve their terms first, preferring the names which are listed frequently.
 *       - If I can use b to satisfy both, that is better. Therefore,
 *   - template by name type so that they can validate, set it by default to udr::name_type
 *   - version type is a thing, with comparison and match() -- template or virtualize?
 */
namespace udr {

    typedef std::string url_type;
    template <typename V>
        using std::function<bool(const V &, const V &)> = constraint_type;

    template <typename N, typename V>
    struct package_specifier
    {
        typedef N name_type;
        typedef V version_type;
        name_type name;
        std::vector< constraint_type<version_type> > constraints;
    }

    template <typename N, typename V>
    struct package
    {
        typedef N name_type;
        typedef V version_type;
        version_type version;
        url_type URL;
        order_type<N, V> requires;
        boost::optional<N> obsoletes;
        order_type<N, V> suggests;
        package() = default;
    };

    template <typename N, typename V>
        using std::vector<package_specifier<N, V> > = possibilities_type;

    template <typename N, typename V>
        using std::vector<possibilities_type<N, V> > = order_type;

    template <typename N, typename V>
    class repository {
        public:
            typedef N name_type;
            typedef V version_type;
            virtual std::vector<package> query(const N & name) const = 0;
    };

    template <typename N, typename V>
    class resolver {
        public:
            enum class suggests_policy {
                REQUIRED,
                BEST_EFFORT,
                IGNORE
            };

        private:
            suggests_policy policy;
        // herein lies a lot of typedefs and the use of boost::variant.
        // Please excuse my heavy LISP accent.
        // Or should I just say, pleathe excuthe my lithp? :P
        typedef std::vector<package<N,V> > resolver_success_type;
        typedef order_type<N, V> resolver_failure_type;
        // not useful, given possibilities. Even candidates for one possibility
        /*
        template <typename N, typename V>
        struct resolver_failed_type
        {
            typedef N name_type;
            typedef V version_type;

            std::vector<package_specifier<N, V> > ancestor_line;
            N name;
            resolver_failed_type() = default;
        };
        typedef boost::variant<resolver_success_type,
                               resolver_failed_type> resolver_result_type;
        */
        typedef boost::optional<resolver_success_type>
            resolver_result_type;

        const repository *repo;
        const bool reorder;

        resolver_result_type resolve_requirement(
                const possibilities_type & requirement,
                order_type<N, V> & suggests,
                std::map<N, V> & already_installed,
                std::map<N, N> & obsoleted_by,
                std::map<N, std::vector< constraint_type > > & conflicts) {
            for (auto possibility : requirement) {
                auto search_installed = already_installed.find(possibility.name);
                if (search_installed != already_installed.end()) {
                    if (all_of(possibility.constraints.cbegin(),
                                possibility.constraints.cend(),
                                [&search_installed](auto constraint) {
                                    constraint((*search_installed).second)
                                })) {
                        return resolver_success_type{};
                    }
                    else {
                        return boost::none;
                    }
                }
                else {
                    auto search_conflicts = conflicts.find(possibility.name);
                    // check for blank-check conflict with _name_
                    if (search_conflicts != conflicts.cend() &&
                            (*search_conflicts).second.empty()) {
                        return boost::none;
                    }
                    else {
                        auto candidates = repo->query(possibility.name);
                        for (auto candidate : candidates) {
                            if (!all_of(possibility.constraints.cbegin(),
                                        possibility.constraints.cend(),
                                        [&candidate](auto constraint) {
                                            return constraint(candidate.version)
                                        }))
                            {
                                continue;
                            }
                            // check again for granular conflict this time
                            if (search_conflicts != conflicts.cend() &&
                                    all_of((*search_conflicts).second.cbegin(),
                                        (*search_conflicts).second.cend(),
                                        [&candidate](auto conflict) {
                                            return conflict(candidate.version)
                                        })) {
                                continue;
                            }
                            // OOOOOOKay.
                            // We'll take this mutation one at a time.
                            // newsuggests. suppose we add stuff. if it fails,
                            // we just remove it again.
                            // newalreadyinstalled. Suppose we add stuff.
                            suggests.insert(suggests.end(),
                                    candidate.suggests.begin(),
                                    candidate.suggests.end());
                            already_installed.insert(possibility.name,
                                    candidate.version);
                            // Can we do this safely yet? NO.
                            // We cannot prove yet with current code that
                            // obsoleted_by doesn't already have the
                            // given name in it already.
                            //
                            // We need to provide this guarantee by implementing
                            // code that uses this feature.
                            if (candidate.obsoletes) {
                                obsoleted_by.insert(*(candidate.obsoletes),
                                        possibility.name);
                            }
                            conflicts.insert(suggests.end(),
                                    candidate.conflicts.begin(),
                                    candidate.conflicts.end());

                            auto results = resolve(
                                    candidate.requires,
                                    suggests,
                                    already_installed,
                                    obsoleted_by,
                                    conflicts);
                            if (results) {
                                // In this case, we want the mutations above to
                                // persist, so we are clear to simply return
                                // success and leave the sets mutated for future
                                // use by the calling resolve() function.
                                return *results;
                            }
                            else {
                                // Put everything back.
                                suggests.erase(
                                        suggests.end() -
                                            candidate.suggests.size(),
                                        suggests.end());
                                already_installed.erase(possibility.name);
                                if (candidate.obsoletes) {
                                    obsoleted_by.erase(*(candidate.obsoletes));
                                }
                                conflicts.erase(
                                        conflicts.end() - candidate.conflicts.size(),
                                        conflicts.end());
                            }
                        }
                    }
                }
            }
            return boost::none;
        }

        // this is the private version
        // because in order to do stuff like
        // pass sets and maps down recursively
        // and perform reordering, everything passed
        // in must be mutable.
        resolver_result_type resolve(
                // A constant reference to an ordered collection.
                // Aaah, determinism.
                const order_type<N, V> & requires,
                order_type<N, V> & suggests,
                std::map<N, V> & already_installed,
                std::map<N, N> & obsoleted_by,
                std::map<N, std::vector< constraint_type> > & conflicts) {

            resolver_success_type cumulative;

            for (auto requirement : requires) {
                auto results = resolve_requirement(requirement,
                        suggests,
                        already_installed,
                        obsoleted_by,
                        conflicts
                        );
                if (results) {
                    cumulative.insert(cumulative.cend(), (*results).first(),
                            (*results).last());
                }
                else
                {
                    return boost::none;
                }
            }
            if (this->policy != suggests_policy::IGNORE) {
                for (auto suggestion : suggests) {
                    auto results = resolve_requirement(requirement,
                            suggests,
                            already_installed,
                            obsoleted_by,
                            conflicts
                            );
                    if (results) {
                        cumulative.insert(cumulative.cend(), (*results).first(),
                                (*results).last());
                    }
                    else
                    {
                        return boost::none;
                    }
                }
            }
            return cumulative;
        }

    public:
        resolver_success_type resolve(const order_type & order) const {
            // call *actual* resolver here
                std::vector<URL> result;
                for (auto package : order) {
                    auto name = package.first;
                    auto constraints = package.second;

                    // a list for a REASON
                    // Lets the package system determine
                    // what version of package to try first
                    // This allows pooling vs priority repositories.
                    std::vector<std::pair<version_type, URL> > candidates =
                        repo->query_available(name);
                    for (auto constraint : constraints) {
                        std::remove_if(candidates.begin(), candidates.end(),
                                [constraint&](auto x){return constraint(x.first);});
                    }

                    if (candidates.empty() ) {
                        std::vector<std::pair<name_type, std::set<constraint_type> > > lst;
                        lst.push_front(package);
                        return make_unexpected(lst);
                    }

                    for (auto candidate : candidates) {
                        auto candidate_version = candidate.first;
                        auto candidate_url = candidate.second;
                        auto candidate_deps =
                            repo->query_dependencies(name, candidate_version);
                        auto candidate_dep_urls = this->resolve(candidate_deps);
                        if (candidate_dep_urls) {
                            if (!(*candidate_dep_urls).empty()) {
                                result.splice(result.cend(), *candidate_dep_urls);
                            }
                        }
                        else
                        {
                            // return error case
                            std::forwared_list<std::pair<name_type, set<constraint_type> > >
                                lst;
                            lst.splice(lst.cend(),candidate_dep_urls.error().value());
                            return lst;
                        }
                        result.push_back(candidate_url);
                    }
                }
                return result;
            }
    };
}

#endif // UDR_REPOSITORY_HPP
<commit_msg>Some cleanup<commit_after>/*
   Copyright 2015 Daniel Jay Haskin

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

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

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#ifndef UDR_RESOLVER_HPP
#define UDR_RESOLVER_HPP 1
#include <list>
#include <algorithm>
#include <string>
#include "types.hpp"
#include <boost/expected.hpp>

/// HAVING A CONSTRAINT IS NOT GOOD.
// I NEED A VECTOR OF CONSTRAINTS
// SO THAT I CAN ALSO EASILY HAVE NO
// CONSTRAINTS AT ALL
// EITHER THAT OR GET IT TO WHERE
// CONSTRAINTS DEFAULT TO "true"
//
/* TODO:
 *   - Expected value use for repository
 *   - Start implementing stuff
 *   - circular dependencies
 *   - conjunctive normal form orders
 *   - DNF is easier to process, but CNF is more natural to specify.
 *   - (a>1&a<2|b>1&b<2)&(b>3&b<4|c>3&c<8)
 *   - (b>1&b<2&b>3&b<4)|(a>1&a<2&c>3&c<8)
     *   cnf terms: {name -> (relation -> version)}
     *   dnf terms: {name -> (relation -> version)}
     *   convert from cnf to dnf, or at least specify in dnf to start with
 *   - memoize what candidates have already been found a&b|(a&c);(in the case of (a|b@1)&(b@|c)&d&b@3 -> b@1,2,3|a&c, previously memoize that we've already found b (or so? i don't like first found).
 *       - Get list of which names are in multiple terms and how many terms they are in. Resolve their terms first, preferring the names which are listed frequently.
 *       - If I can use b to satisfy both, that is better. Therefore,
 *   - template by name type so that they can validate, set it by default to udr::name_type
 *   - version type is a thing, with comparison and match() -- template or virtualize?
 */
namespace udr {

    typedef std::string url_type;
    template <typename V>
        using std::function<bool(const V &, const V &)> = constraint_type;

    template <typename N, typename V>
    struct package_specifier
    {
        typedef N name_type;
        typedef V version_type;
        name_type name;
        std::vector< constraint_type<version_type> > constraints;
    }

    template <typename N, typename V>
    struct package
    {
        typedef N name_type;
        typedef V version_type;
        version_type version;
        url_type URL;
        order_type<N, V> requires;
        boost::optional<N> obsoletes;
        order_type<N, V> suggests;
        package() = default;
    };

    template <typename N, typename V>
        using std::vector<package_specifier<N, V> > = possibilities_type;

    template <typename N, typename V>
        using std::vector<possibilities_type<N, V> > = order_type;

    template <typename N, typename V>
    class repository {
        public:
            typedef N name_type;
            typedef V version_type;
            virtual std::vector<package> query(const N & name) const = 0;
    };

    template <typename N, typename V>
    class resolver {
        public:
            enum class suggests_policy {
                REQUIRED,
                BEST_EFFORT,
                IGNORE
            };

        private:
            suggests_policy policy;
        // herein lies a lot of typedefs and the use of boost::variant.
        // Please excuse my heavy LISP accent.
        // Or should I just say, pleathe excuthe my lithp? :P
        typedef std::vector<package<N,V> > resolver_success_type;
        typedef order_type<N, V> resolver_failure_type;
        // not useful, given possibilities. Even candidates for one possibility
        /*
        template <typename N, typename V>
        struct resolver_failed_type
        {
            typedef N name_type;
            typedef V version_type;

            std::vector<package_specifier<N, V> > ancestor_line;
            N name;
            resolver_failed_type() = default;
        };
        typedef boost::variant<resolver_success_type,
                               resolver_failed_type> resolver_result_type;
        */
        typedef boost::optional<resolver_success_type>
            resolver_result_type;

        const repository *repo;
        const bool reorder;

        resolver_result_type resolve_requirement(
                const possibilities_type & requirement,
                order_type<N, V> & suggests,
                std::map<N, V> & already_installed,
                std::map<N, N> & obsoleted_by,
                std::map<N, std::vector< constraint_type > > & conflicts) {
            for (auto possibility : requirement) {
                auto search_installed = already_installed.find(possibility.name);
                if (search_installed != already_installed.end()) {
                    if (all_of(possibility.constraints.cbegin(),
                                possibility.constraints.cend(),
                                [&search_installed](auto constraint) {
                                    constraint((*search_installed).second)
                                })) {
                        return resolver_success_type{};
                    }
                    else {
                        return boost::none;
                    }
                }
                else {
                    auto search_conflicts = conflicts.find(possibility.name);
                    // check for blank-check conflict with _name_
                    if (search_conflicts != conflicts.cend() &&
                            (*search_conflicts).second.empty()) {
                        return boost::none;
                    }
                    else {
                        auto candidates = repo->query(possibility.name);
                        for (auto candidate : candidates) {
                            if (!all_of(possibility.constraints.cbegin(),
                                        possibility.constraints.cend(),
                                        [&candidate](auto constraint) {
                                            return constraint(candidate.version)
                                        }))
                            {
                                continue;
                            }
                            // check again for granular conflict this time
                            if (search_conflicts != conflicts.cend() &&
                                    all_of((*search_conflicts).second.cbegin(),
                                        (*search_conflicts).second.cend(),
                                        [&candidate](auto conflict) {
                                            return conflict(candidate.version)
                                        })) {
                                continue;
                            }
                            // OOOOOOKay.
                            // We'll take this mutation one at a time.
                            // newsuggests. suppose we add stuff. if it fails,
                            // we just remove it again.
                            // newalreadyinstalled. Suppose we add stuff.
                            suggests.insert(suggests.end(),
                                    candidate.suggests.begin(),
                                    candidate.suggests.end());
                            already_installed.insert(possibility.name,
                                    candidate.version);
                            // Can we do this safely yet? NO.
                            // We cannot prove yet with current code that
                            // obsoleted_by doesn't already have the
                            // given name in it already.
                            //
                            // We need to provide this guarantee by implementing
                            // code that uses this feature.
                            if (candidate.obsoletes) {
                                obsoleted_by.insert(*(candidate.obsoletes),
                                        possibility.name);
                            }
                            conflicts.insert(suggests.end(),
                                    candidate.conflicts.begin(),
                                    candidate.conflicts.end());

                            auto results = resolve(
                                    candidate.requires,
                                    suggests,
                                    already_installed,
                                    obsoleted_by,
                                    conflicts);
                            if (results) {
                                // In this case, we want the mutations above to
                                // persist, so we are clear to simply return
                                // success and leave the sets mutated for future
                                // use by the calling resolve() function.
                                return *results;
                            }
                            else {
                                // Put everything back.
                                suggests.erase(
                                        suggests.end() -
                                            candidate.suggests.size(),
                                        suggests.end());
                                already_installed.erase(possibility.name);
                                if (candidate.obsoletes) {
                                    obsoleted_by.erase(*(candidate.obsoletes));
                                }
                                conflicts.erase(
                                        conflicts.end() - candidate.conflicts.size(),
                                        conflicts.end());
                            }
                        }
                    }
                }
            }
            return boost::none;
        }

        // This is the private version
        // because in order to do stuff like
        // pass sets and maps down recursively,
        // (most) everything passed in must be mutable.
        resolver_result_type resolve(
                // A constant reference to an ordered collection.
                // Aaah, determinism.
                const order_type<N, V> & requires,
                order_type<N, V> & suggests,
                std::map<N, V> & already_installed,
                std::map<N, N> & obsoleted_by,
                std::map<N, std::vector< constraint_type> > & conflicts) {

            resolver_success_type cumulative;

            for (auto requirement : requires) {
                auto results = resolve_requirement(requirement,
                        suggests,
                        already_installed,
                        obsoleted_by,
                        conflicts
                        );
                if (results) {
                    cumulative.insert(cumulative.cend(), (*results).first(),
                            (*results).last());
                }
                else
                {
                    return boost::none;
                }
            }
            if (this->policy != suggests_policy::IGNORE) {
                for (auto suggestion : suggests) {
                    auto results = resolve_requirement(requirement,
                            suggests,
                            already_installed,
                            obsoleted_by,
                            conflicts
                            );
                    if (results) {
                        cumulative.insert(cumulative.cend(), (*results).first(),
                                (*results).last());
                    }
                    else
                    {
                        return boost::none;
                    }
                }
            }
            return cumulative;
        }

    public:
        resolver_success_type resolve(const order_type & order) const {
        }
    };
}

#endif // UDR_REPOSITORY_HPP
<|endoftext|>


<commit_before><commit_msg>Fix for 3D Window ignoring mouse commands.<commit_after><|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file imu.c
 *
 * \author MAV'RIC Team
 * \author Felix Schill
 * \author Gregoire Heitz
 * \author Julien Lecoeur
 *
 * \brief Inertial measurement unit (IMU)
 *
 ******************************************************************************/

#include "imu.hpp"

extern "C"
{
	#include "time_keeper.hpp"
	#include "print_util.h"
	#include "constants.h"
}

Imu::Imu(Accelerometer& accelerometer,
		Gyroscope& gyroscope,
		Magnetometer& magnetometer,
		imu_conf_t config):
	accelerometer_(accelerometer),
	gyroscope_(gyroscope),
	magnetometer_(magnetometer),
	config_(config),
	oriented_acc_( 	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	oriented_gyro_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	oriented_mag_( 	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_acc_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_gyro_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_mag_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	do_accelerometer_bias_calibration_(false),
	do_gyroscope_bias_calibration_(false),
	do_magnetometer_bias_calibration_(false),
	dt_s_(0.004f),
	last_update_us_(time_keeper_get_us())
{}

bool Imu::update(void)
{
	bool success = false;

	// Update timing
	uint32_t t 		= time_keeper_get_us();
	dt_s_ 			= (float)( t - last_update_us_ ) / 1000000.0f;
	last_update_us_ = t;

	// Read new values from sensors
	success &= accelerometer_.update();
	success &= gyroscope_.update();
	success &= magnetometer_.update();

	// Retrieve data
	std::array<float, 3> raw_acc  = accelerometer_.acc();
	std::array<float, 3> raw_gyro = gyroscope_.gyro();
	std::array<float, 3> raw_mag  = magnetometer_.mag();

	// Rotate sensor values
	for( uint8_t i=0; i<3; i++ )
	{
		oriented_acc_[i]  = raw_acc[  config_.accelerometer.axis[i]	] * config_.accelerometer.sign[i];
		oriented_gyro_[i] = raw_gyro[ config_.gyroscope.axis[i]		] * config_.gyroscope.sign[i];
		oriented_mag_[i]  = raw_mag[  config_.magnetometer.axis[i]	] * config_.magnetometer.sign[i];
	}

	// Scale sensor values
	float new_scaled_acc[3];
	float new_scaled_gyro[3];
	float new_scaled_mag[3];
	for( int8_t i = 0; i < 3; i++ )
	{
		// Scale, then remove bias
		new_scaled_acc[i]  = oriented_acc_[i]  / config_.accelerometer.scale_factor[i] - config_.accelerometer.bias[i];
		new_scaled_gyro[i] = oriented_gyro_[i] / config_.gyroscope.scale_factor[i]     - config_.gyroscope.bias[i];
		new_scaled_mag[i]  = oriented_mag_[i]  / config_.magnetometer.scale_factor[i]  - config_.magnetometer.bias[i];

		// Low pass filter
		scaled_acc_[i]  = config_.lpf_acc  	* new_scaled_acc[i]  + (1.0f - config_.lpf_acc ) * scaled_acc_[i];
		scaled_gyro_[i] = config_.lpf_gyro  * new_scaled_gyro[i] + (1.0f - config_.lpf_gyro) * scaled_gyro_[i];
		scaled_mag_[i] 	= config_.lpf_mag   * new_scaled_mag[i]  + (1.0f - config_.lpf_mag ) * scaled_mag_[i];
	}

	// Do accelero bias calibration
	if( do_accelerometer_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.accelerometer.mean_values[i] = 0.5 * ( config_.accelerometer.mean_values[i] + scaled_acc_[i] );
		}
	}

	// Do gyroscope bias calibration
	if( do_gyroscope_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.gyroscope.mean_values[i] = 0.5 * ( config_.gyroscope.mean_values[i] + scaled_gyro_[i] );
		}
	}

	// Do magnetometer bias calibration
	if( do_magnetometer_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.magnetometer.max_values[i]  = maths_f_max(config_.magnetometer.max_values[i], scaled_mag_[i]);
			config_.magnetometer.min_values[i]  = maths_f_min(config_.magnetometer.min_values[i], scaled_mag_[i]);
		}
	}

	return success;
}

const float& Imu::last_update_us(void) const
{
	return last_update_us_;
}

const float& Imu::dt_s(void) const
{
	return dt_s_;
}

const std::array<float, 3>& Imu::acc(void) const
{
	return scaled_acc_;
}

const std::array<float, 3>& Imu::gyro(void) const
{
	return scaled_gyro_;
}

const std::array<float, 3>& Imu::mag(void) const
{
	return scaled_mag_;
}

imu_conf_t* Imu::get_config(void)
{
	return &config_;
}

bool Imu::start_accelerometer_bias_calibration(void)
{
	// Success if not already doing magnetometer calibration
	bool success = !do_magnetometer_bias_calibration_;

	if( success )
	{
		do_accelerometer_bias_calibration_ 		= true;
		config_.accelerometer.mean_values[X] 	= scaled_acc_[X];
		config_.accelerometer.mean_values[Y] 	= scaled_acc_[Y];
		config_.accelerometer.mean_values[Z] 	= scaled_acc_[Z];
	}

	return success;
}

bool Imu::start_gyroscope_bias_calibration(void)
{
	// Success if not already doing magnetometer calibration
	bool success = !do_magnetometer_bias_calibration_;

	if( success )
	{
		do_gyroscope_bias_calibration_ 		= true;
		config_.gyroscope.mean_values[X] 	= scaled_gyro_[X];
		config_.gyroscope.mean_values[Y] 	= scaled_gyro_[Y];
		config_.gyroscope.mean_values[Z] 	= scaled_gyro_[Z];
	}

	return success;
}

bool Imu::start_magnetometer_bias_calibration(void)
{
	// Success if not already doing accelerometer or gyroscope calibration
	bool success  = true;
	success &= !do_accelerometer_bias_calibration_;
	success &= !do_gyroscope_bias_calibration_;

	if( success )
	{
		do_magnetometer_bias_calibration_ 	= true;
		config_.magnetometer.max_values[X] 	= -10000.0f;
		config_.magnetometer.max_values[Y] 	= -10000.0f;
		config_.magnetometer.max_values[Z] 	= -10000.0f;
		config_.magnetometer.min_values[X] 	=  10000.0f;
		config_.magnetometer.min_values[Y] 	=  10000.0f;
		config_.magnetometer.min_values[Z] 	=  10000.0f;
	}

	return success;
}

bool Imu::stop_accelerometer_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_accelerometer_bias_calibration_;

	// Stop calibrating
	do_accelerometer_bias_calibration_ = false;

	// Update biases
	config_.accelerometer.bias[X] = config_.accelerometer.mean_values[X];
	config_.accelerometer.bias[Y] = config_.accelerometer.mean_values[Y];
	config_.accelerometer.bias[Z] = config_.accelerometer.mean_values[Z] - (-1.0f);

	return success;
}

bool Imu::stop_gyroscope_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_gyroscope_bias_calibration_;

	// Stop calibrating
	do_gyroscope_bias_calibration_ = false;

	// Update biases
	config_.gyroscope.bias[X] = config_.gyroscope.mean_values[X];
	config_.gyroscope.bias[Y] = config_.gyroscope.mean_values[Y];
	config_.gyroscope.bias[Z] = config_.gyroscope.mean_values[Z];

	return success;
}

bool Imu::stop_magnetometer_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_magnetometer_bias_calibration_;

	// Stop calibrating
	do_magnetometer_bias_calibration_ = false;

	// Update biases
	config_.magnetometer.bias[X] = 0.5f * (config_.magnetometer.max_values[X] + config_.magnetometer.min_values[X]);
	config_.magnetometer.bias[Y] = 0.5f * (config_.magnetometer.max_values[Y] + config_.magnetometer.min_values[Y]);
	config_.magnetometer.bias[Z] = 0.5f * (config_.magnetometer.max_values[Z] + config_.magnetometer.min_values[Z]);

	return success;
}<commit_msg>Fix bug in imu calibration, biases were not updated properly<commit_after>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file imu.c
 *
 * \author MAV'RIC Team
 * \author Felix Schill
 * \author Gregoire Heitz
 * \author Julien Lecoeur
 *
 * \brief Inertial measurement unit (IMU)
 *
 ******************************************************************************/

#include "imu.hpp"

extern "C"
{
	#include "time_keeper.hpp"
	#include "print_util.h"
	#include "constants.h"
}

Imu::Imu(Accelerometer& accelerometer,
		Gyroscope& gyroscope,
		Magnetometer& magnetometer,
		imu_conf_t config):
	accelerometer_(accelerometer),
	gyroscope_(gyroscope),
	magnetometer_(magnetometer),
	config_(config),
	oriented_acc_( 	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	oriented_gyro_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	oriented_mag_( 	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_acc_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_gyro_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	scaled_mag_(	std::array<float,3>{{0.0f, 0.0f, 0.0f}} ),
	do_accelerometer_bias_calibration_(false),
	do_gyroscope_bias_calibration_(false),
	do_magnetometer_bias_calibration_(false),
	dt_s_(0.004f),
	last_update_us_(time_keeper_get_us())
{}

bool Imu::update(void)
{
	bool success = false;

	// Update timing
	uint32_t t 		= time_keeper_get_us();
	dt_s_ 			= (float)( t - last_update_us_ ) / 1000000.0f;
	last_update_us_ = t;

	// Read new values from sensors
	success &= accelerometer_.update();
	success &= gyroscope_.update();
	success &= magnetometer_.update();

	// Retrieve data
	std::array<float, 3> raw_acc  = accelerometer_.acc();
	std::array<float, 3> raw_gyro = gyroscope_.gyro();
	std::array<float, 3> raw_mag  = magnetometer_.mag();

	// Rotate sensor values
	for( uint8_t i=0; i<3; i++ )
	{
		oriented_acc_[i]  = raw_acc[  config_.accelerometer.axis[i]	] * config_.accelerometer.sign[i];
		oriented_gyro_[i] = raw_gyro[ config_.gyroscope.axis[i]		] * config_.gyroscope.sign[i];
		oriented_mag_[i]  = raw_mag[  config_.magnetometer.axis[i]	] * config_.magnetometer.sign[i];
	}

	// Scale sensor values
	float new_scaled_acc[3];
	float new_scaled_gyro[3];
	float new_scaled_mag[3];
	for( int8_t i = 0; i < 3; i++ )
	{
		// Scale, then remove bias
		new_scaled_acc[i]  = oriented_acc_[i]  / config_.accelerometer.scale_factor[i] - config_.accelerometer.bias[i];
		new_scaled_gyro[i] = oriented_gyro_[i] / config_.gyroscope.scale_factor[i]     - config_.gyroscope.bias[i];
		new_scaled_mag[i]  = oriented_mag_[i]  / config_.magnetometer.scale_factor[i]  - config_.magnetometer.bias[i];

		// Low pass filter
		scaled_acc_[i]  = config_.lpf_acc  	* new_scaled_acc[i]  + (1.0f - config_.lpf_acc ) * scaled_acc_[i];
		scaled_gyro_[i] = config_.lpf_gyro  * new_scaled_gyro[i] + (1.0f - config_.lpf_gyro) * scaled_gyro_[i];
		scaled_mag_[i] 	= config_.lpf_mag   * new_scaled_mag[i]  + (1.0f - config_.lpf_mag ) * scaled_mag_[i];
	}

	// Do accelero bias calibration
	if( do_accelerometer_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.accelerometer.mean_values[i] = 0.5 * ( config_.accelerometer.mean_values[i] + scaled_acc_[i] );
		}
	}

	// Do gyroscope bias calibration
	if( do_gyroscope_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.gyroscope.mean_values[i] = 0.5 * ( config_.gyroscope.mean_values[i] + scaled_gyro_[i] );
		}
	}

	// Do magnetometer bias calibration
	if( do_magnetometer_bias_calibration_ )
	{
		for( uint8_t i=0; i<3; i++ )
		{
			config_.magnetometer.max_values[i]  = maths_f_max(config_.magnetometer.max_values[i], scaled_mag_[i]);
			config_.magnetometer.min_values[i]  = maths_f_min(config_.magnetometer.min_values[i], scaled_mag_[i]);
		}
	}

	return success;
}

const float& Imu::last_update_us(void) const
{
	return last_update_us_;
}

const float& Imu::dt_s(void) const
{
	return dt_s_;
}

const std::array<float, 3>& Imu::acc(void) const
{
	return scaled_acc_;
}

const std::array<float, 3>& Imu::gyro(void) const
{
	return scaled_gyro_;
}

const std::array<float, 3>& Imu::mag(void) const
{
	return scaled_mag_;
}

imu_conf_t* Imu::get_config(void)
{
	return &config_;
}

bool Imu::start_accelerometer_bias_calibration(void)
{
	// Success if not already doing magnetometer calibration
	bool success = !do_magnetometer_bias_calibration_;

	if( success )
	{
		do_accelerometer_bias_calibration_ 		= true;
		config_.accelerometer.mean_values[X] 	= scaled_acc_[X];
		config_.accelerometer.mean_values[Y] 	= scaled_acc_[Y];
		config_.accelerometer.mean_values[Z] 	= scaled_acc_[Z];
	}

	return success;
}

bool Imu::start_gyroscope_bias_calibration(void)
{
	// Success if not already doing magnetometer calibration
	bool success = !do_magnetometer_bias_calibration_;

	if( success )
	{
		do_gyroscope_bias_calibration_ 		= true;
		config_.gyroscope.mean_values[X] 	= scaled_gyro_[X];
		config_.gyroscope.mean_values[Y] 	= scaled_gyro_[Y];
		config_.gyroscope.mean_values[Z] 	= scaled_gyro_[Z];
	}

	return success;
}

bool Imu::start_magnetometer_bias_calibration(void)
{
	// Success if not already doing accelerometer or gyroscope calibration
	bool success  = true;
	success &= !do_accelerometer_bias_calibration_;
	success &= !do_gyroscope_bias_calibration_;

	if( success )
	{
		do_magnetometer_bias_calibration_ 	= true;
		config_.magnetometer.max_values[X] 	= -10000.0f;
		config_.magnetometer.max_values[Y] 	= -10000.0f;
		config_.magnetometer.max_values[Z] 	= -10000.0f;
		config_.magnetometer.min_values[X] 	=  10000.0f;
		config_.magnetometer.min_values[Y] 	=  10000.0f;
		config_.magnetometer.min_values[Z] 	=  10000.0f;
	}

	return success;
}

bool Imu::stop_accelerometer_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_accelerometer_bias_calibration_;

	// Stop calibrating
	do_accelerometer_bias_calibration_ = false;

	// Update biases
	if( success )
	{
		config_.accelerometer.bias[X] += config_.accelerometer.mean_values[X];
		config_.accelerometer.bias[Y] += config_.accelerometer.mean_values[Y];
		config_.accelerometer.bias[Z] += config_.accelerometer.mean_values[Z] - (-1.0f);
	}

	return success;
}

bool Imu::stop_gyroscope_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_gyroscope_bias_calibration_;

	// Stop calibrating
	do_gyroscope_bias_calibration_ = false;

	// Update biases
	if( success )
	{
		config_.gyroscope.bias[X] += config_.gyroscope.mean_values[X];
		config_.gyroscope.bias[Y] += config_.gyroscope.mean_values[Y];
		config_.gyroscope.bias[Z] += config_.gyroscope.mean_values[Z];
	}

	return success;
}

bool Imu::stop_magnetometer_bias_calibration(void)
{
	// Success if calibration is ongoing
	bool success = do_magnetometer_bias_calibration_;

	// Stop calibrating
	do_magnetometer_bias_calibration_ = false;

	// Update biases
	config_.magnetometer.bias[X] += 0.5f * (config_.magnetometer.max_values[X] + config_.magnetometer.min_values[X]);
	config_.magnetometer.bias[Y] += 0.5f * (config_.magnetometer.max_values[Y] + config_.magnetometer.min_values[Y]);
	config_.magnetometer.bias[Z] += 0.5f * (config_.magnetometer.max_values[Z] + config_.magnetometer.min_values[Z]);

	return success;
}<|endoftext|>