#include <iostream>
#include <stdio.h>
using namespace std;

class mat_t {
public:
	int32_t row;
	int32_t col;
	double* p = NULL;
	mat_t(int32_t row = 1, int32_t col = 1, double val = 0.0) {
		this->row = row;
		this->col = col;
		int32_t cnt = this->row * this->col;
        if (cnt > 0) {
            this->p = new double[cnt];
            for (int32_t i = 0; i < cnt; i++) {
                this->p[i] = val;
            }
        }
	}
	~mat_t(void) {
        int32_t cnt = this->row * this->col;
        if (cnt > 0) {
            this->row = 0;
            this->col = 0;
            if (this->p) {
                delete this->p;
                this->p = 0;
            }
        }
	}
    double* ptr(int32_t row, int32_t col) {
	    return &(this->p[row * this->col + col]);
	}
	double read(int32_t row, int32_t col) {
	    return this->ptr(row, col)[0];
	}
	void write(int32_t row, int32_t col, double val) {
	    this->ptr(row, col)[0] = val;
	}
	mat_t eyes(int size, double gain = 1.0) {
	    mat_t temp = mat_t(size, size, 0.0);
        for (int i = 0; i < size; i++) {
            temp.write(i, i, gain);
        }
        return temp;
	}
	void copy(mat_t& src) {
	    if ((this->row != src.row) || (this->col != src.col)) {
            return;
	    }
	    for (int i = 0; i < this->row; i++) {
            for (int j = 0; j < this->col; j++) {
                double val = src.read(i, j);
                this->write(i, j, val);
            }
	    }
	}
	mat_t trans(void) {
		mat_t temp(this->col, this->row, 0.0);
		int32_t i, j;
		for (i = 0; i < this->col; i++) {
			for (j = 0; j < this->row; j++) {
				temp.write(i, j, this->read(j, i));
			}
		}
		return temp;
	}
	mat_t rm(int32_t i, int32_t j) {
	    mat_t temp(this->row - 1, this->col - 1, 0.0);
	    int32_t i1, j1, k1 = 0, k2 = 0;
        for (i1 = 0; i1 < this->row; i1++) {
            if (i1 == i) {
                k2 += col;
                continue;
            }
            for (j1 = 0; j1 < this->col; j1++) {
                if (j1 == j) {
                    k2++;
                    continue;
                }
                temp.p[k1++] = this->p[k2++];
            }
        }
        return temp;
	}
	void print(void) {
	    for (int i = 0; i < this->row; i++) {
	        for (int j = 0; j < this->col; j++) {
	            cout << this->read(i, j) << " ";
	        }
            cout << endl;
	    }
	}
	double det(void) {
	    if ((1 == this->row) && (1 == this->col)) {
            return this->p[0];
	    }
	    int32_t i, j = 1;
	    double sum = 0.0;
        for (i = 0; i < this->col; i++) {
            mat_t temp = this->rm(0, i);
            sum += j * this->p[i] * temp.det();
            j = -j;
        }
        return sum;
	}
	mat_t inv(void) {
		mat_t inv(this->row, this->col, 0.0);
	    double k, det_inv = 1.0 / this->det();
	    int32_t i, j;
        for (i = 0; i < this->row; i++) {
            for (j = 0; j < this->col; j++) {
                k = (i + j) & 1? -det_inv : det_inv;
                mat_t temp = this->rm(j, i);
                inv.write(i, j, k * temp.det());
            }
        }
        return inv;
	}
	void writes(double src) {
	    for (int32_t i = this->row * this->col - 1; i >= 0; i--) {
            this->p[i] = src;
	    }
	}
	mat_t operator + (mat_t &src) {
		if ((this->row != src.row) || (this->col != src.col)) {
			cout << "mat operator + is illegal, mat size is not equal!" << endl;
			return mat_t(1, 1, 0.0);
		}
		mat_t dst(this->row, this->col, 0.0);
		for (int32_t i = 0; i < dst.row; i++) {
			for (int32_t j = 0; j < dst.col; j++) {
                dst.write(i, j, this->read(i, j) + src.read(i, j));
			}
		}
		return dst;
	}
	mat_t operator - (mat_t &src) {
		if ((this->row != src.row) || (this->col != src.col)) {
			cout << "mat operator - is illegal, mat size is not equal!" << endl;
			return mat_t(1, 1, 0.0);
		}
		mat_t dst(this->row, this->col, 0.0);
		for (int32_t i = 0; i < dst.row; i++) {
			for (int32_t j = 0; j < dst.col; j++) {
                dst.write(i, j, this->read(i, j) - src.read(i, j));
			}
		}
		return dst;
	}
    mat_t operator * (mat_t &src) {
		if (this->col != src.row) {
			cout << "mat operator * is illegal, mat col and row is not equal!" << endl;
			return mat_t(1, 1, 0.0);
		}
		mat_t dst(this->row, src.col, 0.0);
		for (int32_t i = 0; i < dst.row; i++) {
			for (int32_t j = 0; j < dst.col; j++) {
                double sum = 0.0;
				for (int32_t k = 0; k < this->col; k++) {
                    sum += this->read(i, k) * src.read(k, j);
				}
				dst.write(i, j, sum);
			}
		}
		return dst;
	}
};

mat_t F(4, 4, 0.0);
mat_t H(2, 4, 0.0);
mat_t P0(4, 4, 0.0);
mat_t I(4, 4, 0.0);
mat_t Xkf(4, 1, 0.0);
mat_t Q(4, 4, 0.0);
mat_t R(2, 2, 0.0);
mat_t Z(2, 1, 0.0);

mat_t kalm_pool(double x, double y) {
    Z.write(0, 0, x);
    Z.write(1, 0, y);
    mat_t X_pre = F * Xkf;
    mat_t F_trans = F.trans();
    mat_t P_pre = (F * P0 * F_trans) + Q;
    mat_t H_trans = H.trans();
    mat_t K1 = (H * P_pre * H_trans + R);
    mat_t K1_inv = K1.inv();
    mat_t K = P_pre * H_trans * K1_inv;
    mat_t KH = K * H;
    mat_t P0_temp = (I - KH) * P_pre;
    mat_t HX_pre = H * X_pre;
    mat_t temp = Z - HX_pre;
    mat_t Xkf_temp = K * temp + X_pre;
    P0.copy(P0_temp);
    Xkf.copy(Xkf_temp);
    Xkf.trans().print();
    return Xkf;
}

void kalm_init(double T) {
    F.write(0, 0, 1.0);
    F.write(0, 1, T);
    F.write(1, 1, 1.0);
    F.write(2, 2, 1.0);
    F.write(2, 3, T);
    F.write(3, 3, 1.0);
    H.write(0, 0, 1.0);
    H.write(1, 2, 1.0);
    P0.write(0, 0, 1.0);
    P0.write(1, 1, 1.0);
    P0.write(2, 2, 1.0);
    P0.write(3, 3, 1.0);
    I.write(0, 0, 1.0);
    I.write(1, 1, 1.0);
    I.write(2, 2, 1.0);
    I.write(3, 3, 1.0);
    Q.write(0, 0, 0.05);
    Q.write(1, 1, 0.1);
    Q.write(2, 2, 0.05);
    Q.write(3, 3, 0.1);
    R.write(0, 0, 100.0);
    R.write(1, 1, 100.0);
}

int main() {
    FILE* f = fopen("Z.csv", "r");
    kalm_init(0.5);
    while (1) {
        double x, y;
        if (2 != fscanf(f, "%lf,%lf", &x, &y)) {
            break;
        }
        kalm_pool(x, y);
    }
    fclose(f);
    return 0;
}