/*
 * =====================================================================================
 *
 *       Filename:  filter.cpp
 *
 *    Description:  Class for FIR and IIR filters
 *
 *        Version:  1.0
 *        Created:  03/31/2017 02:40:02 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */
#include "filter.h"

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Filter
 *      Method:  Filter
 * Description:  constructor
 *--------------------------------------------------------------------------------------
 */
Filter::Filter (const VectorXd &b, const VectorXd &a)
{
    int rows = b.rows()+a.rows();
    F = Matrix<double,Dynamic,1>::Zero(rows,1);
    F.head(b.rows()) = b;
    F.segment(b.rows(),a.rows()) = -a;

    Vector3d z;
    z << 0,0,0;
    for (int i=0; i<b.rows(); ++i) {
        X.push_back(z);
    }
    for (int i=0; i<a.rows(); ++i) {
        Y.push_back(z);
    }
}  /* -----  end of method Filter::Filter  (constructor)  ----- */

Vector3d
Filter::update ( const Vector3d &x )
{
    X.push_front(x);
    X.pop_back();

    MatrixXd M;
    M = listToMatrix();

    Vector3d y;
    y = F.transpose()*M;

    Y.push_front(y);
    Y.pop_back();

    return y;
}		/* -----  end of method Filter::update  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Filter
 *      Method:  Filter :: listToMatrix
 * Description:  Returns X and Y as a matrix
 *--------------------------------------------------------------------------------------
 */
MatrixXd
Filter::listToMatrix() {
    Matrix<double,Dynamic,3> M;
    int rows = X.size() + Y.size();
    M = Matrix<double,Dynamic,3>::Zero(rows,3);

    int row = 0;
    for (auto i=X.begin(); i!=X.end(); ++i,++row) {
        M.row(row) = *i;
    }
    for (auto i=Y.begin(); i!=Y.end(); ++i,++row) {
        M.row(row) = *i;
    }

    return M;
}