/*
 * assertions.hpp
 *
 *  Created on: Aug 6, 2009
 *      Author: Jonathan Brandmeyer
 *
 *          This file is part of libeknav, imported into the OpenPilot
 *          project by Jonathan Brandmeyer.
 *
 *  Libeknav is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, version 3.
 *
 *  Libeknav is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.

 *  You should have received a copy of the GNU General Public License
 *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef AHRS_ASSERTIONS_HPP
#define AHRS_ASSERTIONS_HPP

#include <Eigen/Core>
#include <cmath>

template<typename MatrixBase>
bool hasNaN(const MatrixBase & expr);

template<typename MatrixBase>
bool hasInf(const MatrixBase & expr);

template<typename MatrixBase>
bool perpendicular(const MatrixBase & expl, const MatrixBase & expr);

template<typename MatrixBase>
bool hasNaN(const MatrixBase & expr)
{
    for (int j = 0; j != expr.cols(); ++j) {
        for (int i = 0; i != expr.rows(); ++i) {
            if (std::isnan(expr.coeff(i, j))) {
                return true;
            }
        }
    }
    return false;
}

template<typename MatrixBase>
bool hasInf(const MatrixBase & expr)
{
    for (int i = 0; i != expr.cols(); ++i) {
        for (int j = 0; j != expr.rows(); ++j) {
            if (std::isinf(expr.coeff(j, i))) {
                return true;
            }
        }
    }
    return false;
}

template<typename MatrixBase>
bool isReal(const MatrixBase & exp)
{
    return !hasNaN(exp) && !hasInf(exp);
}

template<typename MatrixBase>
bool perpendicular(const MatrixBase & lhs, const MatrixBase & rhs)
{
    // A really weak check for "almost perpendicular".  Use it for debugging
    // purposes only.
    return fabs(rhs.dot(lhs)) < 0.0001;
}

#endif /* ASSERTIONS_HPP_ */