/*
 * =====================================================================================
 *
 *       Filename:  test_filter.cpp
 *
 *    Description:  Tests the filter class
 *
 *        Version:  1.0
 *        Created:  03/31/2017 03:13:54 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */


#include <iostream>

#include "filter.h"
using std::cout;
using std::endl;

int main(int argc, char **argv)
{
    Matrix<double,4,1> b;
    Matrix<double,3,1> a;
    b << 0.00289819,  0.00869458,  0.00869458,  0.00289819;
    a << -2.37409474,  1.92935567, -0.53207537;
    a/a[0];
    Filter H(b,a);
    double x,y,z;
    while (scanf("%lf,%lf,%lf", &x, &y, &z)!=EOF) {
        Vector3d X, Y;
        X << x,y,z;
        Y = H.update(X);
        cout << "X: " << X.transpose() << " Y: " << Y.transpose() << endl;
    }
	return 0;
}