// Listing 17.1. Kod przykładowego filtra Kalmana

#include <vector>
#include <iostream>
#include <cstdlib>
#include <fstream>
#include <opencv2/opencv.hpp>

using std::cout;
using std::endl;

#define phi2xy(mat) \
    cv::Point(cvRound(img.cols / 2 + img.cols / 3 * cos(mat.at<float>(0))), \
              cvRound(img.rows / 2 - img.cols / 3 * sin(mat.at<float>(0))))


void help(char** argv ) {
    cout << "\n"
         << "Listing 17.1. Kod przykładowego filtra Kalmana\n"
         << argv[0] << "\n\n"
         << "Na przykład:\n"
         << argv[0] <<"\n\n"
         << "Esc, aby zakończyć\n"
         << endl;
}

int main(int argc, char** argv) {
    help(argv);

	// inicjalizacja, utworzenie obiektu filtra Kalmana, okienka, generatora liczb losowych itd.
    //
    cv::Mat img(500, 500, CV_8UC3);
    cv::KalmanFilter kalman(2, 1, 0);

	// stan to (phi, delta_phi) — kąt i prędkość kątowa
	// inicjalizacja losowymi liczbami
    //
    cv::Mat x_k(2, 1, CV_32F);
    randn(x_k, 0.0, 0.1);

	// szum przetwarzania
    //
    cv::Mat w_k(2, 1, CV_32F);

	// pomiary, tylko jeden parametr dla kąta
    //
    cv::Mat z_k = cv::Mat::zeros(1, 1, CV_32F);

	// macierz zmiany stanu F opisuje relację między parametrami modelu w krokach k i k+1 (to jest dynamika w naszym modelu)
    //
    float F[] = {1, 1, 0, 1};
    kalman.transitionMatrix = cv::Mat(2, 2, CV_32F, F).clone();

	// inicjalizacja innych parametrów filtra Kalmana
    //
    cv::setIdentity(kalman.measurementMatrix, cv::Scalar(1));
    cv::setIdentity(kalman.processNoiseCov, cv::Scalar(1e-5));
    cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar(1e-1));
    cv::setIdentity(kalman.errorCovPost, cv::Scalar(1));

	// wybór losowego stanu początkowego
    //
    randn(kalman.statePost, 0.0, 0.1);

    for (;;) {
		// przewiduje pozycję punktu
        //
        cv::Mat y_k = kalman.predict();

		// generuje pomiar (z_k)
        //
        cv::randn(z_k, 0.0,
            sqrt(static_cast<double>(kalman.measurementNoiseCov.at<float>(0, 0))));
        z_k = kalman.measurementMatrix*x_k + z_k;

        // rysowanie punktów
        //
        img = cv::Scalar::all(0);
        cv::circle(img, phi2xy(z_k), 4, cv::Scalar(128, 255, 255));  // // obserwacja
        cv::circle(img, phi2xy(y_k), 4, cv::Scalar(255, 255, 255), 2);  // // przewidywanie
        cv::circle(img, phi2xy(x_k), 4, cv::Scalar(0, 0, 255));  // rzeczywistość
                                                                 // współrzędne płaszczyznowe i rysowanie

        cv::imshow("Kalman", img);

		// ustawianie stanu filtra Kalmana
        //
        kalman.correct(z_k);

		// zastosowanie macierzy zmiany stanu F (czyli krok w czasie do przodu) i dodanie szumu przetwarzania w_k
        //
        cv::randn(w_k, 0.0, sqrt(static_cast<double>(kalman.processNoiseCov.at<float>(0, 0))));
        x_k = kalman.transitionMatrix*x_k + w_k;

		// zamykanie programu, gdy użytkownik naciśnie klawisz Esc
        if ((cv::waitKey(100) & 255) == 27) {
            break;
        }
    }

    return 0;
}
