#include "nav_map_match_markov.h"
#include <iostream>
#include <iomanip>

void NAV_Map_Match_origin_search(NAV_Map_Match &global, NAV_Map_Match &local, NAV_Map_Base<ScoredHeading> &markov)
{
    markov.clear();
    if (global.getResolution()!=local.getResolution()) return;

    const int    srad = 10;
    const double dx   = global.getResolution();
    const double dy   = global.getResolution();
    // ranges must not go outside the global map range or crash
    // let's search only in the neighborhood of the solution to prove the concept of fidning the minimum
    const double xpmin= max( -srad*dx, global.getMinX() );
    const double xpmax= min( +srad*dx, global.getMaxX() );
    const double ypmin= max( -srad*dy, global.getMinY() );
    const double ypmax= min( +srad*dy, global.getMaxY() );

    markov.resize_as_needed(xpmin, ypmin,xpmax, ypmax);

    for (double tp=-45; tp<45; tp=tp+5) {
        // Lock is needed if the robot keeps on building the map as we advance in matching
        // However, it might be better to copy the map and use the copy for all matches
        // Do not lock the map "forever" as this might block the robot loop where the map is updated
        local.lock(); 
        NAV_Map_Match trial(local, tp);
        local.unlock();
        for (double xp=xpmin; xp<=xpmax; xp=xp+dx) {
            for (double yp=ypmin; yp<=xpmax; yp=yp+dy) {
                global.lock();
                long int score = global.map_match(trial, xp, yp);
                global.unlock();
                std::cout<<std::setw(10)<<tp<<std::setw(10)<<xp<<std::setw(10)<<yp<<"\r";
                if (markov(xp, yp).score < score) {
                    markov(xp, yp) = ScoredHeading(ArPose(xp, yp, tp), score);
                }
            }
        }
    }
    std::cout << std::endl;
}



void NAV_Map_Match_origin_adjust(NAV_Map_Match &global, NAV_Map_Match &local, ArPose &best_pose)
{
    if (global.getResolution()!=local.getResolution()) return;


    NAV_Map_Match map_try;                     // try not to allocate and deallocate large chunks of memory
    ArPose pose_try(best_pose);
    local.lock();
    map_try.map_assign(local, pose_try);
    local.unlock();
    global.lock();
    long int best_score = global.map_match(map_try);
    global.unlock();
    std::cout<<std::setw(10)<<best_pose.getX()<<std::setw(10)<<best_pose.getY()<<std::setw(10)<<best_pose.getTh()<<std::setw(10)<<best_score<<"\r";

    for (;;) {
        bool improved = false;
        long int score_try;

        // improve X
        pose_try=ArPose(best_pose.getX()+global.getResolution(), best_pose.getY(), best_pose.getTh());
        local.lock();
        map_try.map_assign(local, pose_try);
        local.unlock();
        global.lock();
        score_try = global.map_match(map_try);
        global.unlock();
        if (score_try>best_score)
        {
            best_pose=pose_try;
            best_score=score_try;
            improved=true;
        }
        else {
            pose_try=ArPose(best_pose.getX()-global.getResolution(), best_pose.getY(), best_pose.getTh());
            local.lock();
            map_try.map_assign(local, pose_try);
            local.unlock();
            global.lock();
            score_try = global.map_match(map_try);
            global.unlock();
            if (score_try>best_score)
            {
                best_pose=pose_try;
                best_score=score_try;
                improved=true;
            }
            // else nothing;
        }

        // improve Y
        pose_try=ArPose(best_pose.getX(), best_pose.getY()+global.getResolution(), best_pose.getTh());
        map_try.map_assign(local, pose_try);
        score_try = global.map_match(map_try);
        if (score_try>best_score)
        {
            best_pose=pose_try;
            best_score=score_try;
            improved=true;
        }
        else {
            pose_try=ArPose(best_pose.getX(), best_pose.getY()-global.getResolution(), best_pose.getTh());
            local.lock();
            map_try.map_assign(local, pose_try);
            local.unlock();
            global.lock();
            score_try = global.map_match(map_try);
            global.unlock();
            if (score_try>best_score)
            {
                best_pose=pose_try;
                best_score=score_try;
                improved=true;
            }
            // else nothing;
        }

        // improve Th
        pose_try=ArPose(best_pose.getX(), best_pose.getY(), best_pose.getTh()+1);
        local.lock();
        map_try.map_assign(local, pose_try);
        local.unlock();
        global.lock();
        score_try = global.map_match(map_try);
        global.unlock();
        if (score_try>best_score)
        {
            best_pose=pose_try;
            best_score=score_try;
            improved=true;
        }
        else {
            pose_try=ArPose(best_pose.getX(), best_pose.getY(), best_pose.getTh()-1);
            local.lock();
            map_try.map_assign(local, pose_try);
            local.unlock();
            global.lock();
            score_try = global.map_match(map_try);
            global.unlock();
            if (score_try>best_score)
            {
                best_pose=pose_try;
                best_score=score_try;
                improved=true;
            }
            // else nothing;
        }

        std::cout<<std::setw(10)<<best_pose.getX()<<std::setw(10)<<best_pose.getY()<<std::setw(10)<<best_pose.getTh()<<std::setw(10)<<best_score<<"\r";
        if (!improved) {
            std::cout<<std::endl;
            return;
        }
    }
}