// Copyright (C) 2018  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#ifndef DLIB_KALMAN_FiLTER_CPp_
#define DLIB_KALMAN_FiLTER_CPp_

#include "kalman_filter.h"
#include "../global_optimization.h"
#include "../statistics.h"

namespace dlib
{

// ----------------------------------------------------------------------------------------

    momentum_filter find_optimal_momentum_filter (
        const std::vector<std::vector<double>>& sequences,
        const double smoothness 
    )
    {
        DLIB_CASSERT(sequences.size() != 0);
        for (auto& vals : sequences)
            DLIB_CASSERT(vals.size() > 4);
        DLIB_CASSERT(smoothness >= 0);

        // define the objective function we optimize to find the best filter
        auto obj = [&](double measurement_noise, double typical_acceleration, double max_measurement_deviation)
        {
            running_stats<double> rs;
            for (auto& vals : sequences)
            {
                momentum_filter filt(measurement_noise, typical_acceleration, max_measurement_deviation);
                double prev_filt = 0;
                for (size_t i = 0; i < vals.size(); ++i)
                {
                    // we care about smoothness and fitting the data.
                    if (i > 0)
                    {
                        // the filter should fit the data
                        rs.add(std::abs(vals[i]-filt.get_predicted_next_state()));
                    }
                    double next_filt = filt(vals[i]);
                    if (i > 0)
                    {
                        // the filter should also output a smooth trajectory
                        rs.add(smoothness*std::abs(next_filt-prev_filt));
                    }
                    prev_filt = next_filt;
                }
            }
            return rs.mean();
        };

        running_stats<double> avgdiff;
        for (auto& vals : sequences)
        {
            for (size_t i = 1; i < vals.size(); ++i)
                avgdiff.add(vals[i]-vals[i-1]);
        }
        const double scale = avgdiff.stddev();

        function_evaluation opt = find_min_global(obj, {scale*0.01, scale*0.0001, 0.00001}, {scale*10, scale*10, 10}, max_function_calls(400));

        momentum_filter filt(opt.x(0), opt.x(1), opt.x(2));

        return filt;
    }

// ----------------------------------------------------------------------------------------

    momentum_filter find_optimal_momentum_filter (
        const std::vector<double>& sequence,
        const double smoothness 
    )
    {
        return find_optimal_momentum_filter({1,sequence}, smoothness);
    }

// ----------------------------------------------------------------------------------------

    rect_filter find_optimal_rect_filter (
        const std::vector<rectangle>& rects,
        const double smoothness
    )
    {
        DLIB_CASSERT(rects.size() > 4);
        DLIB_CASSERT(smoothness >= 0);

        std::vector<std::vector<double>> vals(4);
        for (auto& r : rects)
        {
            vals[0].push_back(r.left());
            vals[1].push_back(r.top());
            vals[2].push_back(r.right());
            vals[3].push_back(r.bottom());
        }
        return rect_filter(find_optimal_momentum_filter(vals, smoothness));
    }

// ----------------------------------------------------------------------------------------

}

#endif // DLIB_KALMAN_FiLTER_CPp_

